WIP: replay overhaul (no more special ekf2 replay)

This commit is contained in:
Daniel Agar
2024-03-21 13:58:22 -04:00
parent 710286da72
commit 245fc46840
28 changed files with 792 additions and 1029 deletions
-1
View File
@@ -813,7 +813,6 @@ void printTopics() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener commander_state" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener commander_state" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf2_timestamps" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_fake_pos" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_fake_pos" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_gnss_pos" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_gnss_pos" || true'
-7
View File
@@ -122,13 +122,6 @@ endif
SRC_DIR := $(shell dirname "$(realpath $(lastword $(MAKEFILE_LIST)))") SRC_DIR := $(shell dirname "$(realpath $(lastword $(MAKEFILE_LIST)))")
# check if replay env variable is set & set build dir accordingly
ifdef replay
BUILD_DIR_SUFFIX := _replay
else
BUILD_DIR_SUFFIX :=
endif
CMAKE_ARGS ?= CMAKE_ARGS ?=
# additional config parameters passed to cmake # additional config parameters passed to cmake
@@ -37,6 +37,12 @@ px4_add_romfs_files(
px4-rc.mavlink px4-rc.mavlink
px4-rc.params px4-rc.params
px4-rc.simulator px4-rc.simulator
rc.replay
rc.replay_ekf2
rc.replay_mc_hover_thrust_estimator
rc.replay_mixing
rc.replay_sensor_calibration
rc.replay_sensor_filtering
rcS rcS
) )
@@ -6,35 +6,36 @@ param set-default IMU_INTEG_RATE 250
if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then
echo "INFO [init] SIH simulator" echo "INFO [init] SIH simulator"
if [ -n "${PX4_HOME_LAT}" ]; then # if [ -n "${PX4_HOME_LAT}" ]; then
param set SIH_LOC_LAT0 ${PX4_HOME_LAT} # param set SIH_LOC_LAT0 ${PX4_HOME_LAT}
fi # fi
if [ -n "${PX4_HOME_LON}" ]; then # if [ -n "${PX4_HOME_LON}" ]; then
param set SIH_LOC_LON0 ${PX4_HOME_LON} # param set SIH_LOC_LON0 ${PX4_HOME_LON}
fi # fi
if simulator_sih start; then # if simulator_sih start; then
if param compare -s SENS_EN_BAROSIM 1 # if param compare -s SENS_EN_BAROSIM 1
then # then
sensor_baro_sim start # sensor_baro_sim start
fi # fi
if param compare -s SENS_EN_GPSSIM 1 # if param compare -s SENS_EN_GPSSIM 1
then # then
sensor_gps_sim start # sensor_gps_sim start
fi # fi
if param compare -s SENS_EN_MAGSIM 1 # if param compare -s SENS_EN_MAGSIM 1
then # then
sensor_mag_sim start # sensor_mag_sim start
fi # fi
else # else
echo "ERROR [init] simulator_sih failed to start" # echo "ERROR [init] simulator_sih failed to start"
exit 1 # exit 1
fi # fi
elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
# Use Gazebo # Use Gazebo
@@ -190,7 +191,7 @@ else
else else
echo "INFO [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOSTNAME}" echo "INFO [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOSTNAME}"
simulator_mavlink start -h "${PX4_SIM_HOSTNAME}" "${simulator_tcp_port}" #simulator_mavlink start -h "${PX4_SIM_HOSTNAME}" "${simulator_tcp_port}"
fi fi
fi fi
@@ -0,0 +1,54 @@
#!/bin/sh
# EKF2 replay script
# shellcheck disable=SC2154
if [ ! -f ${replay_file} ]; then
echo "Invalid replay log file ${replay}"
exit 1
fi
echo "replay file: ${replay_file}"
if [ ! -f replay_params.txt ]; then
echo "Creating $(pwd)/replay_params.txt"
ulog_params -i "${replay_file}" -l ' ' | grep -e '^EKF2' > replay_params.txt
fi
param set SDLOG_DIRS_MAX 7
# TODO:
# input: vehicle_imu, vehicle_magnetometer, distance_sensor
# output: estimator_status
# replay start --ignore=estimator_status,
# TODO: replay file?
# apply all params before ekf starts, as some params cannot be changed after startup
replay tryapplyparams
#param set SENS_IMU_MODE 0
#param set EKF2_MULTI_IMU 3
ekf2 start &
ekf2 status
logger start -f -t -b 1000 -p vehicle_attitude
logger status
replay start
# ekf2 status
# logger status
# logger stop
# TODO: easy debug from gdb (and vscode)
# TODO
# loop through parameter changes? (noise, sensor delays, etc)
# - create log files with appropriate names (logger custom logfile name?)
# TODO
# - work queue lockstep handling
# - parameter defaults
# - progress (percentage, time in seconds, etc)
# - realtime vs execute as fast as possible
# - controls (pause, reset, etc)
@@ -13,18 +13,14 @@ if [ ! -f replay_params.txt ]; then
ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt
fi fi
publisher_rules_file="orb_publisher.rules"
cat <<EOF > "$publisher_rules_file"
restrict_topics: sensor_combined, vehicle_gps_position, vehicle_land_detected
module: replay
ignore_others: false
EOF
param set SDLOG_DIRS_MAX 7 param set SDLOG_DIRS_MAX 7
param set SDLOG_PROFILE 3
# TODO:
# input: vehicle_local_position, vehicle_local_position_setpoint
# output: hover_thrust_estimate
# apply all params before ekf starts, as some params cannot be changed after startup # apply all params before ekf starts, as some params cannot be changed after startup
replay tryapplyparams #replay tryapplyparams
ekf2 start -r mc_hover_thrust_estimator start
logger start -f -t -b 1000 -p vehicle_attitude logger start -f -t -b 1000 -p vehicle_attitude
replay start replay start
@@ -0,0 +1,29 @@
#!/bin/sh
# EKF2 replay script
# shellcheck disable=SC2154
if [ ! -f ${replay} ]; then
echo "Invalid replay log file ${replay}"
exit 1
fi
if [ ! -f replay_params.txt ]; then
echo "Creating $(pwd)/replay_params.txt"
ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt
fi
param set SDLOG_DIRS_MAX 7
# TODO:
# input: actuator_controls_0
# output:
# apply all params before ekf starts, as some params cannot be changed after startup
#replay tryapplyparams
pwm_out_sim start
mixer load /dev/pwm_output0 etc/mixers/quad_x.main.mix
logger start -f -t -b 1000 -p vehicle_attitude
replay start
@@ -0,0 +1,30 @@
#!/bin/sh
# EKF2 replay script
# shellcheck disable=SC2154
if [ ! -f ${replay} ]; then
echo "Invalid replay log file ${replay}"
exit 1
fi
if [ ! -f replay_params.txt ]; then
echo "Creating $(pwd)/replay_params.txt"
ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt
fi
param set SDLOG_DIRS_MAX 7
# TODO:
# input: sensor_accel, sensor_gyro, sensor_mag, ?
# output: mag_worker_data
# apply all params before ekf starts, as some params cannot be changed after startup
#replay tryapplyparams
commander start
gyro_calibration start
temperature_compensatoin start
logger start -f -t -b 1000 -p vehicle_attitude
replay start
@@ -0,0 +1,28 @@
#!/bin/sh
# EKF2 replay script
# shellcheck disable=SC2154
if [ ! -f ${replay} ]; then
echo "Invalid replay log file ${replay}"
exit 1
fi
if [ ! -f replay_params.txt ]; then
echo "Creating $(pwd)/replay_params.txt"
ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt
fi
param set SDLOG_DIRS_MAX 7
# TODO:
# input: sensor_accel, sensor_gyro, sensor_mag, ?
# output: mag_worker_data
# apply all params before ekf starts, as some params cannot be changed after startup
#replay tryapplyparams
sensors start
logger start -f -t -b 1000 -p vehicle_angular_velocity
replay start
+5 -11
View File
@@ -17,11 +17,10 @@ PATH="$PATH:${R}etc/init.d-posix"
# Main SITL startup script # Main SITL startup script
# #
# check for ekf2 replay # check for replay
# shellcheck disable=SC2154 if [ ! -z ${replay_mode+x} ]; then
if [ "$replay_mode" = "ekf2" ] echo "Replay Mode: ${replay_mode}"
then . ${R}etc/init.d-posix/rc.replay_${replay_mode}
. ${R}etc/init.d-posix/rc.replay
exit 0 exit 0
fi fi
@@ -330,12 +329,7 @@ fi
[ -e "$autostart_file".post ] && . "$autostart_file".post [ -e "$autostart_file".post ] && . "$autostart_file".post
# Run script to start logging # Run script to start logging
if param compare SYS_MC_EST_GROUP 2 set LOGGER_ARGS "-p vehicle_attitude"
then
set LOGGER_ARGS "-p ekf2_timestamps"
else
set LOGGER_ARGS "-p vehicle_attitude"
fi
. ${R}etc/init.d/rc.logging . ${R}etc/init.d/rc.logging
mavlink boot_complete mavlink boot_complete
-1
View File
@@ -74,7 +74,6 @@ set(msg_files
DifferentialDriveSetpoint.msg DifferentialDriveSetpoint.msg
DifferentialPressure.msg DifferentialPressure.msg
DistanceSensor.msg DistanceSensor.msg
Ekf2Timestamps.msg
EscReport.msg EscReport.msg
EscStatus.msg EscStatus.msg
EstimatorAidSource1d.msg EstimatorAidSource1d.msg
-23
View File
@@ -1,23 +0,0 @@
# this message contains the (relative) timestamps of the sensor inputs used by EKF2.
# It can be used for reproducible replay.
# the timestamp field is the ekf2 reference time and matches the timestamp of
# the sensor_combined topic.
uint64 timestamp # time since system start (microseconds)
int16 RELATIVE_TIMESTAMP_INVALID = 32767 # (0x7fff) If one of the relative timestamps
# is set to this value, it means the associated sensor values did not update
# timestamps are relative to the main timestamp and are in 0.1 ms (timestamp +
# *_timestamp_rel = absolute timestamp). For int16, this allows a maximum
# difference of +-3.2s to the sensor_combined topic.
int16 airspeed_timestamp_rel
int16 distance_sensor_timestamp_rel
int16 optical_flow_timestamp_rel
int16 vehicle_air_data_timestamp_rel
int16 vehicle_magnetometer_timestamp_rel
int16 visual_odometry_timestamp_rel
# Note: this is a high-rate logged topic, so it needs to be as small as possible
-197
View File
@@ -77,24 +77,6 @@ bool uORB::Manager::terminate()
return false; return false;
} }
uORB::Manager::Manager()
{
#ifdef ORB_USE_PUBLISHER_RULES
const char *file_name = PX4_STORAGEDIR"/orb_publisher.rules";
int ret = readPublisherRulesFromFile(file_name, _publisher_rule);
if (ret == PX4_OK) {
_has_publisher_rules = true;
PX4_INFO("Using orb rules from %s", file_name);
} else {
PX4_ERR("Failed to read publisher rules file %s (%s)", file_name, strerror(-ret));
}
#endif /* ORB_USE_PUBLISHER_RULES */
}
uORB::Manager::~Manager() uORB::Manager::~Manager()
{ {
delete _device_master; delete _device_master;
@@ -267,30 +249,6 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance)
{ {
#ifdef ORB_USE_PUBLISHER_RULES
// check publisher rule
if (_has_publisher_rules) {
const char *prog_name = px4_get_taskname();
if (strcmp(_publisher_rule.module_name, prog_name) == 0) {
if (_publisher_rule.ignore_other_topics) {
if (!findTopic(_publisher_rule, meta->o_name)) {
PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name);
return (orb_advert_t)_Instance;
}
}
} else {
if (findTopic(_publisher_rule, meta->o_name)) {
PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name);
return (orb_advert_t)_Instance;
}
}
}
#endif /* ORB_USE_PUBLISHER_RULES */
/* open the node as an advertiser */ /* open the node as an advertiser */
int fd = node_open(meta, true, instance); int fd = node_open(meta, true, instance);
@@ -335,14 +293,6 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
int uORB::Manager::orb_unadvertise(orb_advert_t handle) int uORB::Manager::orb_unadvertise(orb_advert_t handle)
{ {
#ifdef ORB_USE_PUBLISHER_RULES
if (handle == _Instance) {
return PX4_OK; //pretend success
}
#endif /* ORB_USE_PUBLISHER_RULES */
return uORB::DeviceNode::unadvertise(handle); return uORB::DeviceNode::unadvertise(handle);
} }
@@ -364,14 +314,6 @@ int uORB::Manager::orb_unsubscribe(int fd)
int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
{ {
#ifdef ORB_USE_PUBLISHER_RULES
if (handle == _Instance) {
return PX4_OK; //pretend success
}
#endif /* ORB_USE_PUBLISHER_RULES */
return uORB::DeviceNode::publish(meta, handle, data); return uORB::DeviceNode::publish(meta, handle, data);
} }
@@ -715,142 +657,3 @@ int16_t uORB::Manager::process_received_message(const char *messageName, int32_t
} }
#endif /* CONFIG_ORB_COMMUNICATOR */ #endif /* CONFIG_ORB_COMMUNICATOR */
#ifdef ORB_USE_PUBLISHER_RULES
bool uORB::Manager::startsWith(const char *pre, const char *str)
{
size_t lenpre = strlen(pre),
lenstr = strlen(str);
return lenstr < lenpre ? false : strncmp(pre, str, lenpre) == 0;
}
bool uORB::Manager::findTopic(const PublisherRule &rule, const char *topic_name)
{
const char **topics_ptr = rule.topics;
while (*topics_ptr) {
if (strcmp(*topics_ptr, topic_name) == 0) {
return true;
}
++topics_ptr;
}
return false;
}
void uORB::Manager::strTrim(const char **str)
{
while (**str == ' ' || **str == '\t') { ++(*str); }
}
int uORB::Manager::readPublisherRulesFromFile(const char *file_name, PublisherRule &rule)
{
FILE *fp;
static const int line_len = 1024;
int ret = PX4_OK;
char *line = new char[line_len];
if (!line) {
return -ENOMEM;
}
fp = fopen(file_name, "r");
if (fp == NULL) {
delete[](line);
return -errno;
}
const char *restrict_topics_str = "restrict_topics:";
const char *module_str = "module:";
const char *ignore_others = "ignore_others:";
rule.ignore_other_topics = false;
rule.module_name = nullptr;
rule.topics = nullptr;
while (fgets(line, line_len, fp) && ret == PX4_OK) {
if (strlen(line) < 2 || line[0] == '#') {
continue;
}
if (startsWith(restrict_topics_str, line)) {
//read topics list
char *start = line + strlen(restrict_topics_str);
strTrim((const char **)&start);
char *topics = strdup(start);
int topic_len = 0, num_topics = 0;
for (int i = 0; topics[i]; ++i) {
if (topics[i] == ',' || topics[i] == '\n') {
if (topic_len > 0) {
topics[i] = 0;
++num_topics;
}
topic_len = 0;
} else {
++topic_len;
}
}
if (num_topics > 0) {
rule.topics = new const char *[num_topics + 1];
int topic = 0;
strTrim((const char **)&topics);
rule.topics[topic++] = topics;
while (topic < num_topics) {
if (*topics == 0) {
++topics;
strTrim((const char **)&topics);
rule.topics[topic++] = topics;
} else {
++topics;
}
}
rule.topics[num_topics] = nullptr;
}
} else if (startsWith(module_str, line)) {
//read module name
char *start = line + strlen(module_str);
strTrim((const char **)&start);
int len = strlen(start);
if (len > 0 && start[len - 1] == '\n') {
start[len - 1] = 0;
}
rule.module_name = strdup(start);
} else if (startsWith(ignore_others, line)) {
const char *start = line + strlen(ignore_others);
strTrim(&start);
if (startsWith("true", start)) {
rule.ignore_other_topics = true;
}
} else {
PX4_ERR("orb rules file: wrong format: %s", line);
ret = -EINVAL;
}
}
if (ret == PX4_OK && (!rule.module_name || !rule.topics)) {
PX4_ERR("Wrong format in orb publisher rules file");
ret = -EINVAL;
}
delete[](line);
fclose(fp);
return ret;
}
#endif /* ORB_USE_PUBLISHER_RULES */
+1 -41
View File
@@ -503,7 +503,7 @@ private: // data members
DeviceMaster *_device_master{nullptr}; DeviceMaster *_device_master{nullptr};
private: //class methods private: //class methods
Manager(); Manager() = default;
virtual ~Manager(); virtual ~Manager();
#ifdef CONFIG_ORB_COMMUNICATOR #ifdef CONFIG_ORB_COMMUNICATOR
@@ -559,46 +559,6 @@ private: //class methods
*/ */
virtual int16_t process_received_message(const char *messageName, int32_t length, uint8_t *data); virtual int16_t process_received_message(const char *messageName, int32_t length, uint8_t *data);
#endif /* CONFIG_ORB_COMMUNICATOR */ #endif /* CONFIG_ORB_COMMUNICATOR */
#ifdef ORB_USE_PUBLISHER_RULES
struct PublisherRule {
const char **topics; //null-terminated list of topic names
const char *module_name; //only this module is allowed to publish one of the topics
bool ignore_other_topics;
};
/**
* test if str starts with pre
*/
bool startsWith(const char *pre, const char *str);
/**
* find a topic in a rule
*/
bool findTopic(const PublisherRule &rule, const char *topic_name);
/**
* trim whitespace from the beginning of a string
*/
void strTrim(const char **str);
/**
* Read publisher rules from a file. It has the format:
*
* restrict_topics: <topic1>, <topic2>, <topic3>
* module: <module_name>
* [ignore_others:true]
*
* @return 0 on success, <0 otherwise
*/
int readPublisherRulesFromFile(const char *file_name, PublisherRule &rule);
PublisherRule _publisher_rule;
bool _has_publisher_rules = false;
#endif /* ORB_USE_PUBLISHER_RULES */
}; };
#endif /* _uORBManager_hpp_ */ #endif /* _uORBManager_hpp_ */
+2 -2
View File
@@ -181,7 +181,7 @@ MessageFormatReader::State MessageFormatReader::readMore()
} }
// Not expected to get here // Not expected to get here
PX4_ERR("logic error"); PX4_ERR("logic error (not expected to get here)");
_state = State::Failure; _state = State::Failure;
return _state; return _state;
} }
@@ -237,7 +237,7 @@ int MessageFormatReader::expandMessageFormat(char *format, unsigned len, unsigne
} }
if (format_idx + 1 != (int)len) { if (format_idx + 1 != (int)len) {
PX4_ERR("logic error"); PX4_ERR("logic error (format_idx %d, len %d)", format_idx, len);
return -1; return -1;
} }
@@ -13,4 +13,4 @@ param set-default -s MC_AT_EN 1
param set-default -s UAVCAN_ENABLE 2 param set-default -s UAVCAN_ENABLE 2
set LOGGER_BUF 64 set LOGGER_BUF 256
+56 -104
View File
@@ -49,10 +49,9 @@ static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
static px4::atomic<EKF2Selector *> _ekf2_selector {nullptr}; static px4::atomic<EKF2Selector *> _ekf2_selector {nullptr};
#endif // CONFIG_EKF2_MULTI_INSTANCE #endif // CONFIG_EKF2_MULTI_INSTANCE
EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config):
ModuleParams(nullptr), ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, config), ScheduledWorkItem(MODULE_NAME, config),
_replay_mode(replay_mode && !multi_mode),
_multi_mode(multi_mode), _multi_mode(multi_mode),
_instance(multi_mode ? -1 : 0), _instance(multi_mode ? -1 : 0),
_attitude_pub(multi_mode ? ORB_ID(estimator_attitude) : ORB_ID(vehicle_attitude)), _attitude_pub(multi_mode ? ORB_ID(estimator_attitude) : ORB_ID(vehicle_attitude)),
@@ -237,7 +236,6 @@ EKF2::~EKF2()
bool EKF2::multi_init(int imu, int mag) bool EKF2::multi_init(int imu, int mag)
{ {
// advertise all topics to ensure consistent uORB instance numbering // advertise all topics to ensure consistent uORB instance numbering
_ekf2_timestamps_pub.advertise();
_estimator_event_flags_pub.advertise(); _estimator_event_flags_pub.advertise();
_estimator_innovation_test_ratios_pub.advertise(); _estimator_innovation_test_ratios_pub.advertise();
_estimator_innovation_variances_pub.advertise(); _estimator_innovation_variances_pub.advertise();
@@ -703,42 +701,32 @@ void EKF2::Run()
_last_time_slip_us = 0; _last_time_slip_us = 0;
} }
// ekf2_timestamps (using 0.1 ms relative timestamps)
ekf2_timestamps_s ekf2_timestamps {
.timestamp = now,
.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
.optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
.vehicle_air_data_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
.vehicle_magnetometer_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
.visual_odometry_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
};
#if defined(CONFIG_EKF2_AIRSPEED) #if defined(CONFIG_EKF2_AIRSPEED)
UpdateAirspeedSample(ekf2_timestamps); UpdateAirspeedSample();
#endif // CONFIG_EKF2_AIRSPEED #endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_AUXVEL) #if defined(CONFIG_EKF2_AUXVEL)
UpdateAuxVelSample(ekf2_timestamps); UpdateAuxVelSample();
#endif // CONFIG_EKF2_AUXVEL #endif // CONFIG_EKF2_AUXVEL
#if defined(CONFIG_EKF2_BAROMETER) #if defined(CONFIG_EKF2_BAROMETER)
UpdateBaroSample(ekf2_timestamps); UpdateBaroSample();
#endif // CONFIG_EKF2_BAROMETER #endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_EXTERNAL_VISION) #if defined(CONFIG_EKF2_EXTERNAL_VISION)
UpdateExtVisionSample(ekf2_timestamps); UpdateExtVisionSample();
#endif // CONFIG_EKF2_EXTERNAL_VISION #endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_OPTICAL_FLOW) #if defined(CONFIG_EKF2_OPTICAL_FLOW)
UpdateFlowSample(ekf2_timestamps); UpdateFlowSample();
#endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_GNSS) #if defined(CONFIG_EKF2_GNSS)
UpdateGpsSample(ekf2_timestamps); UpdateGpsSample();
#endif // CONFIG_EKF2_GNSS #endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER) #if defined(CONFIG_EKF2_MAGNETOMETER)
UpdateMagSample(ekf2_timestamps); UpdateMagSample();
#endif // CONFIG_EKF2_MAGNETOMETER #endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_RANGE_FINDER) #if defined(CONFIG_EKF2_RANGE_FINDER)
UpdateRangeSample(ekf2_timestamps); UpdateRangeSample();
#endif // CONFIG_EKF2_RANGE_FINDER #endif // CONFIG_EKF2_RANGE_FINDER
UpdateSystemFlagsSample(ekf2_timestamps);
UpdateSystemFlagsSample();
// run the EKF update and output // run the EKF update and output
const hrt_abstime ekf_update_start = hrt_absolute_time(); const hrt_abstime ekf_update_start = hrt_absolute_time();
@@ -793,9 +781,6 @@ void EKF2::Run()
UpdateMagCalibration(now); UpdateMagCalibration(now);
#endif // CONFIG_EKF2_MAGNETOMETER #endif // CONFIG_EKF2_MAGNETOMETER
} }
// publish ekf2_timestamps
_ekf2_timestamps_pub.publish(ekf2_timestamps);
} }
// re-schedule as backup timeout // re-schedule as backup timeout
@@ -921,13 +906,7 @@ void EKF2::PublishAttitude(const hrt_abstime &timestamp)
_ekf.getQuaternion().copyTo(att.q); _ekf.getQuaternion().copyTo(att.q);
_ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter); _ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter);
att.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); att.timestamp = hrt_absolute_time();
_attitude_pub.publish(att);
} else if (_replay_mode) {
// in replay mode we have to tell the replay module not to wait for an update
// we do this by publishing an attitude with zero timestamp
vehicle_attitude_s att{};
_attitude_pub.publish(att); _attitude_pub.publish(att);
} }
} }
@@ -1004,7 +983,7 @@ void EKF2::PublishEvPosBias(const hrt_abstime &timestamp)
if ((bias_vec - _last_ev_bias_published).longerThan(0.01f)) { if ((bias_vec - _last_ev_bias_published).longerThan(0.01f)) {
bias.timestamp_sample = _ekf.aid_src_ev_hgt().timestamp_sample; bias.timestamp_sample = _ekf.aid_src_ev_hgt().timestamp_sample;
bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); bias.timestamp = hrt_absolute_time();
_estimator_ev_pos_bias_pub.publish(bias); _estimator_ev_pos_bias_pub.publish(bias);
_last_ev_bias_published = Vector3f(bias.bias); _last_ev_bias_published = Vector3f(bias.bias);
@@ -1024,7 +1003,7 @@ estimator_bias_s EKF2::fillEstimatorBiasMsg(const BiasEstimator::status &status,
bias.innov = status.innov; bias.innov = status.innov;
bias.innov_var = status.innov_var; bias.innov_var = status.innov_var;
bias.innov_test_ratio = status.innov_test_ratio; bias.innov_test_ratio = status.innov_test_ratio;
bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); bias.timestamp = hrt_absolute_time();
return bias; return bias;
} }
@@ -1086,7 +1065,7 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
event_flags.emergency_yaw_reset_mag_stopped = _ekf.warning_event_flags().emergency_yaw_reset_mag_stopped; event_flags.emergency_yaw_reset_mag_stopped = _ekf.warning_event_flags().emergency_yaw_reset_mag_stopped;
event_flags.emergency_yaw_reset_gps_yaw_stopped = _ekf.warning_event_flags().emergency_yaw_reset_gps_yaw_stopped; event_flags.emergency_yaw_reset_gps_yaw_stopped = _ekf.warning_event_flags().emergency_yaw_reset_gps_yaw_stopped;
event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); event_flags.timestamp = hrt_absolute_time();
_estimator_event_flags_pub.update(event_flags); _estimator_event_flags_pub.update(event_flags);
_last_event_flags_publish = event_flags.timestamp; _last_event_flags_publish = event_flags.timestamp;
@@ -1096,7 +1075,7 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
} else if ((_last_event_flags_publish != 0) && (timestamp >= _last_event_flags_publish + 1_s)) { } else if ((_last_event_flags_publish != 0) && (timestamp >= _last_event_flags_publish + 1_s)) {
// continue publishing periodically // continue publishing periodically
_estimator_event_flags_pub.get().timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_event_flags_pub.get().timestamp = hrt_absolute_time();
_estimator_event_flags_pub.update(); _estimator_event_flags_pub.update();
_last_event_flags_publish = _estimator_event_flags_pub.get().timestamp; _last_event_flags_publish = _estimator_event_flags_pub.get().timestamp;
} }
@@ -1153,7 +1132,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning
|| _ekf.control_status_flags().wind_dead_reckoning; || _ekf.control_status_flags().wind_dead_reckoning;
global_pos.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); global_pos.timestamp = hrt_absolute_time();
_global_position_pub.publish(global_pos); _global_position_pub.publish(global_pos);
} }
} }
@@ -1187,7 +1166,7 @@ void EKF2::PublishGpsStatus(const hrt_abstime &timestamp)
estimator_gps_status.check_fail_max_horz_spd_err = _ekf.gps_check_fail_status_flags().hspeed; estimator_gps_status.check_fail_max_horz_spd_err = _ekf.gps_check_fail_status_flags().hspeed;
estimator_gps_status.check_fail_max_vert_spd_err = _ekf.gps_check_fail_status_flags().vspeed; estimator_gps_status.check_fail_max_vert_spd_err = _ekf.gps_check_fail_status_flags().vspeed;
estimator_gps_status.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); estimator_gps_status.timestamp = hrt_absolute_time();
_estimator_gps_status_pub.publish(estimator_gps_status); _estimator_gps_status_pub.publish(estimator_gps_status);
@@ -1288,7 +1267,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
innovations.hagl_rate = _ekf.getHaglRateInnov(); innovations.hagl_rate = _ekf.getHaglRateInnov();
#endif // CONFIG_EKF2_RANGE_FINDER #endif // CONFIG_EKF2_RANGE_FINDER
innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); innovations.timestamp = hrt_absolute_time();
_estimator_innovations_pub.publish(innovations); _estimator_innovations_pub.publish(innovations);
// calculate noise filtered velocity innovations which are used for pre-flight checking // calculate noise filtered velocity innovations which are used for pre-flight checking
@@ -1422,7 +1401,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
test_ratios.hagl_rate = _ekf.getHaglRateInnovRatio(); test_ratios.hagl_rate = _ekf.getHaglRateInnovRatio();
#endif // CONFIG_EKF2_RANGE_FINDER #endif // CONFIG_EKF2_RANGE_FINDER
test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); test_ratios.timestamp = hrt_absolute_time();
_estimator_innovation_test_ratios_pub.publish(test_ratios); _estimator_innovation_test_ratios_pub.publish(test_ratios);
} }
@@ -1519,7 +1498,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
variances.hagl_rate = _ekf.getHaglRateInnovVar(); variances.hagl_rate = _ekf.getHaglRateInnovVar();
#endif // CONFIG_EKF2_RANGE_FINDER #endif // CONFIG_EKF2_RANGE_FINDER
variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); variances.timestamp = hrt_absolute_time();
_estimator_innovation_variances_pub.publish(variances); _estimator_innovation_variances_pub.publish(variances);
} }
@@ -1625,7 +1604,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
} }
// publish vehicle local position data // publish vehicle local position data
lpos.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); lpos.timestamp = hrt_absolute_time();
_local_position_pub.publish(lpos); _local_position_pub.publish(lpos);
} }
@@ -1667,7 +1646,7 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sa
odom.quality = 0; odom.quality = 0;
// publish vehicle odometry data // publish vehicle odometry data
odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); odom.timestamp = hrt_absolute_time();
_odometry_pub.publish(odom); _odometry_pub.publish(odom);
} }
@@ -1733,7 +1712,7 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
#endif // CONFIG_EKF2_MAGNETOMETER #endif // CONFIG_EKF2_MAGNETOMETER
bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); bias.timestamp = hrt_absolute_time();
_estimator_sensor_bias_pub.publish(bias); _estimator_sensor_bias_pub.publish(bias);
_last_sensor_bias_published = bias.timestamp; _last_sensor_bias_published = bias.timestamp;
@@ -1749,7 +1728,7 @@ void EKF2::PublishStates(const hrt_abstime &timestamp)
state_vector.copyTo(states.states); state_vector.copyTo(states.states);
states.n_states = state_vector.size(); states.n_states = state_vector.size();
_ekf.covariances_diagonal().copyTo(states.covariances); _ekf.covariances_diagonal().copyTo(states.covariances);
states.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); states.timestamp = hrt_absolute_time();
_estimator_states_pub.publish(states); _estimator_states_pub.publish(states);
} }
@@ -1810,7 +1789,7 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
status.mag_strength_ref_gs); status.mag_strength_ref_gs);
#endif // CONFIG_EKF2_MAGNETOMETER #endif // CONFIG_EKF2_MAGNETOMETER
status.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); status.timestamp = hrt_absolute_time();
_estimator_status_pub.publish(status); _estimator_status_pub.publish(status);
} }
@@ -1911,7 +1890,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
status_flags.reject_optflow_x = _ekf.innov_check_fail_status_flags().reject_optflow_X; status_flags.reject_optflow_x = _ekf.innov_check_fail_status_flags().reject_optflow_X;
status_flags.reject_optflow_y = _ekf.innov_check_fail_status_flags().reject_optflow_Y; status_flags.reject_optflow_y = _ekf.innov_check_fail_status_flags().reject_optflow_Y;
status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); status_flags.timestamp = hrt_absolute_time();
_estimator_status_flags_pub.publish(status_flags); _estimator_status_flags_pub.publish(status_flags);
_last_status_flags_publish = status_flags.timestamp; _last_status_flags_publish = status_flags.timestamp;
@@ -1933,7 +1912,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
yaw_est_test_data.yaw_composite_valid = _ekf.isYawEmergencyEstimateAvailable(); yaw_est_test_data.yaw_composite_valid = _ekf.isYawEmergencyEstimateAvailable();
yaw_est_test_data.timestamp_sample = _ekf.time_delayed_us(); yaw_est_test_data.timestamp_sample = _ekf.time_delayed_us();
yaw_est_test_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); yaw_est_test_data.timestamp = hrt_absolute_time();
_yaw_est_pub.publish(yaw_est_test_data); _yaw_est_pub.publish(yaw_est_test_data);
} }
@@ -1964,7 +1943,7 @@ void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
wind.windspeed_east = wind_vel(1); wind.windspeed_east = wind_vel(1);
wind.variance_north = wind_vel_var(0); wind.variance_north = wind_vel_var(0);
wind.variance_east = wind_vel_var(1); wind.variance_east = wind_vel_var(1);
wind.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); wind.timestamp = hrt_absolute_time();
_wind_pub.publish(wind); _wind_pub.publish(wind);
} }
@@ -1992,7 +1971,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
_ekf.getFlowGyroBias().copyTo(flow_vel.gyro_bias); _ekf.getFlowGyroBias().copyTo(flow_vel.gyro_bias);
_ekf.getRefBodyRate().copyTo(flow_vel.ref_gyro); _ekf.getRefBodyRate().copyTo(flow_vel.ref_gyro);
flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); flow_vel.timestamp = hrt_absolute_time();
_estimator_optical_flow_vel_pub.publish(flow_vel); _estimator_optical_flow_vel_pub.publish(flow_vel);
@@ -2025,7 +2004,7 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
#endif // CONFIG_EKF2_GNSS #endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_AIRSPEED) #if defined(CONFIG_EKF2_AIRSPEED)
void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) void EKF2::UpdateAirspeedSample()
{ {
// EKF airspeed sample // EKF airspeed sample
// prefer ORB_ID(airspeed_validated) if available, otherwise fallback to raw airspeed ORB_ID(airspeed) // prefer ORB_ID(airspeed_validated) if available, otherwise fallback to raw airspeed ORB_ID(airspeed)
@@ -2050,7 +2029,7 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
_airspeed_validated_timestamp_last = airspeed_validated.timestamp; _airspeed_validated_timestamp_last = airspeed_validated.timestamp;
} }
} else if (((ekf2_timestamps.timestamp - _airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) { } else if ((hrt_elapsed_time(&_airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) {
// use ORB_ID(airspeed) if ORB_ID(airspeed_validated) is unavailable // use ORB_ID(airspeed) if ORB_ID(airspeed_validated) is unavailable
airspeed_s airspeed; airspeed_s airspeed;
@@ -2070,16 +2049,13 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
}; };
_ekf.setAirspeedData(airspeed_sample); _ekf.setAirspeedData(airspeed_sample);
} }
ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} }
} }
} }
#endif // CONFIG_EKF2_AIRSPEED #endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_AUXVEL) #if defined(CONFIG_EKF2_AUXVEL)
void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps) void EKF2::UpdateAuxVelSample()
{ {
// EKF auxiliary velocity sample // EKF auxiliary velocity sample
// - use the landing target pose estimate as another source of velocity data // - use the landing target pose estimate as another source of velocity data
@@ -2101,7 +2077,7 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
#endif // CONFIG_EKF2_AUXVEL #endif // CONFIG_EKF2_AUXVEL
#if defined(CONFIG_EKF2_BAROMETER) #if defined(CONFIG_EKF2_BAROMETER)
void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps) void EKF2::UpdateBaroSample()
{ {
// EKF baro sample // EKF baro sample
vehicle_air_data_s airdata; vehicle_air_data_s airdata;
@@ -2132,15 +2108,12 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
_ekf.set_air_density(airdata.rho); _ekf.set_air_density(airdata.rho);
_ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter, reset}); _ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter, reset});
ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} }
} }
#endif // CONFIG_EKF2_BAROMETER #endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_EXTERNAL_VISION) #if defined(CONFIG_EKF2_EXTERNAL_VISION)
bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps) bool EKF2::UpdateExtVisionSample()
{ {
// EKF external vision sample // EKF external vision sample
bool new_ev_odom = false; bool new_ev_odom = false;
@@ -2279,9 +2252,6 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
if (new_ev_odom) { if (new_ev_odom) {
_ekf.setExtVisionData(ev_data); _ekf.setExtVisionData(ev_data);
} }
ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)ev_odom.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} }
return new_ev_odom; return new_ev_odom;
@@ -2289,7 +2259,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
#endif // CONFIG_EKF2_EXTERNAL_VISION #endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_OPTICAL_FLOW) #if defined(CONFIG_EKF2_OPTICAL_FLOW)
bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) bool EKF2::UpdateFlowSample()
{ {
// EKF flow sample // EKF flow sample
bool new_optical_flow = false; bool new_optical_flow = false;
@@ -2332,7 +2302,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
} }
// use optical_flow distance as range sample if distance_sensor unavailable // use optical_flow distance as range sample if distance_sensor unavailable
if (PX4_ISFINITE(optical_flow.distance_m) && (ekf2_timestamps.timestamp > _last_range_sensor_update + 1_s)) { if (PX4_ISFINITE(optical_flow.distance_m) && (hrt_elapsed_time(&_last_range_sensor_update) > 1_s)) {
int8_t quality = static_cast<float>(optical_flow.quality) / static_cast<float>(UINT8_MAX) * 100.f; int8_t quality = static_cast<float>(optical_flow.quality) / static_cast<float>(UINT8_MAX) * 100.f;
@@ -2346,9 +2316,6 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
// set sensor limits // set sensor limits
_ekf.set_rangefinder_limits(optical_flow.min_ground_distance, optical_flow.max_ground_distance); _ekf.set_rangefinder_limits(optical_flow.min_ground_distance, optical_flow.max_ground_distance);
} }
ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} }
return new_optical_flow; return new_optical_flow;
@@ -2356,7 +2323,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
#endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_GNSS) #if defined(CONFIG_EKF2_GNSS)
void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) void EKF2::UpdateGpsSample()
{ {
// EKF GPS message // EKF GPS message
sensor_gps_s vehicle_gps_position; sensor_gps_s vehicle_gps_position;
@@ -2401,7 +2368,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
#endif // CONFIG_EKF2_GNSS #endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER) #if defined(CONFIG_EKF2_MAGNETOMETER)
void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) void EKF2::UpdateMagSample()
{ {
vehicle_magnetometer_s magnetometer; vehicle_magnetometer_s magnetometer;
@@ -2432,29 +2399,23 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
} }
_ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}, reset}); _ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}, reset});
ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} }
} }
#endif // CONFIG_EKF2_MAGNETOMETER #endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_RANGE_FINDER) #if defined(CONFIG_EKF2_RANGE_FINDER)
void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) void EKF2::UpdateRangeSample()
{ {
distance_sensor_s distance_sensor; distance_sensor_s distance_sensor;
if (_distance_sensor_selected < 0) { if (_distance_sensor_selected < 0) {
// only consider distance sensors that have updated within the last 0.1s
const hrt_abstime timestamp_stale = math::max(ekf2_timestamps.timestamp, 100_ms) - 100_ms;
if (_distance_sensor_subs.advertised()) { if (_distance_sensor_subs.advertised()) {
for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) { for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) {
if (_distance_sensor_subs[i].update(&distance_sensor)) { if (_distance_sensor_subs[i].update(&distance_sensor)) {
// only consider distance sensors that have updated within the last 0.1s
// only use the first instace which has the correct orientation // only use the first instace which has the correct orientation
if ((distance_sensor.timestamp != 0) && (distance_sensor.timestamp > timestamp_stale) if ((distance_sensor.timestamp > 100_ms) && (hrt_elapsed_time(&distance_sensor.timestamp) > 100_ms)
&& (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) { && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) {
int ndist = orb_group_count(ORB_ID(distance_sensor)); int ndist = orb_group_count(ORB_ID(distance_sensor));
@@ -2485,33 +2446,32 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
// Save sensor limits reported by the rangefinder // Save sensor limits reported by the rangefinder
_ekf.set_rangefinder_limits(distance_sensor.min_distance, distance_sensor.max_distance); _ekf.set_rangefinder_limits(distance_sensor.min_distance, distance_sensor.max_distance);
_last_range_sensor_update = ekf2_timestamps.timestamp; _last_range_sensor_update = hrt_absolute_time();
} }
ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)distance_sensor.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} }
if (_last_range_sensor_update < ekf2_timestamps.timestamp - 1_s) { if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) {
// force reselection after timeout // force reselection after timeout
_distance_sensor_selected = -1; _distance_sensor_selected = -1;
} }
} }
#endif // CONFIG_EKF2_RANGE_FINDER #endif // CONFIG_EKF2_RANGE_FINDER
void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps) void EKF2::UpdateSystemFlagsSample()
{ {
// EKF system flags // EKF system flags
if (_status_sub.updated() || _vehicle_land_detected_sub.updated()) { if (_status_sub.updated() || _vehicle_land_detected_sub.updated()) {
systemFlagUpdate flags{}; systemFlagUpdate flags{};
flags.time_us = ekf2_timestamps.timestamp; flags.time_us = 0;
// vehicle_status // vehicle_status
vehicle_status_s vehicle_status; vehicle_status_s vehicle_status;
if (_status_sub.copy(&vehicle_status) if (_status_sub.copy(&vehicle_status)
&& (ekf2_timestamps.timestamp < vehicle_status.timestamp + 3_s)) { && (hrt_elapsed_time(&vehicle_status.timestamp) < 3_s)) {
flags.time_us = math::max(flags.time_us, vehicle_status.timestamp);
// initially set in_air from arming_state (will be overridden if land detector is available) // initially set in_air from arming_state (will be overridden if land detector is available)
flags.in_air = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); flags.in_air = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
@@ -2534,7 +2494,9 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps)
vehicle_land_detected_s vehicle_land_detected; vehicle_land_detected_s vehicle_land_detected;
if (_vehicle_land_detected_sub.copy(&vehicle_land_detected) if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)
&& (ekf2_timestamps.timestamp < vehicle_land_detected.timestamp + 3_s)) { && (hrt_elapsed_time(&vehicle_land_detected.timestamp) < 3_s)) {
flags.time_us = math::max(flags.time_us, vehicle_land_detected.timestamp);
flags.at_rest = vehicle_land_detected.at_rest; flags.at_rest = vehicle_land_detected.at_rest;
flags.in_air = !vehicle_land_detected.landed; flags.in_air = !vehicle_land_detected.landed;
@@ -2666,12 +2628,6 @@ int EKF2::custom_command(int argc, char *argv[])
int EKF2::task_spawn(int argc, char *argv[]) int EKF2::task_spawn(int argc, char *argv[])
{ {
bool success = false; bool success = false;
bool replay_mode = false;
if (argc > 1 && !strcmp(argv[1], "-r")) {
PX4_INFO("replay mode enabled");
replay_mode = true;
}
#if defined(CONFIG_EKF2_MULTI_INSTANCE) #if defined(CONFIG_EKF2_MULTI_INSTANCE)
bool multi_mode = false; bool multi_mode = false;
@@ -2731,7 +2687,7 @@ int EKF2::task_spawn(int argc, char *argv[])
#endif // CONFIG_EKF2_MAGNETOMETER #endif // CONFIG_EKF2_MAGNETOMETER
} }
if (multi_mode && !replay_mode) { if (multi_mode) {
// Start EKF2Selector if it's not already running // Start EKF2Selector if it's not already running
if (_ekf2_selector.load() == nullptr) { if (_ekf2_selector.load() == nullptr) {
EKF2Selector *inst = new EKF2Selector(); EKF2Selector *inst = new EKF2Selector();
@@ -2756,7 +2712,7 @@ int EKF2::task_spawn(int argc, char *argv[])
while ((multi_instances_allocated < multi_instances) while ((multi_instances_allocated < multi_instances)
&& (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED) && (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED)
&& ((hrt_elapsed_time(&time_started) < 30_s) && ((hrt_elapsed_time(&time_started) < 300_s)
|| (vehicle_status_sub.get().hil_state == vehicle_status_s::HIL_STATE_ON))) { || (vehicle_status_sub.get().hil_state == vehicle_status_s::HIL_STATE_ON))) {
vehicle_status_sub.update(); vehicle_status_sub.update();
@@ -2773,7 +2729,7 @@ int EKF2::task_spawn(int argc, char *argv[])
if ((vehicle_mag_sub.advertised() || mag == 0) && (vehicle_imu_sub.advertised())) { if ((vehicle_mag_sub.advertised() || mag == 0) && (vehicle_imu_sub.advertised())) {
if (!ekf2_instance_created[imu][mag]) { if (!ekf2_instance_created[imu][mag]) {
EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false); EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu));
if (ekf2_inst && ekf2_inst->multi_init(imu, mag)) { if (ekf2_inst && ekf2_inst->multi_init(imu, mag)) {
int actual_instance = ekf2_inst->instance(); // match uORB instance numbering int actual_instance = ekf2_inst->instance(); // match uORB instance numbering
@@ -2821,7 +2777,7 @@ int EKF2::task_spawn(int argc, char *argv[])
{ {
// otherwise launch regular // otherwise launch regular
EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0, replay_mode); EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0);
if (ekf2_inst) { if (ekf2_inst) {
_objects[0].store(ekf2_inst); _objects[0].store(ekf2_inst);
@@ -2846,14 +2802,10 @@ Attitude and position estimator using an Extended Kalman Filter. It is used for
The documentation can be found on the [ECL/EKF Overview & Tuning](https://docs.px4.io/main/en/advanced_config/tuning_the_ecl_ekf.html) page. The documentation can be found on the [ECL/EKF Overview & Tuning](https://docs.px4.io/main/en/advanced_config/tuning_the_ecl_ekf.html) page.
ekf2 can be started in replay mode (`-r`): in this mode, it does not access the system time, but only uses the
timestamps from the sensor topics.
)DESCR_STR"); )DESCR_STR");
PRINT_MODULE_USAGE_NAME("ekf2", "estimator"); PRINT_MODULE_USAGE_NAME("ekf2", "estimator");
PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAM_FLAG('r', "Enable replay mode", true);
PRINT_MODULE_USAGE_COMMAND("stop"); PRINT_MODULE_USAGE_COMMAND("stop");
PRINT_MODULE_USAGE_COMMAND_DESCR("status", "print status info"); PRINT_MODULE_USAGE_COMMAND_DESCR("status", "print status info");
#if defined(CONFIG_EKF2_VERBOSE_STATUS) #if defined(CONFIG_EKF2_VERBOSE_STATUS)
+10 -13
View File
@@ -64,7 +64,6 @@
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp> #include <uORB/SubscriptionCallback.hpp>
#include <uORB/SubscriptionMultiArray.hpp> #include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/estimator_bias.h> #include <uORB/topics/estimator_bias.h>
#include <uORB/topics/estimator_bias3d.h> #include <uORB/topics/estimator_bias3d.h>
#include <uORB/topics/estimator_event_flags.h> #include <uORB/topics/estimator_event_flags.h>
@@ -127,7 +126,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
{ {
public: public:
EKF2() = delete; EKF2() = delete;
EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode); EKF2(bool multi_mode, const px4::wq_config_t &config);
~EKF2() override; ~EKF2() override;
/** @see ModuleBase */ /** @see ModuleBase */
@@ -196,16 +195,16 @@ private:
#endif // CONFIG_EKF2_WIND #endif // CONFIG_EKF2_WIND
#if defined(CONFIG_EKF2_AIRSPEED) #if defined(CONFIG_EKF2_AIRSPEED)
void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateAirspeedSample();
#endif // CONFIG_EKF2_AIRSPEED #endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_AUXVEL) #if defined(CONFIG_EKF2_AUXVEL)
void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateAuxVelSample();
#endif // CONFIG_EKF2_AUXVEL #endif // CONFIG_EKF2_AUXVEL
#if defined(CONFIG_EKF2_BAROMETER) #if defined(CONFIG_EKF2_BAROMETER)
void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateBaroSample();
#endif // CONFIG_EKF2_BAROMETER #endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_EXTERNAL_VISION) #if defined(CONFIG_EKF2_EXTERNAL_VISION)
bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps); bool UpdateExtVisionSample();
#endif // CONFIG_EKF2_EXTERNAL_VISION #endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_GNSS) #if defined(CONFIG_EKF2_GNSS)
/* /*
@@ -216,20 +215,20 @@ private:
void PublishGpsStatus(const hrt_abstime &timestamp); void PublishGpsStatus(const hrt_abstime &timestamp);
void PublishGnssHgtBias(const hrt_abstime &timestamp); void PublishGnssHgtBias(const hrt_abstime &timestamp);
void PublishYawEstimatorStatus(const hrt_abstime &timestamp); void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateGpsSample();
#endif // CONFIG_EKF2_GNSS #endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_OPTICAL_FLOW) #if defined(CONFIG_EKF2_OPTICAL_FLOW)
bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps); bool UpdateFlowSample();
void PublishOpticalFlowVel(const hrt_abstime &timestamp); void PublishOpticalFlowVel(const hrt_abstime &timestamp);
#endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_MAGNETOMETER) #if defined(CONFIG_EKF2_MAGNETOMETER)
void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateMagSample();
#endif // CONFIG_EKF2_MAGNETOMETER #endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_RANGE_FINDER) #if defined(CONFIG_EKF2_RANGE_FINDER)
void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateRangeSample();
#endif // CONFIG_EKF2_RANGE_FINDER #endif // CONFIG_EKF2_RANGE_FINDER
void UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateSystemFlagsSample();
// Used to check, save and use learned accel/gyro/mag biases // Used to check, save and use learned accel/gyro/mag biases
struct InFlightCalibration { struct InFlightCalibration {
@@ -265,7 +264,6 @@ private:
static constexpr float sq(float x) { return x * x; }; static constexpr float sq(float x) { return x * x; };
const bool _replay_mode{false}; ///< true when we use replay data from a log
const bool _multi_mode; const bool _multi_mode;
int _instance{0}; int _instance{0};
@@ -434,7 +432,6 @@ private:
uint32_t _filter_warning_event_changes{0}; uint32_t _filter_warning_event_changes{0};
uint32_t _filter_information_event_changes{0}; uint32_t _filter_information_event_changes{0};
uORB::PublicationMulti<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
uORB::PublicationMultiData<estimator_event_flags_s> _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; uORB::PublicationMultiData<estimator_event_flags_s> _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
+62 -56
View File
@@ -151,64 +151,67 @@ void LoggedTopics::add_default_topics()
#if CONSTRAINED_MEMORY #if CONSTRAINED_MEMORY
static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 1; static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 1;
#else #else
static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 6; // artificially limited until PlotJuggler fixed static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 4; // artificially limited until PlotJuggler fixed
add_optional_topic("estimator_selector_status"); add_optional_topic("estimator_selector_status");
add_optional_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_attitude", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_global_position", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_local_position", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_wind", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_wind", 0, MAX_ESTIMATOR_INSTANCES);
#endif #endif
// always add the first instance // always add the first instance
add_topic("estimator_baro_bias", 500); add_topic_multi("estimator_baro_bias", 0, MAX_ESTIMATOR_INSTANCES);
add_topic("estimator_gnss_hgt_bias", 500); add_topic_multi("estimator_gnss_hgt_bias", 0, MAX_ESTIMATOR_INSTANCES);
add_topic("estimator_rng_hgt_bias", 500); add_topic_multi("estimator_rng_hgt_bias", 0, MAX_ESTIMATOR_INSTANCES);
add_topic("estimator_ev_pos_bias", 500); add_topic_multi("estimator_ev_pos_bias", 0, MAX_ESTIMATOR_INSTANCES);
add_topic("estimator_event_flags", 0); add_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES);
add_topic("estimator_gps_status", 1000); add_topic_multi("estimator_gps_status", 0, MAX_ESTIMATOR_INSTANCES);
add_topic("estimator_innovation_test_ratios", 500); add_topic_multi("estimator_innovation_test_ratios", 0, MAX_ESTIMATOR_INSTANCES);
add_topic("estimator_innovation_variances", 500); add_topic_multi("estimator_innovation_variances", 0, MAX_ESTIMATOR_INSTANCES);
add_topic("estimator_innovations", 500); add_topic_multi("estimator_innovations", 0, MAX_ESTIMATOR_INSTANCES);
add_topic("estimator_optical_flow_vel", 200); add_topic_multi("estimator_optical_flow_vel", 0, MAX_ESTIMATOR_INSTANCES);
add_topic("estimator_sensor_bias", 0); add_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES);
add_topic("estimator_states", 1000); add_topic_multi("estimator_states", 0, MAX_ESTIMATOR_INSTANCES);
add_topic("estimator_status", 200); add_topic_multi("estimator_status", 0, MAX_ESTIMATOR_INSTANCES);
add_topic("estimator_status_flags", 0); add_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES);
add_topic("yaw_estimator_status", 1000); add_topic_multi("yaw_estimator_status", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_baro_bias", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_gnss_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_gnss_hgt_bias", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_rng_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_rng_hgt_bias", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_ev_pos_bias", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_ev_pos_bias", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_gps_status", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_gps_status", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_innovation_test_ratios", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_innovation_variances", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_innovations", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_innovations", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_optical_flow_vel", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_states", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_status", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("yaw_estimator_status", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_airspeed", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_airspeed", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_baro_hgt", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_baro_hgt", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_ev_pos", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_ev_pos", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_ev_vel", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_ev_vel", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_gravity", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_gravity", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_rng_hgt", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_rng_hgt", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_fake_hgt", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_fake_hgt", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_fake_pos", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_fake_pos", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_gnss_yaw", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_gnss_yaw", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_gnss_vel", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_gnss_vel", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_gnss_pos", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_gnss_pos", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_optical_flow", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_terrain_optical_flow", 0, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_aid_src_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_aid_src_wind", 0, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_aid_src_air_flow_speed", 0, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_aid_src_air_flow_direction", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_aid_src_aux_global_position", 100, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_aux_global_position", 100, MAX_ESTIMATOR_INSTANCES);
// log all raw sensors at minimal rate (at least 1 Hz) // log all raw sensors at minimal rate (at least 1 Hz)
@@ -338,22 +341,25 @@ void LoggedTopics::add_debug_topics()
void LoggedTopics::add_estimator_replay_topics() void LoggedTopics::add_estimator_replay_topics()
{ {
// for estimator replay (need to be at full rate)
add_topic("ekf2_timestamps");
// current EKF2 subscriptions // current EKF2 subscriptions
add_topic("airspeed"); add_topic("airspeed");
add_topic("vehicle_optical_flow"); add_topic("airspeed_validated");
add_topic("landing_target_pose");
add_topic("parameter_update");
add_topic("sensor_airflow");
add_topic("sensor_combined"); add_topic("sensor_combined");
add_topic("sensor_selection"); add_topic("sensor_selection");
add_topic("vehicle_air_data"); add_topic("vehicle_air_data");
add_topic("vehicle_command");
add_topic("vehicle_gps_position"); add_topic("vehicle_gps_position");
add_topic("vehicle_land_detected"); add_topic("vehicle_land_detected");
add_topic("vehicle_magnetometer"); add_topic("vehicle_magnetometer");
add_topic("vehicle_optical_flow");
add_topic("vehicle_status"); add_topic("vehicle_status");
add_topic("vehicle_visual_odometry"); add_topic("vehicle_visual_odometry");
add_topic("aux_global_position"); add_topic("aux_global_position");
add_topic_multi("distance_sensor"); add_topic_multi("distance_sensor");
add_topic_multi("vehicle_imu");
} }
void LoggedTopics::add_thermal_calibration_topics() void LoggedTopics::add_thermal_calibration_topics()
+6 -5
View File
@@ -690,7 +690,8 @@ void Logger::run()
int next_subscribe_topic_index = -1; // this is used to distribute the checks over time int next_subscribe_topic_index = -1; // this is used to distribute the checks over time
if (polling_topic_sub >= 0) { if (polling_topic_sub >= 0) {
_lockstep_component = px4_lockstep_register_component(); //_lockstep_component = px4_lockstep_register_component();
//PX4_ERR("Lockstep component: %i", _lockstep_component);
} }
bool was_started = false; bool was_started = false;
@@ -899,7 +900,7 @@ void Logger::run()
// wait for next loop iteration... // wait for next loop iteration...
if (polling_topic_sub >= 0) { if (polling_topic_sub >= 0) {
px4_lockstep_progress(_lockstep_component); //px4_lockstep_progress(_lockstep_component);
px4_pollfd_struct_t fds[1]; px4_pollfd_struct_t fds[1];
fds[0].fd = polling_topic_sub; fds[0].fd = polling_topic_sub;
@@ -928,7 +929,7 @@ void Logger::run()
} }
} }
px4_lockstep_unregister_component(_lockstep_component); //px4_lockstep_unregister_component(_lockstep_component);
stop_log_file(LogType::Full); stop_log_file(LogType::Full);
stop_log_file(LogType::Mission); stop_log_file(LogType::Mission);
@@ -1472,8 +1473,8 @@ void Logger::start_log_mavlink()
} }
// mavlink log does not work in combination with lockstep, it leads to dead-locks // mavlink log does not work in combination with lockstep, it leads to dead-locks
px4_lockstep_unregister_component(_lockstep_component); //px4_lockstep_unregister_component(_lockstep_component);
_lockstep_component = -1; //_lockstep_component = -1;
// initialize cpu load as early as possible to get more data // initialize cpu load as early as possible to get more data
initialize_load_output(PrintLoadReason::Preflight); initialize_load_output(PrintLoadReason::Preflight);
+3 -3
View File
@@ -1,6 +1,6 @@
############################################################################ ############################################################################
# #
# Copyright (c) 2016-2019 PX4 Development Team. All rights reserved. # Copyright (c) 2016-2023 PX4 Development Team. All rights reserved.
# #
# Redistribution and use in source and binary forms, with or without # Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions # modification, are permitted provided that the following conditions
@@ -34,11 +34,11 @@ px4_add_module(
MODULE modules__replay MODULE modules__replay
MAIN replay MAIN replay
COMPILE_FLAGS COMPILE_FLAGS
-O3
#-DDEBUG_BUILD
SRCS SRCS
definitions.hpp definitions.hpp
replay_main.cpp replay_main.cpp
Replay.cpp Replay.cpp
Replay.hpp Replay.hpp
ReplayEkf2.cpp
ReplayEkf2.hpp
) )
+422 -148
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2016-2019 PX4 Development Team. All rights reserved. * Copyright (c) 2016-2023 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -49,6 +49,10 @@
#include <lib/parameters/param.h> #include <lib/parameters/param.h>
#include <uORB/uORBMessageFields.hpp> #include <uORB/uORBMessageFields.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <cstring> #include <cstring>
#include <float.h> #include <float.h>
#include <fstream> #include <fstream>
@@ -63,7 +67,6 @@
#include <logger/messages.h> #include <logger/messages.h>
#include "Replay.hpp" #include "Replay.hpp"
#include "ReplayEkf2.hpp"
#define PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params.txt" #define PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params.txt"
#define DYNAMIC_PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params_dynamic.txt" #define DYNAMIC_PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params_dynamic.txt"
@@ -76,16 +79,6 @@ namespace px4
char *Replay::_replay_file = nullptr; char *Replay::_replay_file = nullptr;
Replay::CompatSensorCombinedDtType::CompatSensorCombinedDtType(int gyro_integral_dt_offset_log,
int gyro_integral_dt_offset_intern, int accelerometer_integral_dt_offset_log,
int accelerometer_integral_dt_offset_intern)
: _gyro_integral_dt_offset_log(gyro_integral_dt_offset_log),
_gyro_integral_dt_offset_intern(gyro_integral_dt_offset_intern),
_accelerometer_integral_dt_offset_log(accelerometer_integral_dt_offset_log),
_accelerometer_integral_dt_offset_intern(accelerometer_integral_dt_offset_intern)
{
}
Replay::~Replay() Replay::~Replay()
{ {
for (size_t i = 0; i < _subscriptions.size(); ++i) { for (size_t i = 0; i < _subscriptions.size(); ++i) {
@@ -95,29 +88,7 @@ Replay::~Replay()
_subscriptions.clear(); _subscriptions.clear();
} }
void * void Replay::setupReplayFile(const char *file_name)
Replay::CompatSensorCombinedDtType::apply(void *data)
{
// the types have the same size so we can do the conversion in-place
uint8_t *ptr = (uint8_t *)data;
float gyro_integral_dt;
memcpy(&gyro_integral_dt, ptr + _gyro_integral_dt_offset_log, sizeof(float));
float accel_integral_dt;
memcpy(&accel_integral_dt, ptr + _accelerometer_integral_dt_offset_log, sizeof(float));
uint32_t igyro_integral_dt = (uint32_t)(gyro_integral_dt * 1e6f);
memcpy(ptr + _gyro_integral_dt_offset_intern, &igyro_integral_dt, sizeof(float));
uint32_t iaccel_integral_dt = (uint32_t)(accel_integral_dt * 1e6f);
memcpy(ptr + _accelerometer_integral_dt_offset_intern, &iaccel_integral_dt, sizeof(float));
return data;
}
void
Replay::setupReplayFile(const char *file_name)
{ {
if (_replay_file) { if (_replay_file) {
free(_replay_file); free(_replay_file);
@@ -126,8 +97,7 @@ Replay::setupReplayFile(const char *file_name)
_replay_file = strdup(file_name); _replay_file = strdup(file_name);
} }
void void Replay::setParameter(const string &parameter_name, const double parameter_value)
Replay::setParameter(const string &parameter_name, const double parameter_value)
{ {
param_t handle = param_find(parameter_name.c_str()); param_t handle = param_find(parameter_name.c_str());
param_type_t param_format = param_type(handle); param_type_t param_format = param_type(handle);
@@ -151,7 +121,7 @@ Replay::setParameter(const string &parameter_name, const double parameter_value)
float value = (float)parameter_value; float value = (float)parameter_value;
if (fabsf(orig_value - value) > FLT_EPSILON) { if (fabsf(orig_value - value) > FLT_EPSILON) {
PX4_WARN("Setting %s (FLOAT) %.3f -> %.3f", param_name(handle), (double)orig_value, (double)value); PX4_WARN("Setting %s (FLOAT) %.6f -> %.6f", param_name(handle), (double)orig_value, (double)value);
} }
param_set(handle, (const void *)&value); param_set(handle, (const void *)&value);
@@ -391,7 +361,6 @@ Replay::readFormat(std::ifstream &file, uint16_t msg_size)
return true; return true;
} }
string Replay::getOrbFields(const orb_metadata *meta) string Replay::getOrbFields(const orb_metadata *meta)
{ {
char format[3000]; char format[3000];
@@ -440,6 +409,53 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
uint8_t multi_id = *(uint8_t *)message; uint8_t multi_id = *(uint8_t *)message;
uint16_t msg_id = ((uint16_t)message[1]) | (((uint16_t)message[2]) << 8); uint16_t msg_id = ((uint16_t)message[1]) | (((uint16_t)message[2]) << 8);
string topic_name((char *)message + 3); string topic_name((char *)message + 3);
const auto whitelist = {
"actuator_armed", // HACK
"action_request", // HACK
"airspeed",
"airspeed_validated",
"landing_target_pose",
"parameter_update",
"sensor_combined",
"sensor_selection",
"sensor_wind",
"vehicle_air_data",
"vehicle_command",
"vehicle_gps_position",
"vehicle_land_detected",
"vehicle_magnetometer",
"vehicle_optical_flow",
"vehicle_status",
"vehicle_visual_odometry",
"distance_sensor",
"vehicle_imu",
// "sensor_accel",
// "sensor_gyro",
};
{
// TODO: ekf2 replay
bool topic_whitelisted = false;
for (auto &t : whitelist) {
if (topic_name.compare(t) == 0) {
topic_whitelisted = true;
break;
}
}
if (!topic_whitelisted) {
PX4_DEBUG("readAndAddSubscription: skipping topic %s (not whitelisted)", topic_name.c_str());
return true;
}
}
const orb_metadata *orb_meta = findTopic(topic_name); const orb_metadata *orb_meta = findTopic(topic_name);
if (!orb_meta) { if (!orb_meta) {
@@ -447,7 +463,6 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
return true; return true;
} }
CompatBase *compat = nullptr;
// check the format: the field definitions must match // check the format: the field definitions must match
// FIXME: this should check recursively, all used nested types // FIXME: this should check recursively, all used nested types
@@ -456,34 +471,9 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
const string orb_fields = getOrbFields(orb_meta); const string orb_fields = getOrbFields(orb_meta);
if (file_format != orb_fields) { if (file_format != orb_fields) {
// check if we have a compatibility conversion available PX4_ERR("Formats for %s (msg_id %d) don't match. Will ignore it.", topic_name.c_str(), msg_id);
if (topic_name == "sensor_combined") {
if (orb_fields == "uint64_t timestamp;float[3] gyro_rad;uint32_t gyro_integral_dt;"
"int32_t accelerometer_timestamp_relative;float[3] accelerometer_m_s2;"
"uint32_t accelerometer_integral_dt" &&
file_format == "uint64_t timestamp;float[3] gyro_rad;float gyro_integral_dt;"
"int32_t accelerometer_timestamp_relative;float[3] accelerometer_m_s2;"
"float accelerometer_integral_dt;") {
int gyro_integral_dt_offset_log; if (false) {
int gyro_integral_dt_offset_intern;
int accelerometer_integral_dt_offset_log;
int accelerometer_integral_dt_offset_intern;
int unused;
if (findFieldOffset(file_format, "gyro_integral_dt", gyro_integral_dt_offset_log, unused) &&
findFieldOffset(orb_fields, "gyro_integral_dt", gyro_integral_dt_offset_intern, unused) &&
findFieldOffset(file_format, "accelerometer_integral_dt", accelerometer_integral_dt_offset_log, unused) &&
findFieldOffset(orb_fields, "accelerometer_integral_dt", accelerometer_integral_dt_offset_intern, unused)) {
compat = new CompatSensorCombinedDtType(gyro_integral_dt_offset_log, gyro_integral_dt_offset_intern,
accelerometer_integral_dt_offset_log, accelerometer_integral_dt_offset_intern);
}
}
}
if (!compat) {
PX4_ERR("Formats for %s don't match. Will ignore it.", topic_name.c_str());
PX4_WARN(" Internal format:"); PX4_WARN(" Internal format:");
size_t start = 0; size_t start = 0;
@@ -519,15 +509,14 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
start = i + 1; start = i + 1;
} }
} }
return true; // not a fatal error
} }
return true; // not a fatal error
} }
Subscription *subscription = new Subscription(); Subscription *subscription = new Subscription();
subscription->orb_meta = orb_meta; subscription->orb_meta = orb_meta;
subscription->multi_id = multi_id; subscription->multi_id = multi_id;
subscription->compat = compat;
//find the timestamp offset //find the timestamp offset
int field_size; int field_size;
@@ -561,7 +550,32 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
return true; return true;
} }
PX4_DEBUG("adding subscription for %s (msg_id %i)", subscription->orb_meta->o_name, msg_id);
{
// TODO: ekf2 replay
bool topic_whitelisted = false;
for (auto &t : whitelist) {
if (strcmp(subscription->orb_meta->o_name, t) == 0) {
topic_whitelisted = true;
break;
}
}
if (!topic_whitelisted) {
PX4_ERR("readAndAddSubscription: skipping topic %s late", subscription->orb_meta->o_name);
delete subscription;
return true;
}
}
PX4_INFO("adding subscription for %s (msg_id %i)", subscription->orb_meta->o_name, msg_id);
//add subscription //add subscription
if (_subscriptions.size() <= msg_id) { if (_subscriptions.size() <= msg_id) {
@@ -773,6 +787,12 @@ Replay::nextDataMessage(std::ifstream &file, Subscription &subscription, int msg
if (file.eof()) { //no more data messages for this subscription if (file.eof()) { //no more data messages for this subscription
subscription.orb_meta = nullptr; subscription.orb_meta = nullptr;
file.clear(); file.clear();
} else {
if (subscription.orb_meta) {
subscription.topic_name = (char *)subscription.orb_meta->o_name;
}
} }
return file.good(); return file.good();
@@ -885,6 +905,15 @@ Replay::readDefinitionsAndApplyParams(std::ifstream &file)
void void
Replay::run() Replay::run()
{ {
uint64_t timestamp_last = 0;
// time starts at 0
{
struct timespec ts {};
abstime_to_ts(&ts, timestamp_last);
px4_clock_settime(CLOCK_MONOTONIC, &ts);
}
ifstream replay_file(_replay_file, ios::in | ios::binary); ifstream replay_file(_replay_file, ios::in | ios::binary);
if (!readDefinitionsAndApplyParams(replay_file)) { if (!readDefinitionsAndApplyParams(replay_file)) {
@@ -898,10 +927,6 @@ Replay::run()
_speed_factor = atof(speedup); _speed_factor = atof(speedup);
} }
onEnterMainLoop();
_replay_start_time = hrt_absolute_time();
PX4_INFO("Replay in progress..."); PX4_INFO("Replay in progress...");
ulog_message_header_s message_header; ulog_message_header_s message_header;
@@ -910,19 +935,36 @@ Replay::run()
//we know the next message must be an ADD_LOGGED_MSG //we know the next message must be an ADD_LOGGED_MSG
replay_file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); replay_file.read((char *)&message_header, ULOG_MSG_HEADER_LEN);
// TODO: add all subscriptions here
// from whitelist find all topics and subscriptions
if (!readAndAddSubscription(replay_file, message_header.msg_size)) { if (!readAndAddSubscription(replay_file, message_header.msg_size)) {
PX4_ERR("Failed to read subscription"); PX4_ERR("Failed to read subscription");
return; return;
} }
const uint64_t timestamp_offset = getTimestampOffset();
uint32_t nr_published_messages = 0; uint32_t nr_published_messages = 0;
streampos last_additional_message_pos = _data_section_start; streampos last_additional_message_pos = _data_section_start;
while (!should_exit() && replay_file) { bool wait_on_imu_pub[4] {};
bool imu_published = false;
uint8_t imu_id = 0;
//Find the next message to publish. Messages from different subscriptions don't need hrt_abstime last_update = 0;
//to be in chronological order, so we need to check all subscriptions
PX4_INFO("Replay begin");
while (!should_exit() && replay_file) {
// Find the next message to publish. Messages from different subscriptions don't need
// to be in chronological order, so we need to check all subscriptions
uint64_t next_file_time = 0; uint64_t next_file_time = 0;
int next_msg_id = -1; int next_msg_id = -1;
bool first_time = true; bool first_time = true;
@@ -944,18 +986,24 @@ Replay::run()
} }
if (next_msg_id == -1) { if (next_msg_id == -1) {
break; //no active subscription anymore. We're done. break; // no active subscription anymore. We're done.
} }
Subscription &sub = *_subscriptions[next_msg_id]; Subscription &sub = *_subscriptions[next_msg_id];
if (next_file_time == 0 || next_file_time < _file_start_time) { if (next_file_time == 0 || next_file_time < _file_start_time) {
//someone didn't set the timestamp properly. Consider the message invalid // someone didn't set the timestamp properly. Consider the message invalid
PX4_ERR("%s:%d missing timestamp", sub.orb_meta->o_name, sub.multi_id);
nextDataMessage(replay_file, sub, next_msg_id); nextDataMessage(replay_file, sub, next_msg_id);
continue; continue;
} }
//handle additional messages between last and next published data if (findTopic(_publisher_rule, sub.orb_meta->o_name)) {
PX4_DEBUG("not allowing publish topic %s", sub.orb_meta->o_name);
//return (orb_advert_t)_Instance;
}
// handle additional messages between last and next published data
replay_file.seekg(last_additional_message_pos); replay_file.seekg(last_additional_message_pos);
streampos next_additional_message_pos = sub.next_read_pos; streampos next_additional_message_pos = sub.next_read_pos;
readAndHandleAdditionalMessages(replay_file, next_additional_message_pos); readAndHandleAdditionalMessages(replay_file, next_additional_message_pos);
@@ -972,14 +1020,164 @@ Replay::run()
_next_param_change++; _next_param_change++;
} }
const uint64_t publish_timestamp = handleTopicDelay(next_file_time, timestamp_offset); // Perform scheduled parameter changes
while (_next_param_change < _dynamic_parameter_schedule.size() &&
_dynamic_parameter_schedule[_next_param_change].timestamp <= next_file_time) {
// It's time to publish const auto param_change = _dynamic_parameter_schedule[_next_param_change];
readTopicDataToBuffer(sub, replay_file); PX4_WARN("Performing param change scheduled for t=%.3lf at t=%.3lf.",
memcpy(_read_buffer.data() + sub.timestamp_offset, &publish_timestamp, sizeof(uint64_t)); //adjust the timestamp (double)param_change.timestamp / 1.e6,
(double)next_file_time / 1.e6);
if (handleTopicUpdate(sub, _read_buffer.data(), replay_file)) { setParameter(param_change.parameter_name, param_change.parameter_value);
++nr_published_messages; _next_param_change++;
}
bool publish_msg = true;
const uint64_t publish_timestamp = sub.next_timestamp;
// // TODO: ekf2 replay
// const auto whitelist = {
// "airspeed",
// "airspeed_validated",
// "landing_target_pose",
// "parameter_update",
// "sensor_combined",
// "sensor_selection",
// "sensor_wind",
// "vehicle_air_data",
// "vehicle_command",
// "vehicle_gps_position",
// "vehicle_land_detected",
// "vehicle_magnetometer",
// "vehicle_optical_flow",
// "vehicle_status",
// "vehicle_visual_odometry",
// "distance_sensor",
// "vehicle_imu",
// "sensor_accel",
// "sensor_gyro",
// };
// if (sub.orb_meta) {
// bool topic_whitelisted = false;
// for (auto &topic : whitelist) {
// if (strcmp(sub.orb_meta->o_name, topic) == 0) {
// topic_whitelisted = true;
// break;
// }
// }
// if (!topic_whitelisted) {
// // skip
// PX4_DEBUG("%lu [%lu] (+%lu us) skipping %s:%d", hrt_absolute_time(), publish_timestamp,
// publish_timestamp - timestamp_last,
// sub.orb_meta->o_name, sub.multi_id);
// //continue;
// publish_msg = false;
// }
// }
if (publish_msg) {
// It's time to publish
readTopicDataToBuffer(sub, replay_file);
if (publish_timestamp >= timestamp_last) {
// adjust the lockstep time to the publication time
struct timespec ts {};
abstime_to_ts(&ts, publish_timestamp);
px4_clock_settime(CLOCK_MONOTONIC, &ts);
if (_replay_start_time == 0) {
_replay_start_time = hrt_absolute_time();
}
PX4_DEBUG("%lu [%lu] (+%lu us) publishing %s:%d", hrt_absolute_time(), publish_timestamp,
publish_timestamp - timestamp_last,
sub.orb_meta->o_name, sub.multi_id);
} else {
PX4_ERR("%s:%d bad timestamp %" PRIu64 " (last timestamp %" PRIu64 ")", sub.orb_meta->o_name, sub.multi_id,
publish_timestamp,
timestamp_last);
}
if (sub.orb_advert) {
}
if (publishTopic(sub, _read_buffer.data())) {
if (strcmp(sub.orb_meta->o_name, "vehicle_imu") == 0) {
imu_published = true;
imu_id = sub.multi_id;
//fprintf(stderr, "\n\n\npublished IMU:%d\n", imu_id);
}
++nr_published_messages;
}
timestamp_last = publish_timestamp;
// Wait for other modules, such as logger or ekf2
if (imu_published) {
if (!wait_on_imu_pub[imu_id]) {
uORB::SubscriptionData<vehicle_local_position_s> est_sub{ORB_ID(estimator_local_position), imu_id};
est_sub.update();
if (est_sub.get().timestamp >= publish_timestamp) {
PX4_INFO("will now wait on IMU %d", imu_id);
wait_on_imu_pub[imu_id] = true;
}
} else {
//PX4_WARN("LOCKSTEP START wait for components, IMU:%d", imu_id);
px4_lockstep_wait_for_components();
//PX4_WARN("LOCKSTEP FINISH waited for components, IMU:%d", imu_id);
}
}
if (hrt_elapsed_time(&last_update) >= 10_s) {
// char buf[80];
// {
// struct timespec now;
// system_clock_gettime(CLOCK_REALTIME, &now);
// time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
// //convert to date time
// struct tm date_time;
// localtime_r(&utc_time_sec, &date_time);
// strftime(buf, sizeof(buf), "%a %Y-%m-%d %H:%M:%S %Z", &date_time);
// }
PX4_INFO("[%.6fs] %d messages published", timestamp_last * 1e-6, nr_published_messages);
last_update = timestamp_last;
}
} }
nextDataMessage(replay_file, sub, next_msg_id); nextDataMessage(replay_file, sub, next_msg_id);
@@ -987,19 +1185,25 @@ Replay::run()
// TODO: output status (eg. every sec), including total duration... // TODO: output status (eg. every sec), including total duration...
} }
for (auto &subscription : _subscriptions) { for (auto &sub : _subscriptions) {
if (!subscription) { if (!sub) {
continue; continue;
} }
if (subscription->compat) { if (sub && (sub->publication_counter > 0 || sub->error_counter > 0)) {
delete subscription->compat;
subscription->compat = nullptr; if (sub->topic_name) {
//PX4_INFO("%s: %i, %i", sub->orb_meta->o_name, sub->publication_counter, sub->error_counter);
PX4_INFO("%s: %i, %i", sub->topic_name, sub->publication_counter, sub->error_counter);
} else {
PX4_WARN("%s: %i, %i", "HUH?", sub->publication_counter, sub->error_counter);
}
} }
if (subscription->orb_advert) { if (sub->orb_advert) {
orb_unadvertise(subscription->orb_advert); orb_unadvertise(sub->orb_advert);
subscription->orb_advert = nullptr; sub->orb_advert = nullptr;
} }
} }
@@ -1008,8 +1212,6 @@ Replay::run()
(double)hrt_elapsed_time(&_replay_start_time) / 1.e6); (double)hrt_elapsed_time(&_replay_start_time) / 1.e6);
} }
onExitMainLoop();
if (!should_exit()) { if (!should_exit()) {
replay_file.close(); replay_file.close();
px4_shutdown_request(); px4_shutdown_request();
@@ -1035,50 +1237,11 @@ Replay::readTopicDataToBuffer(const Subscription &sub, std::ifstream &replay_fil
replay_file.read((char *)_read_buffer.data(), msg_read_size); replay_file.read((char *)_read_buffer.data(), msg_read_size);
} }
bool
Replay::handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file)
{
return publishTopic(sub, data);
}
uint64_t
Replay::handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset)
{
const uint64_t publish_timestamp = next_file_time + timestamp_offset;
// wait if necessary
uint64_t cur_time = hrt_absolute_time();
// if some topics have a timestamp smaller than the log file start, publish them immediately
if (cur_time < publish_timestamp && next_file_time > _file_start_time) {
if (_speed_factor > FLT_EPSILON) {
// avoid many small usleep calls
_accumulated_delay += (publish_timestamp - cur_time) / _speed_factor;
if (_accumulated_delay > 3000) {
system_usleep(_accumulated_delay);
_accumulated_delay = 0.f;
}
}
// adjust the lockstep time to the publication time
struct timespec ts;
abstime_to_ts(&ts, publish_timestamp);
px4_clock_settime(CLOCK_MONOTONIC, &ts);
}
return publish_timestamp;
}
bool bool
Replay::publishTopic(Subscription &sub, void *data) Replay::publishTopic(Subscription &sub, void *data)
{ {
bool published = false; bool published = false;
if (sub.compat) {
data = sub.compat->apply(data);
}
if (sub.orb_advert) { if (sub.orb_advert) {
orb_publish(sub.orb_meta, sub.orb_advert, data); orb_publish(sub.orb_meta, sub.orb_advert, data);
published = true; published = true;
@@ -1120,6 +1283,123 @@ Replay::publishTopic(Subscription &sub, void *data)
return published; return published;
} }
bool Replay::startsWith(const char *pre, const char *str)
{
size_t lenpre = strlen(pre);
size_t lenstr = strlen(str);
if (lenstr < lenpre) {
return false;
}
return (strncmp(pre, str, lenpre) == 0);
}
bool Replay::findTopic(const PublisherRule &rule, const char *topic_name)
{
const char **topics_ptr = rule.topics;
while (topics_ptr && *topics_ptr) {
if (strcmp(*topics_ptr, topic_name) == 0) {
return true;
}
++topics_ptr;
}
return false;
}
void Replay::strTrim(const char **str)
{
while (**str == ' ' || **str == '\t') { ++(*str); }
}
int Replay::readPublisherRulesFromFile(const char *file_name, PublisherRule &rule)
{
static const int line_len = 1024;
int ret = PX4_OK;
char *line = new char[line_len];
if (!line) {
return -ENOMEM;
}
FILE *fp = fopen(file_name, "r");
if (fp == NULL) {
delete[](line);
return -errno;
}
const char *restrict_topics_str = "restrict_topics:";
rule.topics = nullptr;
while (fgets(line, line_len, fp) && ret == PX4_OK) {
if (strlen(line) < 2 || line[0] == '#') {
continue;
}
if (startsWith(restrict_topics_str, line)) {
// read topics list
char *start = line + strlen(restrict_topics_str);
strTrim((const char **)&start);
char *topics = strdup(start);
int topic_len = 0;
int num_topics = 0;
for (int i = 0; topics[i]; ++i) {
if (topics[i] == ',' || topics[i] == '\n') {
if (topic_len > 0) {
topics[i] = 0;
++num_topics;
}
topic_len = 0;
} else {
++topic_len;
}
}
if (num_topics > 0) {
rule.topics = new const char *[num_topics + 1];
int topic = 0;
strTrim((const char **)&topics);
rule.topics[topic++] = topics;
while (topic < num_topics) {
if (*topics == 0) {
++topics;
strTrim((const char **)&topics);
rule.topics[topic++] = topics;
} else {
++topics;
}
}
rule.topics[num_topics] = nullptr;
}
} else {
PX4_ERR("orb rules file: wrong format: %s", line);
ret = -EINVAL;
}
}
if (ret == PX4_OK && !rule.topics) {
PX4_ERR("Wrong format in orb publisher rules file");
ret = -EINVAL;
}
delete[](line);
fclose(fp);
return ret;
}
int int
Replay::custom_command(int argc, char *argv[]) Replay::custom_command(int argc, char *argv[])
{ {
@@ -1131,6 +1411,8 @@ Replay::custom_command(int argc, char *argv[])
return Replay::task_spawn(argc, argv); return Replay::task_spawn(argc, argv);
} }
// ignore?
return print_usage("unknown command"); return print_usage("unknown command");
} }
@@ -1197,17 +1479,9 @@ Replay *
Replay::instantiate(int argc, char *argv[]) Replay::instantiate(int argc, char *argv[])
{ {
// check the replay mode // check the replay mode
const char *replay_mode = getenv(replay::ENV_MODE); //const char *replay_mode = getenv(replay::ENV_MODE);
Replay *instance = nullptr; Replay *instance = new Replay();
if (replay_mode && strcmp(replay_mode, "ekf2") == 0) {
PX4_INFO("Ekf2 replay mode");
instance = new ReplayEkf2();
} else {
instance = new Replay();
}
return instance; return instance;
} }
@@ -1224,7 +1498,7 @@ Replay::print_usage(const char *reason)
### Description ### Description
This module is used to replay ULog files. This module is used to replay ULog files.
There are 2 environment variables used for configuration: `replay`, which must be set to an ULog file name - it's There are 2 environment variables used for configuration: `replay_file`, which must be set to an ULog file name - it's
the log file to be replayed. The second is the mode, specified via `replay_mode`: the log file to be replayed. The second is the mode, specified via `replay_mode`:
- `replay_mode=ekf2`: specific EKF2 replay mode. It can only be used with the ekf2 module, but allows the replay - `replay_mode=ekf2`: specific EKF2 replay mode. It can only be used with the ekf2 module, but allows the replay
to run as fast as possible. to run as fast as possible.
+42 -55
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2016-2019 PX4 Development Team. All rights reserved. * Copyright (c) 2016-2024 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -44,7 +44,6 @@
#include <px4_platform_common/module.h> #include <px4_platform_common/module.h>
#include <uORB/topics/uORBTopics.hpp> #include <uORB/topics/uORBTopics.hpp>
#include <uORB/topics/ekf2_timestamps.h>
namespace px4 namespace px4
{ {
@@ -96,39 +95,10 @@ public:
protected: protected:
/**
* @class Compatibility base class to convert topics to an updated format
*/
class CompatBase
{
public:
virtual ~CompatBase() = default;
/**
* apply compatibility to a topic
* @param data input topic (can be modified in place)
* @return new topic data
*/
virtual void *apply(void *data) = 0;
};
class CompatSensorCombinedDtType : public CompatBase
{
public:
CompatSensorCombinedDtType(int gyro_integral_dt_offset_log, int gyro_integral_dt_offset_intern,
int accelerometer_integral_dt_offset_log, int accelerometer_integral_dt_offset_intern);
void *apply(void *data) override;
private:
int _gyro_integral_dt_offset_log;
int _gyro_integral_dt_offset_intern;
int _accelerometer_integral_dt_offset_log;
int _accelerometer_integral_dt_offset_intern;
};
struct Subscription { struct Subscription {
const orb_metadata *orb_meta = nullptr; ///< if nullptr, this subscription is invalid const orb_metadata *orb_meta = nullptr; ///< if nullptr, this subscription is invalid
char *topic_name = nullptr;
orb_advert_t orb_advert = nullptr; orb_advert_t orb_advert = nullptr;
uint8_t multi_id; uint8_t multi_id;
int timestamp_offset; ///< marks the field of the timestamp int timestamp_offset; ///< marks the field of the timestamp
@@ -138,8 +108,6 @@ protected:
std::streampos next_read_pos; std::streampos next_read_pos;
uint64_t next_timestamp; ///< timestamp of the file uint64_t next_timestamp; ///< timestamp of the file
CompatBase *compat = nullptr;
// statistics // statistics
int error_counter = 0; int error_counter = 0;
int publication_counter = 0; int publication_counter = 0;
@@ -163,34 +131,16 @@ protected:
*/ */
bool publishTopic(Subscription &sub, void *data); bool publishTopic(Subscription &sub, void *data);
/**
* called when entering the main replay loop
*/
virtual void onEnterMainLoop() {}
/**
* called when exiting the main replay loop
*/
virtual void onExitMainLoop() {}
/** /**
* called when a new subscription is added * called when a new subscription is added
*/ */
virtual void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) {} void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) {}
/**
* handle delay until topic can be published.
* @param next_file_timestamp timestamp of next message to publish
* @param timestamp_offset offset between file start time and replay start time
* @return timestamp that the message to publish should have
*/
virtual uint64_t handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset);
/** /**
* handle the publication of a topic update * handle the publication of a topic update
* @return true if published, false otherwise * @return true if published, false otherwise
*/ */
virtual bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file); bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file);
/** /**
* read a topic from the file (offset given by the subscription) into _read_buffer * read a topic from the file (offset given by the subscription) into _read_buffer
@@ -207,7 +157,7 @@ protected:
*/ */
bool nextDataMessage(std::ifstream &file, Subscription &subscription, int msg_id); bool nextDataMessage(std::ifstream &file, Subscription &subscription, int msg_id);
virtual uint64_t getTimestampOffset() uint64_t getTimestampOffset()
{ {
//we update the timestamps from the file by a constant offset to match //we update the timestamps from the file by a constant offset to match
//the current replay time //the current replay time
@@ -300,6 +250,43 @@ private:
std::string getOrbFields(const orb_metadata *meta); std::string getOrbFields(const orb_metadata *meta);
static char *_replay_file; static char *_replay_file;
struct PublisherRule {
const char **topics{nullptr}; //null-terminated list of topic names
};
/**
* test if str starts with pre
*/
bool startsWith(const char *pre, const char *str);
/**
* find a topic in a rule
*/
bool findTopic(const PublisherRule &rule, const char *topic_name);
/**
* trim whitespace from the beginning of a string
*/
void strTrim(const char **str);
/**
* Read publisher rules from a file. It has the format:
*
* restrict_topics: <topic1>, <topic2>, <topic3>
* module: <module_name>
* [ignore_others:true]
*
* @return 0 on success, <0 otherwise
*/
int readPublisherRulesFromFile(const char *file_name, PublisherRule &rule);
PublisherRule _publisher_rule{};
bool _has_publisher_rules{false};
}; };
} //namespace px4 } //namespace px4
-225
View File
@@ -1,225 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <drivers/drv_hrt.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <lib/parameters/param.h>
// for ekf2 replay
#include <uORB/topics/airspeed.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/vehicle_optical_flow.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_odometry.h>
#include "ReplayEkf2.hpp"
namespace px4
{
bool
ReplayEkf2::handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file)
{
if (sub.orb_meta == ORB_ID(ekf2_timestamps)) {
ekf2_timestamps_s ekf2_timestamps;
memcpy(&ekf2_timestamps, data, sub.orb_meta->o_size);
if (!publishEkf2Topics(ekf2_timestamps, replay_file)) {
return false;
}
// Wait for modules to process the data
px4_lockstep_wait_for_components();
return true;
} else if (sub.orb_meta == ORB_ID(vehicle_status) || sub.orb_meta == ORB_ID(vehicle_land_detected)
|| sub.orb_meta == ORB_ID(vehicle_gps_position)) {
return publishTopic(sub, data);
} // else: do not publish
return false;
}
void
ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
{
if (sub.orb_meta == ORB_ID(sensor_combined)) {
_sensor_combined_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(airspeed)) {
_airspeed_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(distance_sensor)) {
_distance_sensor_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(vehicle_optical_flow)) {
_optical_flow_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(vehicle_air_data)) {
_vehicle_air_data_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(vehicle_magnetometer)) {
_vehicle_magnetometer_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(vehicle_visual_odometry)) {
_vehicle_visual_odometry_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(aux_global_position)) {
_aux_global_position_msg_id = msg_id;
}
// the main loop should only handle publication of the following topics, the sensor topics are
// handled separately in publishEkf2Topics()
// Note: the GPS is not treated here since not missing data is more important than the accuracy of the timestamp
sub.ignored = sub.orb_meta != ORB_ID(ekf2_timestamps) && sub.orb_meta != ORB_ID(vehicle_status)
&& sub.orb_meta != ORB_ID(vehicle_land_detected) && sub.orb_meta != ORB_ID(vehicle_gps_position);
}
bool
ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file)
{
auto handle_sensor_publication = [&](int16_t timestamp_relative, uint16_t msg_id) {
if (timestamp_relative != ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID) {
// timestamp_relative is already given in 0.1 ms
uint64_t t = timestamp_relative + ekf2_timestamps.timestamp / 100; // in 0.1 ms
findTimestampAndPublish(t, msg_id, replay_file);
}
};
handle_sensor_publication(ekf2_timestamps.airspeed_timestamp_rel, _airspeed_msg_id);
handle_sensor_publication(ekf2_timestamps.distance_sensor_timestamp_rel, _distance_sensor_msg_id);
handle_sensor_publication(ekf2_timestamps.optical_flow_timestamp_rel, _optical_flow_msg_id);
handle_sensor_publication(ekf2_timestamps.vehicle_air_data_timestamp_rel, _vehicle_air_data_msg_id);
handle_sensor_publication(ekf2_timestamps.vehicle_magnetometer_timestamp_rel, _vehicle_magnetometer_msg_id);
handle_sensor_publication(ekf2_timestamps.visual_odometry_timestamp_rel, _vehicle_visual_odometry_msg_id);
handle_sensor_publication(0, _aux_global_position_msg_id);
// sensor_combined: publish last because ekf2 is polling on this
if (!findTimestampAndPublish(ekf2_timestamps.timestamp / 100, _sensor_combined_msg_id, replay_file)) {
if (_sensor_combined_msg_id == msg_id_invalid) {
// subscription not found yet or sensor_combined not contained in log
return false;
} else if (!_subscriptions[_sensor_combined_msg_id]->orb_meta) {
return false; // read past end of file
} else {
// we should publish a topic, just publish the same again
readTopicDataToBuffer(*_subscriptions[_sensor_combined_msg_id], replay_file);
publishTopic(*_subscriptions[_sensor_combined_msg_id], _read_buffer.data());
}
}
return true;
}
bool
ReplayEkf2::findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::ifstream &replay_file)
{
if (msg_id == msg_id_invalid) {
// could happen if a topic is not logged
return false;
}
Subscription &sub = *_subscriptions[msg_id];
while (sub.next_timestamp / 100 < timestamp && sub.orb_meta) {
nextDataMessage(replay_file, sub, msg_id);
}
if (!sub.orb_meta) { // no messages anymore
return false;
}
if (sub.next_timestamp / 100 != timestamp) {
// this can happen in beginning of the log or on a dropout
PX4_DEBUG("No timestamp match found for topic %s (%i, %i)", sub.orb_meta->o_name, (int)sub.next_timestamp / 100,
timestamp);
++sub.error_counter;
return false;
}
readTopicDataToBuffer(sub, replay_file);
publishTopic(sub, _read_buffer.data());
return true;
}
void
ReplayEkf2::onEnterMainLoop()
{
_speed_factor = 0.f; // iterate as fast as possible
// disable parameter auto save
param_control_autosave(false);
}
void
ReplayEkf2::onExitMainLoop()
{
// print statistics
auto print_sensor_statistics = [this](uint16_t msg_id, const char *name) {
if (msg_id != msg_id_invalid) {
Subscription &sub = *_subscriptions[msg_id];
if (sub.publication_counter > 0 || sub.error_counter > 0) {
PX4_INFO("%s: %i, %i", name, sub.publication_counter, sub.error_counter);
}
}
};
PX4_INFO("");
PX4_INFO("Topic, Num Published, Num Error (no timestamp match found):");
print_sensor_statistics(_airspeed_msg_id, "airspeed");
print_sensor_statistics(_distance_sensor_msg_id, "distance_sensor");
print_sensor_statistics(_optical_flow_msg_id, "vehicle_optical_flow");
print_sensor_statistics(_sensor_combined_msg_id, "sensor_combined");
print_sensor_statistics(_vehicle_air_data_msg_id, "vehicle_air_data");
print_sensor_statistics(_vehicle_magnetometer_msg_id, "vehicle_magnetometer");
print_sensor_statistics(_vehicle_visual_odometry_msg_id, "vehicle_visual_odometry");
print_sensor_statistics(_aux_global_position_msg_id, "aux_global_position");
}
} // namespace px4
-94
View File
@@ -1,94 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "Replay.hpp"
namespace px4
{
/**
* @class ReplayEkf2
* replay specialization for Ekf2 replay
*/
class ReplayEkf2 : public Replay
{
public:
protected:
void onEnterMainLoop() override;
void onExitMainLoop() override;
/**
* handle ekf2 topic publication in ekf2 replay mode
* @param sub
* @param data
* @param replay_file file currently replayed (file seek position should be considered arbitrary after this call)
* @return true if published, false otherwise
*/
bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file) override;
void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) override;
uint64_t getTimestampOffset() override
{
// avoid offsetting timestamps as we use them to compare against the log
return 0;
}
private:
bool publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file);
/**
* find the next message for a subscription that matches a given timestamp and publish it
* @param timestamp in 0.1 ms
* @param msg_id
* @param replay_file file currently replayed (file seek position should be considered arbitrary after this call)
* @return true if timestamp found and published
*/
bool findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::ifstream &replay_file);
static constexpr uint16_t msg_id_invalid = 0xffff;
uint16_t _airspeed_msg_id = msg_id_invalid;
uint16_t _distance_sensor_msg_id = msg_id_invalid;
uint16_t _optical_flow_msg_id = msg_id_invalid;
uint16_t _sensor_combined_msg_id = msg_id_invalid;
uint16_t _vehicle_air_data_msg_id = msg_id_invalid;
uint16_t _vehicle_magnetometer_msg_id = msg_id_invalid;
uint16_t _vehicle_visual_odometry_msg_id = msg_id_invalid;
uint16_t _aux_global_position_msg_id = msg_id_invalid;
};
} //namespace px4
+1 -1
View File
@@ -38,7 +38,7 @@ namespace px4
namespace replay namespace replay
{ {
static const char __attribute__((unused)) *ENV_FILENAME = "replay"; ///< name for getenv() static const char __attribute__((unused)) *ENV_FILENAME = "replay_file"; ///< name for getenv()
static const char __attribute__((unused)) *ENV_MODE = "replay_mode"; ///< name for getenv() static const char __attribute__((unused)) *ENV_MODE = "replay_mode"; ///< name for getenv()
+2 -3
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2016-2019 PX4 Development Team. All rights reserved. * Copyright (c) 2016-2023 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -35,8 +35,7 @@
using namespace px4; using namespace px4;
extern "C" __EXPORT int extern "C" __EXPORT int replay_main(int argc, char *argv[])
replay_main(int argc, char *argv[])
{ {
//check for logfile env variable //check for logfile env variable
const char *logfile = getenv(replay::ENV_FILENAME); const char *logfile = getenv(replay::ENV_FILENAME);
-3
View File
@@ -78,9 +78,6 @@ The code is optimized to minimize the memory footprint and the latency to exchan
Messages are defined in the `/msg` directory. They are converted into C/C++ code at build-time. Messages are defined in the `/msg` directory. They are converted into C/C++ code at build-time.
If compiled with ORB_USE_PUBLISHER_RULES, a file with uORB publication rules can be used to configure which
modules are allowed to publish which topics. This is used for system-wide replay.
### Examples ### Examples
Monitor topic publication rates. Besides `top`, this is an important command for general system inspection: Monitor topic publication rates. Besides `top`, this is an important command for general system inspection:
$ uorb top $ uorb top