diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index 73483107a7..d0ffaff8c9 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -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 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 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 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' diff --git a/Makefile b/Makefile index 56c373663f..d5444678a7 100644 --- a/Makefile +++ b/Makefile @@ -122,13 +122,6 @@ endif 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 ?= # additional config parameters passed to cmake diff --git a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt index 1ba7ccde27..d58fede8cc 100644 --- a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt @@ -37,6 +37,12 @@ px4_add_romfs_files( px4-rc.mavlink px4-rc.params 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 ) diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index 8121f3914c..f1c2ee9e99 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -6,35 +6,36 @@ param set-default IMU_INTEG_RATE 250 if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then + echo "INFO [init] SIH simulator" - if [ -n "${PX4_HOME_LAT}" ]; then - param set SIH_LOC_LAT0 ${PX4_HOME_LAT} - fi + # if [ -n "${PX4_HOME_LAT}" ]; then + # param set SIH_LOC_LAT0 ${PX4_HOME_LAT} + # fi - if [ -n "${PX4_HOME_LON}" ]; then - param set SIH_LOC_LON0 ${PX4_HOME_LON} - fi + # if [ -n "${PX4_HOME_LON}" ]; then + # param set SIH_LOC_LON0 ${PX4_HOME_LON} + # fi - if simulator_sih start; then + # if simulator_sih start; then - if param compare -s SENS_EN_BAROSIM 1 - then - sensor_baro_sim start - fi - if param compare -s SENS_EN_GPSSIM 1 - then - sensor_gps_sim start - fi - if param compare -s SENS_EN_MAGSIM 1 - then - sensor_mag_sim start - fi + # if param compare -s SENS_EN_BAROSIM 1 + # then + # sensor_baro_sim start + # fi + # if param compare -s SENS_EN_GPSSIM 1 + # then + # sensor_gps_sim start + # fi + # if param compare -s SENS_EN_MAGSIM 1 + # then + # sensor_mag_sim start + # fi - else - echo "ERROR [init] simulator_sih failed to start" - exit 1 - fi + # else + # echo "ERROR [init] simulator_sih failed to start" + # exit 1 + # fi elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then # Use Gazebo @@ -190,7 +191,7 @@ else else 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 diff --git a/ROMFS/px4fmu_common/init.d-posix/rc.replay_ekf2 b/ROMFS/px4fmu_common/init.d-posix/rc.replay_ekf2 new file mode 100644 index 0000000000..23155b0dca --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/rc.replay_ekf2 @@ -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) diff --git a/ROMFS/px4fmu_common/init.d-posix/rc.replay b/ROMFS/px4fmu_common/init.d-posix/rc.replay_mc_hover_thrust_estimator similarity index 64% rename from ROMFS/px4fmu_common/init.d-posix/rc.replay rename to ROMFS/px4fmu_common/init.d-posix/rc.replay_mc_hover_thrust_estimator index ea0510280f..118a163cd3 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rc.replay +++ b/ROMFS/px4fmu_common/init.d-posix/rc.replay_mc_hover_thrust_estimator @@ -13,18 +13,14 @@ if [ ! -f replay_params.txt ]; then ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt fi -publisher_rules_file="orb_publisher.rules" -cat < "$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_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 -replay tryapplyparams -ekf2 start -r +#replay tryapplyparams +mc_hover_thrust_estimator start logger start -f -t -b 1000 -p vehicle_attitude replay start diff --git a/ROMFS/px4fmu_common/init.d-posix/rc.replay_mixing b/ROMFS/px4fmu_common/init.d-posix/rc.replay_mixing new file mode 100644 index 0000000000..91200b3e2a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/rc.replay_mixing @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/rc.replay_sensor_calibration b/ROMFS/px4fmu_common/init.d-posix/rc.replay_sensor_calibration new file mode 100644 index 0000000000..123efb5440 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/rc.replay_sensor_calibration @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/rc.replay_sensor_filtering b/ROMFS/px4fmu_common/init.d-posix/rc.replay_sensor_filtering new file mode 100644 index 0000000000..c46a2fb3a7 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/rc.replay_sensor_filtering @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 0084a6701e..16d3090bbf 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -17,11 +17,10 @@ PATH="$PATH:${R}etc/init.d-posix" # Main SITL startup script # -# check for ekf2 replay -# shellcheck disable=SC2154 -if [ "$replay_mode" = "ekf2" ] -then - . ${R}etc/init.d-posix/rc.replay +# check for replay +if [ ! -z ${replay_mode+x} ]; then + echo "Replay Mode: ${replay_mode}" + . ${R}etc/init.d-posix/rc.replay_${replay_mode} exit 0 fi @@ -330,12 +329,7 @@ fi [ -e "$autostart_file".post ] && . "$autostart_file".post # Run script to start logging -if param compare SYS_MC_EST_GROUP 2 -then - set LOGGER_ARGS "-p ekf2_timestamps" -else - set LOGGER_ARGS "-p vehicle_attitude" -fi +set LOGGER_ARGS "-p vehicle_attitude" . ${R}etc/init.d/rc.logging mavlink boot_complete diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index c53402bd0f..3b909297c2 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -74,7 +74,6 @@ set(msg_files DifferentialDriveSetpoint.msg DifferentialPressure.msg DistanceSensor.msg - Ekf2Timestamps.msg EscReport.msg EscStatus.msg EstimatorAidSource1d.msg diff --git a/msg/Ekf2Timestamps.msg b/msg/Ekf2Timestamps.msg deleted file mode 100644 index ae3ac06766..0000000000 --- a/msg/Ekf2Timestamps.msg +++ /dev/null @@ -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 diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index 3fa75a3454..6f330745fe 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -77,24 +77,6 @@ bool uORB::Manager::terminate() 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() { 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) { -#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 */ 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) { -#ifdef ORB_USE_PUBLISHER_RULES - - if (handle == _Instance) { - return PX4_OK; //pretend success - } - -#endif /* ORB_USE_PUBLISHER_RULES */ - 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) { -#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); } @@ -715,142 +657,3 @@ int16_t uORB::Manager::process_received_message(const char *messageName, int32_t } #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 */ diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp index d5202b9236..da06f81007 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -503,7 +503,7 @@ private: // data members DeviceMaster *_device_master{nullptr}; private: //class methods - Manager(); + Manager() = default; virtual ~Manager(); #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); #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: , , - * module: - * [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_ */ diff --git a/platforms/common/uORB/uORBMessageFields.cpp b/platforms/common/uORB/uORBMessageFields.cpp index 24ec09f438..b2316faa59 100644 --- a/platforms/common/uORB/uORBMessageFields.cpp +++ b/platforms/common/uORB/uORBMessageFields.cpp @@ -181,7 +181,7 @@ MessageFormatReader::State MessageFormatReader::readMore() } // Not expected to get here - PX4_ERR("logic error"); + PX4_ERR("logic error (not expected to get here)"); _state = State::Failure; return _state; } @@ -237,7 +237,7 @@ int MessageFormatReader::expandMessageFormat(char *format, unsigned len, unsigne } if (format_idx + 1 != (int)len) { - PX4_ERR("logic error"); + PX4_ERR("logic error (format_idx %d, len %d)", format_idx, len); return -1; } diff --git a/platforms/nuttx/init/stm32h7/rc.board_arch_defaults b/platforms/nuttx/init/stm32h7/rc.board_arch_defaults index 60979fa61b..5924eb9564 100644 --- a/platforms/nuttx/init/stm32h7/rc.board_arch_defaults +++ b/platforms/nuttx/init/stm32h7/rc.board_arch_defaults @@ -13,4 +13,4 @@ param set-default -s MC_AT_EN 1 param set-default -s UAVCAN_ENABLE 2 -set LOGGER_BUF 64 +set LOGGER_BUF 256 diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index c75093711e..dbfaa1ef64 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -49,10 +49,9 @@ static px4::atomic _objects[EKF2_MAX_INSTANCES] {}; static px4::atomic _ekf2_selector {nullptr}; #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), ScheduledWorkItem(MODULE_NAME, config), - _replay_mode(replay_mode && !multi_mode), _multi_mode(multi_mode), _instance(multi_mode ? -1 : 0), _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) { // advertise all topics to ensure consistent uORB instance numbering - _ekf2_timestamps_pub.advertise(); _estimator_event_flags_pub.advertise(); _estimator_innovation_test_ratios_pub.advertise(); _estimator_innovation_variances_pub.advertise(); @@ -703,42 +701,32 @@ void EKF2::Run() _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) - UpdateAirspeedSample(ekf2_timestamps); + UpdateAirspeedSample(); #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_AUXVEL) - UpdateAuxVelSample(ekf2_timestamps); + UpdateAuxVelSample(); #endif // CONFIG_EKF2_AUXVEL #if defined(CONFIG_EKF2_BAROMETER) - UpdateBaroSample(ekf2_timestamps); + UpdateBaroSample(); #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_EXTERNAL_VISION) - UpdateExtVisionSample(ekf2_timestamps); + UpdateExtVisionSample(); #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_OPTICAL_FLOW) - UpdateFlowSample(ekf2_timestamps); + UpdateFlowSample(); #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_GNSS) - UpdateGpsSample(ekf2_timestamps); + UpdateGpsSample(); #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) - UpdateMagSample(ekf2_timestamps); + UpdateMagSample(); #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_RANGE_FINDER) - UpdateRangeSample(ekf2_timestamps); + UpdateRangeSample(); #endif // CONFIG_EKF2_RANGE_FINDER - UpdateSystemFlagsSample(ekf2_timestamps); + + UpdateSystemFlagsSample(); // run the EKF update and output const hrt_abstime ekf_update_start = hrt_absolute_time(); @@ -793,9 +781,6 @@ void EKF2::Run() UpdateMagCalibration(now); #endif // CONFIG_EKF2_MAGNETOMETER } - - // publish ekf2_timestamps - _ekf2_timestamps_pub.publish(ekf2_timestamps); } // re-schedule as backup timeout @@ -921,13 +906,7 @@ void EKF2::PublishAttitude(const hrt_abstime ×tamp) _ekf.getQuaternion().copyTo(att.q); _ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter); - att.timestamp = _replay_mode ? 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{}; + att.timestamp = hrt_absolute_time(); _attitude_pub.publish(att); } } @@ -1004,7 +983,7 @@ void EKF2::PublishEvPosBias(const hrt_abstime ×tamp) if ((bias_vec - _last_ev_bias_published).longerThan(0.01f)) { 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); _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_var = status.innov_var; bias.innov_test_ratio = status.innov_test_ratio; - bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + bias.timestamp = hrt_absolute_time(); return bias; } @@ -1086,7 +1065,7 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp) 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.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + event_flags.timestamp = hrt_absolute_time(); _estimator_event_flags_pub.update(event_flags); _last_event_flags_publish = event_flags.timestamp; @@ -1096,7 +1075,7 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp) } else if ((_last_event_flags_publish != 0) && (timestamp >= _last_event_flags_publish + 1_s)) { // 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(); _last_event_flags_publish = _estimator_event_flags_pub.get().timestamp; } @@ -1153,7 +1132,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) global_pos.dead_reckoning = _ekf.control_status_flags().inertial_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); } } @@ -1187,7 +1166,7 @@ void EKF2::PublishGpsStatus(const hrt_abstime ×tamp) 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.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + estimator_gps_status.timestamp = hrt_absolute_time(); _estimator_gps_status_pub.publish(estimator_gps_status); @@ -1288,7 +1267,7 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) innovations.hagl_rate = _ekf.getHaglRateInnov(); #endif // CONFIG_EKF2_RANGE_FINDER - innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + innovations.timestamp = hrt_absolute_time(); _estimator_innovations_pub.publish(innovations); // calculate noise filtered velocity innovations which are used for pre-flight checking @@ -1422,7 +1401,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) test_ratios.hagl_rate = _ekf.getHaglRateInnovRatio(); #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); } @@ -1519,7 +1498,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) variances.hagl_rate = _ekf.getHaglRateInnovVar(); #endif // CONFIG_EKF2_RANGE_FINDER - variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + variances.timestamp = hrt_absolute_time(); _estimator_innovation_variances_pub.publish(variances); } @@ -1625,7 +1604,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) } // publish vehicle local position data - lpos.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + lpos.timestamp = hrt_absolute_time(); _local_position_pub.publish(lpos); } @@ -1667,7 +1646,7 @@ void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu_sa odom.quality = 0; // publish vehicle odometry data - odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + odom.timestamp = hrt_absolute_time(); _odometry_pub.publish(odom); } @@ -1733,7 +1712,7 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) #endif // CONFIG_EKF2_MAGNETOMETER - bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + bias.timestamp = hrt_absolute_time(); _estimator_sensor_bias_pub.publish(bias); _last_sensor_bias_published = bias.timestamp; @@ -1749,7 +1728,7 @@ void EKF2::PublishStates(const hrt_abstime ×tamp) state_vector.copyTo(states.states); states.n_states = state_vector.size(); _ekf.covariances_diagonal().copyTo(states.covariances); - states.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + states.timestamp = hrt_absolute_time(); _estimator_states_pub.publish(states); } @@ -1810,7 +1789,7 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) status.mag_strength_ref_gs); #endif // CONFIG_EKF2_MAGNETOMETER - status.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + status.timestamp = hrt_absolute_time(); _estimator_status_pub.publish(status); } @@ -1911,7 +1890,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) 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.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + status_flags.timestamp = hrt_absolute_time(); _estimator_status_flags_pub.publish(status_flags); _last_status_flags_publish = status_flags.timestamp; @@ -1933,7 +1912,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime ×tamp) yaw_est_test_data.yaw_composite_valid = _ekf.isYawEmergencyEstimateAvailable(); 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); } @@ -1964,7 +1943,7 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) wind.windspeed_east = wind_vel(1); wind.variance_north = wind_vel_var(0); wind.variance_east = wind_vel_var(1); - wind.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + wind.timestamp = hrt_absolute_time(); _wind_pub.publish(wind); } @@ -1992,7 +1971,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) _ekf.getFlowGyroBias().copyTo(flow_vel.gyro_bias); _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); @@ -2025,7 +2004,7 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt) #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_AIRSPEED) -void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) +void EKF2::UpdateAirspeedSample() { // EKF airspeed sample // 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; } - } 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 airspeed_s airspeed; @@ -2070,16 +2049,13 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) }; _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 #if defined(CONFIG_EKF2_AUXVEL) -void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps) +void EKF2::UpdateAuxVelSample() { // EKF auxiliary velocity sample // - 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 #if defined(CONFIG_EKF2_BAROMETER) -void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps) +void EKF2::UpdateBaroSample() { // EKF baro sample vehicle_air_data_s airdata; @@ -2132,15 +2108,12 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps) _ekf.set_air_density(airdata.rho); _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 #if defined(CONFIG_EKF2_EXTERNAL_VISION) -bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps) +bool EKF2::UpdateExtVisionSample() { // EKF external vision sample bool new_ev_odom = false; @@ -2279,9 +2252,6 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps) if (new_ev_odom) { _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; @@ -2289,7 +2259,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps) #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_OPTICAL_FLOW) -bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) +bool EKF2::UpdateFlowSample() { // EKF flow sample 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 - 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(optical_flow.quality) / static_cast(UINT8_MAX) * 100.f; @@ -2346,9 +2316,6 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) // set sensor limits _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; @@ -2356,7 +2323,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_GNSS) -void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) +void EKF2::UpdateGpsSample() { // EKF GPS message sensor_gps_s vehicle_gps_position; @@ -2401,7 +2368,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) -void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) +void EKF2::UpdateMagSample() { 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}); - - ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 - - (int64_t)ekf2_timestamps.timestamp / 100); } } #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_RANGE_FINDER) -void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) +void EKF2::UpdateRangeSample() { distance_sensor_s distance_sensor; 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()) { for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) { 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 - 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)) { 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 _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 _distance_sensor_selected = -1; } } #endif // CONFIG_EKF2_RANGE_FINDER -void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps) +void EKF2::UpdateSystemFlagsSample() { // EKF system flags if (_status_sub.updated() || _vehicle_land_detected_sub.updated()) { systemFlagUpdate flags{}; - flags.time_us = ekf2_timestamps.timestamp; + flags.time_us = 0; // vehicle_status vehicle_status_s 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) 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; 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.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[]) { 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) bool multi_mode = false; @@ -2731,7 +2687,7 @@ int EKF2::task_spawn(int argc, char *argv[]) #endif // CONFIG_EKF2_MAGNETOMETER } - if (multi_mode && !replay_mode) { + if (multi_mode) { // Start EKF2Selector if it's not already running if (_ekf2_selector.load() == nullptr) { EKF2Selector *inst = new EKF2Selector(); @@ -2756,7 +2712,7 @@ int EKF2::task_spawn(int argc, char *argv[]) while ((multi_instances_allocated < multi_instances) && (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.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 (!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)) { 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 - 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) { _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. -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"); PRINT_MODULE_USAGE_NAME("ekf2", "estimator"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAM_FLAG('r', "Enable replay mode", true); PRINT_MODULE_USAGE_COMMAND("stop"); PRINT_MODULE_USAGE_COMMAND_DESCR("status", "print status info"); #if defined(CONFIG_EKF2_VERBOSE_STATUS) diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 1d54315b8c..8e509c38b7 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -64,7 +64,6 @@ #include #include #include -#include #include #include #include @@ -127,7 +126,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem { public: 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; /** @see ModuleBase */ @@ -196,16 +195,16 @@ private: #endif // CONFIG_EKF2_WIND #if defined(CONFIG_EKF2_AIRSPEED) - void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps); + void UpdateAirspeedSample(); #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_AUXVEL) - void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps); + void UpdateAuxVelSample(); #endif // CONFIG_EKF2_AUXVEL #if defined(CONFIG_EKF2_BAROMETER) - void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps); + void UpdateBaroSample(); #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_EXTERNAL_VISION) - bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps); + bool UpdateExtVisionSample(); #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_GNSS) /* @@ -216,20 +215,20 @@ private: void PublishGpsStatus(const hrt_abstime ×tamp); void PublishGnssHgtBias(const hrt_abstime ×tamp); void PublishYawEstimatorStatus(const hrt_abstime ×tamp); - void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); + void UpdateGpsSample(); #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_OPTICAL_FLOW) - bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps); + bool UpdateFlowSample(); void PublishOpticalFlowVel(const hrt_abstime ×tamp); #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_MAGNETOMETER) - void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps); + void UpdateMagSample(); #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_RANGE_FINDER) - void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps); + void UpdateRangeSample(); #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 struct InFlightCalibration { @@ -265,7 +264,6 @@ private: 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; int _instance{0}; @@ -434,7 +432,6 @@ private: uint32_t _filter_warning_event_changes{0}; uint32_t _filter_information_event_changes{0}; - uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; uORB::PublicationMultiData _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; uORB::PublicationMulti _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index cd72d324a0..4d6711aa49 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -151,64 +151,67 @@ void LoggedTopics::add_default_topics() #if CONSTRAINED_MEMORY static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 1; #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_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_wind", 1000, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_attitude", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_global_position", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_local_position", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_wind", 0, MAX_ESTIMATOR_INSTANCES); #endif // always add the first instance - add_topic("estimator_baro_bias", 500); - add_topic("estimator_gnss_hgt_bias", 500); - add_topic("estimator_rng_hgt_bias", 500); - add_topic("estimator_ev_pos_bias", 500); - add_topic("estimator_event_flags", 0); - add_topic("estimator_gps_status", 1000); - add_topic("estimator_innovation_test_ratios", 500); - add_topic("estimator_innovation_variances", 500); - add_topic("estimator_innovations", 500); - add_topic("estimator_optical_flow_vel", 200); - add_topic("estimator_sensor_bias", 0); - add_topic("estimator_states", 1000); - add_topic("estimator_status", 200); - add_topic("estimator_status_flags", 0); - add_topic("yaw_estimator_status", 1000); + add_topic_multi("estimator_baro_bias", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_gnss_hgt_bias", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_rng_hgt_bias", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_ev_pos_bias", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_gps_status", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_innovation_test_ratios", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_innovation_variances", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_innovations", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_optical_flow_vel", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_states", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_status", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("yaw_estimator_status", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_gnss_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_rng_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_ev_pos_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_gps_status", 1000, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_innovations", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_baro_bias", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_gnss_hgt_bias", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_rng_hgt_bias", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_ev_pos_bias", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_gps_status", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_innovation_test_ratios", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_innovation_variances", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_innovations", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_optical_flow_vel", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_states", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_status", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_status_flags", 0, 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_optional_topic_multi("estimator_aid_src_baro_hgt", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_ev_pos", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_ev_vel", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_gravity", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_rng_hgt", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_fake_hgt", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_fake_pos", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_gnss_yaw", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_gnss_vel", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_gnss_pos", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_aid_src_airspeed", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_aid_src_baro_hgt", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_aid_src_ev_pos", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_aid_src_ev_vel", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_aid_src_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_aid_src_gravity", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_aid_src_rng_hgt", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_aid_src_fake_hgt", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_aid_src_fake_pos", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_aid_src_gnss_yaw", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_aid_src_gnss_vel", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_aid_src_gnss_pos", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_aid_src_optical_flow", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_aid_src_terrain_optical_flow", 0, 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); // 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() { - // for estimator replay (need to be at full rate) - add_topic("ekf2_timestamps"); - // current EKF2 subscriptions 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_selection"); add_topic("vehicle_air_data"); + add_topic("vehicle_command"); add_topic("vehicle_gps_position"); add_topic("vehicle_land_detected"); add_topic("vehicle_magnetometer"); + add_topic("vehicle_optical_flow"); add_topic("vehicle_status"); add_topic("vehicle_visual_odometry"); add_topic("aux_global_position"); add_topic_multi("distance_sensor"); + add_topic_multi("vehicle_imu"); } void LoggedTopics::add_thermal_calibration_topics() diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index f9c3f66800..778df6a386 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -690,7 +690,8 @@ void Logger::run() int next_subscribe_topic_index = -1; // this is used to distribute the checks over time 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; @@ -899,7 +900,7 @@ void Logger::run() // wait for next loop iteration... if (polling_topic_sub >= 0) { - px4_lockstep_progress(_lockstep_component); + //px4_lockstep_progress(_lockstep_component); px4_pollfd_struct_t fds[1]; 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::Mission); @@ -1472,8 +1473,8 @@ void Logger::start_log_mavlink() } // mavlink log does not work in combination with lockstep, it leads to dead-locks - px4_lockstep_unregister_component(_lockstep_component); - _lockstep_component = -1; + //px4_lockstep_unregister_component(_lockstep_component); + //_lockstep_component = -1; // initialize cpu load as early as possible to get more data initialize_load_output(PrintLoadReason::Preflight); diff --git a/src/modules/replay/CMakeLists.txt b/src/modules/replay/CMakeLists.txt index 1e83c882e2..e8a510abf6 100644 --- a/src/modules/replay/CMakeLists.txt +++ b/src/modules/replay/CMakeLists.txt @@ -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 # modification, are permitted provided that the following conditions @@ -34,11 +34,11 @@ px4_add_module( MODULE modules__replay MAIN replay COMPILE_FLAGS + -O3 + #-DDEBUG_BUILD SRCS definitions.hpp replay_main.cpp Replay.cpp Replay.hpp - ReplayEkf2.cpp - ReplayEkf2.hpp ) diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index b12e899cc0..3a38923eef 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -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 * modification, are permitted provided that the following conditions @@ -49,6 +49,10 @@ #include #include +#include +#include +#include + #include #include #include @@ -63,7 +67,6 @@ #include #include "Replay.hpp" -#include "ReplayEkf2.hpp" #define PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params.txt" #define DYNAMIC_PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params_dynamic.txt" @@ -76,16 +79,6 @@ namespace px4 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() { for (size_t i = 0; i < _subscriptions.size(); ++i) { @@ -95,29 +88,7 @@ Replay::~Replay() _subscriptions.clear(); } -void * -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) +void Replay::setupReplayFile(const char *file_name) { if (_replay_file) { free(_replay_file); @@ -126,8 +97,7 @@ Replay::setupReplayFile(const char *file_name) _replay_file = strdup(file_name); } -void -Replay::setParameter(const string ¶meter_name, const double parameter_value) +void Replay::setParameter(const string ¶meter_name, const double parameter_value) { param_t handle = param_find(parameter_name.c_str()); param_type_t param_format = param_type(handle); @@ -151,7 +121,7 @@ Replay::setParameter(const string ¶meter_name, const double parameter_value) float value = (float)parameter_value; 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); @@ -391,7 +361,6 @@ Replay::readFormat(std::ifstream &file, uint16_t msg_size) return true; } - string Replay::getOrbFields(const orb_metadata *meta) { char format[3000]; @@ -440,6 +409,53 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) uint8_t multi_id = *(uint8_t *)message; uint16_t msg_id = ((uint16_t)message[1]) | (((uint16_t)message[2]) << 8); 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); if (!orb_meta) { @@ -447,7 +463,6 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) return true; } - CompatBase *compat = nullptr; // check the format: the field definitions must match // 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); if (file_format != orb_fields) { - // check if we have a compatibility conversion available - 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;") { + PX4_ERR("Formats for %s (msg_id %d) don't match. Will ignore it.", topic_name.c_str(), msg_id); - int gyro_integral_dt_offset_log; - 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()); + if (false) { PX4_WARN(" Internal format:"); size_t start = 0; @@ -519,15 +509,14 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) start = i + 1; } } - - return true; // not a fatal error } + + return true; // not a fatal error } Subscription *subscription = new Subscription(); subscription->orb_meta = orb_meta; subscription->multi_id = multi_id; - subscription->compat = compat; //find the timestamp offset int field_size; @@ -561,7 +550,32 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) 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 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 subscription.orb_meta = nullptr; file.clear(); + + } else { + if (subscription.orb_meta) { + subscription.topic_name = (char *)subscription.orb_meta->o_name; + } + } return file.good(); @@ -885,6 +905,15 @@ Replay::readDefinitionsAndApplyParams(std::ifstream &file) void 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); if (!readDefinitionsAndApplyParams(replay_file)) { @@ -898,10 +927,6 @@ Replay::run() _speed_factor = atof(speedup); } - onEnterMainLoop(); - - _replay_start_time = hrt_absolute_time(); - PX4_INFO("Replay in progress..."); ulog_message_header_s message_header; @@ -910,19 +935,36 @@ Replay::run() //we know the next message must be an ADD_LOGGED_MSG 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)) { PX4_ERR("Failed to read subscription"); return; } - const uint64_t timestamp_offset = getTimestampOffset(); uint32_t nr_published_messages = 0; 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 - //to be in chronological order, so we need to check all subscriptions + hrt_abstime last_update = 0; + + 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; int next_msg_id = -1; bool first_time = true; @@ -944,18 +986,24 @@ Replay::run() } 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]; 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); 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); streampos next_additional_message_pos = sub.next_read_pos; readAndHandleAdditionalMessages(replay_file, next_additional_message_pos); @@ -972,14 +1020,164 @@ Replay::run() _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 - readTopicDataToBuffer(sub, replay_file); - memcpy(_read_buffer.data() + sub.timestamp_offset, &publish_timestamp, sizeof(uint64_t)); //adjust the timestamp + const auto param_change = _dynamic_parameter_schedule[_next_param_change]; + PX4_WARN("Performing param change scheduled for t=%.3lf at t=%.3lf.", + (double)param_change.timestamp / 1.e6, + (double)next_file_time / 1.e6); - if (handleTopicUpdate(sub, _read_buffer.data(), replay_file)) { - ++nr_published_messages; + setParameter(param_change.parameter_name, param_change.parameter_value); + _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 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); @@ -987,19 +1185,25 @@ Replay::run() // TODO: output status (eg. every sec), including total duration... } - for (auto &subscription : _subscriptions) { - if (!subscription) { + for (auto &sub : _subscriptions) { + if (!sub) { continue; } - if (subscription->compat) { - delete subscription->compat; - subscription->compat = nullptr; + if (sub && (sub->publication_counter > 0 || sub->error_counter > 0)) { + + 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) { - orb_unadvertise(subscription->orb_advert); - subscription->orb_advert = nullptr; + if (sub->orb_advert) { + orb_unadvertise(sub->orb_advert); + sub->orb_advert = nullptr; } } @@ -1008,8 +1212,6 @@ Replay::run() (double)hrt_elapsed_time(&_replay_start_time) / 1.e6); } - onExitMainLoop(); - if (!should_exit()) { replay_file.close(); 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); } -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 Replay::publishTopic(Subscription &sub, void *data) { bool published = false; - if (sub.compat) { - data = sub.compat->apply(data); - } - if (sub.orb_advert) { orb_publish(sub.orb_meta, sub.orb_advert, data); published = true; @@ -1120,6 +1283,123 @@ Replay::publishTopic(Subscription &sub, void *data) 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 Replay::custom_command(int argc, char *argv[]) { @@ -1131,6 +1411,8 @@ Replay::custom_command(int argc, char *argv[]) return Replay::task_spawn(argc, argv); } + // ignore? + return print_usage("unknown command"); } @@ -1197,17 +1479,9 @@ Replay * Replay::instantiate(int argc, char *argv[]) { // check the replay mode - const char *replay_mode = getenv(replay::ENV_MODE); + //const char *replay_mode = getenv(replay::ENV_MODE); - Replay *instance = nullptr; - - if (replay_mode && strcmp(replay_mode, "ekf2") == 0) { - PX4_INFO("Ekf2 replay mode"); - instance = new ReplayEkf2(); - - } else { - instance = new Replay(); - } + Replay *instance = new Replay(); return instance; } @@ -1224,7 +1498,7 @@ Replay::print_usage(const char *reason) ### Description 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`: - `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. diff --git a/src/modules/replay/Replay.hpp b/src/modules/replay/Replay.hpp index 75ee3c1095..f728c728f7 100644 --- a/src/modules/replay/Replay.hpp +++ b/src/modules/replay/Replay.hpp @@ -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 * modification, are permitted provided that the following conditions @@ -44,7 +44,6 @@ #include #include -#include namespace px4 { @@ -96,39 +95,10 @@ public: 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 { const orb_metadata *orb_meta = nullptr; ///< if nullptr, this subscription is invalid + char *topic_name = nullptr; orb_advert_t orb_advert = nullptr; uint8_t multi_id; int timestamp_offset; ///< marks the field of the timestamp @@ -138,8 +108,6 @@ protected: std::streampos next_read_pos; uint64_t next_timestamp; ///< timestamp of the file - CompatBase *compat = nullptr; - // statistics int error_counter = 0; int publication_counter = 0; @@ -163,34 +131,16 @@ protected: */ 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 */ - virtual 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); + void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) {} /** * handle the publication of a topic update * @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 @@ -207,7 +157,7 @@ protected: */ 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 //the current replay time @@ -300,6 +250,43 @@ private: std::string getOrbFields(const orb_metadata *meta); 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: , , + * module: + * [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 diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp deleted file mode 100644 index 1add36489d..0000000000 --- a/src/modules/replay/ReplayEkf2.cpp +++ /dev/null @@ -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 -#include -#include - -#include - -// for ekf2 replay -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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 diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp deleted file mode 100644 index bb7f8d1359..0000000000 --- a/src/modules/replay/ReplayEkf2.hpp +++ /dev/null @@ -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 diff --git a/src/modules/replay/definitions.hpp b/src/modules/replay/definitions.hpp index 100a5e6aad..de626dfc4a 100644 --- a/src/modules/replay/definitions.hpp +++ b/src/modules/replay/definitions.hpp @@ -38,7 +38,7 @@ namespace px4 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() diff --git a/src/modules/replay/replay_main.cpp b/src/modules/replay/replay_main.cpp index 9f18eb21d8..1c44a39d71 100644 --- a/src/modules/replay/replay_main.cpp +++ b/src/modules/replay/replay_main.cpp @@ -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 * modification, are permitted provided that the following conditions @@ -35,8 +35,7 @@ using namespace px4; -extern "C" __EXPORT int -replay_main(int argc, char *argv[]) +extern "C" __EXPORT int replay_main(int argc, char *argv[]) { //check for logfile env variable const char *logfile = getenv(replay::ENV_FILENAME); diff --git a/src/systemcmds/uorb/uorb.cpp b/src/systemcmds/uorb/uorb.cpp index b9fb565c14..3dd5fc76a8 100644 --- a/src/systemcmds/uorb/uorb.cpp +++ b/src/systemcmds/uorb/uorb.cpp @@ -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. -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 Monitor topic publication rates. Besides `top`, this is an important command for general system inspection: $ uorb top