From af8d1c9c6c6cc67bd77a4b792b4bdca20c0a25c4 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 17 Aug 2021 13:08:58 -0400 Subject: [PATCH] WIP: multi-EKF replay hacks and replay overhaul --- .ci/Jenkinsfile-hardware | 2 - Makefile | 7 - .../px4fmu_common/init.d-posix/CMakeLists.txt | 10 +- ...eplay => rc.replay_controller_mc_attitude} | 20 +- .../rc.replay_controller_mc_position | 30 ++ .../init.d-posix/rc.replay_controller_mc_rate | 28 ++ .../px4fmu_common/init.d-posix/rc.replay_ekf2 | 41 ++ .../rc.replay_mc_hover_thrust_estimator | 26 ++ .../init.d-posix/rc.replay_mixing | 29 ++ .../init.d-posix/rc.replay_sensor_calibration | 30 ++ ROMFS/px4fmu_common/init.d-posix/rcS | 16 +- boards/px4/sitl/ctrlalloc.cmake | 11 +- boards/px4/sitl/default.cmake | 13 +- boards/px4/sitl/replay.cmake | 3 - boards/px4/sitl/rtps.cmake | 13 +- boards/px4/sitl/test.cmake | 8 - msg/CMakeLists.txt | 2 - msg/ekf2_timestamps.msg | 23 -- msg/sensor_combined.msg | 20 - msg/sensor_selection.msg | 3 +- platforms/common/px4_work_queue/WorkQueue.cpp | 5 + platforms/common/uORB/uORBManager.cpp | 237 ----------- platforms/common/uORB/uORBManager.hpp | 77 ++-- platforms/posix/src/px4/common/drv_hrt.cpp | 26 +- .../MagnetometerBiasEstimator.cpp | 4 +- .../MagnetometerBiasEstimator.hpp | 6 +- .../terrain_estimation/terrain_estimator.cpp | 12 +- .../terrain_estimation/terrain_estimator.h | 16 +- .../attitude_estimator_q_main.cpp | 37 +- .../convert_px4_sensor_combined_csv_data.m | 8 +- .../EKF/matlab/EKF_replay/px4_replay_import.m | 21 +- src/modules/ekf2/EKF2.cpp | 295 ++++++-------- src/modules/ekf2/EKF2.hpp | 25 +- .../convertULogToSensorData.py | 16 +- .../BlockLocalPositionEstimator.cpp | 6 +- .../BlockLocalPositionEstimator.hpp | 6 +- src/modules/logger/logged_topics.cpp | 23 +- src/modules/logger/logger.cpp | 2 + src/modules/mavlink/streams/SCALED_IMU.hpp | 2 +- src/modules/mavlink/streams/SCALED_IMU2.hpp | 2 +- src/modules/mavlink/streams/SCALED_IMU3.hpp | 2 +- src/modules/replay/CMakeLists.txt | 2 - src/modules/replay/Replay.cpp | 370 +++++++++++------- src/modules/replay/Replay.hpp | 99 +++-- src/modules/replay/ReplayEkf2.cpp | 214 ---------- src/modules/replay/ReplayEkf2.hpp | 93 ----- src/modules/replay/replay_main.cpp | 5 +- src/modules/sensors/sensors.cpp | 18 +- src/modules/sensors/voted_sensors_update.cpp | 66 +--- src/modules/sensors/voted_sensors_update.h | 20 +- src/systemcmds/uorb/uorb.cpp | 3 - .../template_module/template_module.cpp | 14 +- 52 files changed, 773 insertions(+), 1294 deletions(-) rename ROMFS/px4fmu_common/init.d-posix/{rc.replay => rc.replay_controller_mc_attitude} (57%) create mode 100644 ROMFS/px4fmu_common/init.d-posix/rc.replay_controller_mc_position create mode 100644 ROMFS/px4fmu_common/init.d-posix/rc.replay_controller_mc_rate create mode 100644 ROMFS/px4fmu_common/init.d-posix/rc.replay_ekf2 create mode 100644 ROMFS/px4fmu_common/init.d-posix/rc.replay_mc_hover_thrust_estimator create mode 100644 ROMFS/px4fmu_common/init.d-posix/rc.replay_mixing create mode 100644 ROMFS/px4fmu_common/init.d-posix/rc.replay_sensor_calibration delete mode 100644 msg/ekf2_timestamps.msg delete mode 100644 msg/sensor_combined.msg delete mode 100644 src/modules/replay/ReplayEkf2.cpp delete mode 100644 src/modules/replay/ReplayEkf2.hpp diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index ed27c2b6b9..71bf670408 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -898,7 +898,6 @@ void printTopics() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener commander_state"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf2_timestamps"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf_gps_drift"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude"' @@ -936,7 +935,6 @@ void printTopics() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel_fifo"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_baro"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_combined"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fft"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fifo"' diff --git a/Makefile b/Makefile index c4e9f7cc83..bde0ff5ac3 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 - # additional config parameters passed to cmake ifdef EXTERNAL_MODULES_LOCATION CMAKE_ARGS += -DEXTERNAL_MODULES_LOCATION:STRING=$(EXTERNAL_MODULES_LOCATION) diff --git a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt index 419ed0ed18..5ceb17f788 100644 --- a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt @@ -38,6 +38,14 @@ px4_add_romfs_files( px4-rc.params px4-rc.rtps px4-rc.simulator - rc.replay + + rc.replay_controller_mc_attitude + rc.replay_controller_mc_position + rc.replay_controller_mc_rate + rc.replay_ekf2 + rc.replay_mc_hover_thrust_estimator + rc.replay_mixing + rc.replay_sensor_calibration + rcS ) diff --git a/ROMFS/px4fmu_common/init.d-posix/rc.replay b/ROMFS/px4fmu_common/init.d-posix/rc.replay_controller_mc_attitude similarity index 57% rename from ROMFS/px4fmu_common/init.d-posix/rc.replay rename to ROMFS/px4fmu_common/init.d-posix/rc.replay_controller_mc_attitude index 89c999a38c..ee4ffa6df4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rc.replay +++ b/ROMFS/px4fmu_common/init.d-posix/rc.replay_controller_mc_attitude @@ -10,20 +10,20 @@ fi if [ ! -f replay_params.txt ]; then echo "Creating $(pwd)/replay_params.txt" - ulog_params -i "${replay}" -d ' ' | grep -e '^EKF2' > replay_params.txt + 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 +# TODO: +# input: vehicle_attitude, vehicle_attitude_setpoint +# output: vehicle_rates_setpoint + # apply all params before ekf starts, as some params cannot be changed after startup -replay tryapplyparams -ekf2 start -r +#replay tryapplyparams + +mc_att_control start +mc_rate_control start + logger start -f -t -b 1000 -p vehicle_attitude replay start diff --git a/ROMFS/px4fmu_common/init.d-posix/rc.replay_controller_mc_position b/ROMFS/px4fmu_common/init.d-posix/rc.replay_controller_mc_position new file mode 100644 index 0000000000..3f47542032 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/rc.replay_controller_mc_position @@ -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: trajector_setpoint +# output: vehicle_local_position_setpoint vehicle_attitude_setpoint + +# apply all params before ekf starts, as some params cannot be changed after startup +#replay tryapplyparams + +mc_pos_control start +mc_att_control start +mc_rate_control start + +logger start -f -t -b 1000 -p vehicle_attitude +replay start diff --git a/ROMFS/px4fmu_common/init.d-posix/rc.replay_controller_mc_rate b/ROMFS/px4fmu_common/init.d-posix/rc.replay_controller_mc_rate new file mode 100644 index 0000000000..b8ba127422 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/rc.replay_controller_mc_rate @@ -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: vehicle_angular_velocity +# output: actuator_controls + +# apply all params before ekf starts, as some params cannot be changed after startup +#replay tryapplyparams + +mc_rate_control start + +logger start -f -t -b 1000 -p vehicle_attitude +replay start 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..fe8cce476b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/rc.replay_ekf2 @@ -0,0 +1,41 @@ +#!/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: vehicle_imu, vehicle_magnetometer, distance_sensor +# output: estimator_status + +# apply all params before ekf starts, as some params cannot be changed after startup +#replay tryapplyparams +ekf2 start +ekf2 status +logger start -f -t -b 1000 -p vehicle_attitude +replay start + + + +# 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_mc_hover_thrust_estimator b/ROMFS/px4fmu_common/init.d-posix/rc.replay_mc_hover_thrust_estimator new file mode 100644 index 0000000000..118a163cd3 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/rc.replay_mc_hover_thrust_estimator @@ -0,0 +1,26 @@ +#!/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: 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 +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/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index bce77bce8a..6e2677ea63 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -12,11 +12,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 @@ -265,12 +264,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/boards/px4/sitl/ctrlalloc.cmake b/boards/px4/sitl/ctrlalloc.cmake index 3e046829d8..4205296d24 100644 --- a/boards/px4/sitl/ctrlalloc.cmake +++ b/boards/px4/sitl/ctrlalloc.cmake @@ -107,13 +107,4 @@ set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb") # If the environment variable 'replay' is defined, we are building with replay # support. In this case, we enable the orb publisher rules. -set(REPLAY_FILE "$ENV{replay}") -if(REPLAY_FILE) - message(STATUS "Building with uorb publisher rules support") - add_definitions(-DORB_USE_PUBLISHER_RULES) - - message(STATUS "Building without lockstep for replay") - set(ENABLE_LOCKSTEP_SCHEDULER no) -else() - set(ENABLE_LOCKSTEP_SCHEDULER yes) -endif() +set(ENABLE_LOCKSTEP_SCHEDULER yes) diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index ee55255f3d..e1c6b93570 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -104,15 +104,4 @@ set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none") set(config_sitl_debugger disable CACHE STRING "debugger for sitl") set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb") -# If the environment variable 'replay' is defined, we are building with replay -# support. In this case, we enable the orb publisher rules. -set(REPLAY_FILE "$ENV{replay}") -if(REPLAY_FILE) - message(STATUS "Building with uorb publisher rules support") - add_definitions(-DORB_USE_PUBLISHER_RULES) - - message(STATUS "Building without lockstep for replay") - set(ENABLE_LOCKSTEP_SCHEDULER no) -else() - set(ENABLE_LOCKSTEP_SCHEDULER yes) -endif() +set(ENABLE_LOCKSTEP_SCHEDULER yes) diff --git a/boards/px4/sitl/replay.cmake b/boards/px4/sitl/replay.cmake index c437f80b4f..1f3018bfd9 100644 --- a/boards/px4/sitl/replay.cmake +++ b/boards/px4/sitl/replay.cmake @@ -15,7 +15,4 @@ px4_add_board( work_queue ) -message(STATUS "Building with uorb publisher rules support") -add_definitions(-DORB_USE_PUBLISHER_RULES) - set(ENABLE_LOCKSTEP_SCHEDULER yes) diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index 454bbc141f..fabddf1649 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -102,15 +102,4 @@ set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none") set(config_sitl_debugger disable CACHE STRING "debugger for sitl") set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb") -# If the environment variable 'replay' is defined, we are building with replay -# support. In this case, we enable the orb publisher rules. -set(REPLAY_FILE "$ENV{replay}") -if(REPLAY_FILE) - message(STATUS "Building with uorb publisher rules support") - add_definitions(-DORB_USE_PUBLISHER_RULES) - - message(STATUS "Building without lockstep for replay") - set(ENABLE_LOCKSTEP_SCHEDULER no) -else() - set(ENABLE_LOCKSTEP_SCHEDULER yes) -endif() +set(ENABLE_LOCKSTEP_SCHEDULER yes) diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index cab5707d25..c03ae67e52 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -102,13 +102,5 @@ set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none") set(config_sitl_debugger disable CACHE STRING "debugger for sitl") set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb") -# If the environment variable 'replay' is defined, we are building with replay -# support. In this case, we enable the orb publisher rules. -set(REPLAY_FILE "$ENV{replay}") -if(REPLAY_FILE) - message(STATUS "Building with uorb publisher rules support") - add_definitions(-DORB_USE_PUBLISHER_RULES) -endif() - message(STATUS "Building without lockstep for test") set(ENABLE_LOCKSTEP_SCHEDULER no) diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 6904bfce54..69be9123d4 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -56,7 +56,6 @@ set(msg_files cpuload.msg differential_pressure.msg distance_sensor.msg - ekf2_timestamps.msg ekf_gps_drift.msg esc_report.msg esc_status.msg @@ -130,7 +129,6 @@ set(msg_files sensor_accel.msg sensor_accel_fifo.msg sensor_baro.msg - sensor_combined.msg sensor_correction.msg sensor_gps.msg sensor_gyro.msg diff --git a/msg/ekf2_timestamps.msg b/msg/ekf2_timestamps.msg deleted file mode 100644 index ae3ac06766..0000000000 --- a/msg/ekf2_timestamps.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/msg/sensor_combined.msg b/msg/sensor_combined.msg deleted file mode 100644 index 42440213cf..0000000000 --- a/msg/sensor_combined.msg +++ /dev/null @@ -1,20 +0,0 @@ -# Sensor readings in SI-unit form. -# These fields are scaled and offset-compensated where possible and do not -# change with board revisions and sensor updates. - -uint64 timestamp # time since system start (microseconds) - -int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid - -# gyro timstamp is equal to the timestamp of the message -float32[3] gyro_rad # average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period -uint32 gyro_integral_dt # gyro measurement sampling period in microseconds - -int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp -float32[3] accelerometer_m_s2 # average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period -uint32 accelerometer_integral_dt # accelerometer measurement sampling period in microseconds - -uint8 CLIPPING_X = 1 -uint8 CLIPPING_Y = 2 -uint8 CLIPPING_Z = 4 -uint8 accelerometer_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the sampling period diff --git a/msg/sensor_selection.msg b/msg/sensor_selection.msg index 799ccf18a3..069fcbc8de 100644 --- a/msg/sensor_selection.msg +++ b/msg/sensor_selection.msg @@ -1,7 +1,8 @@ # -# Sensor ID's for the voted sensors output on the sensor_combined topic. +# Sensor ID's for the primary IMU. # Will be updated on startup of the sensor module and when sensor selection changes # uint64 timestamp # time since system start (microseconds) + uint32 accel_device_id # unique device ID for the selected accelerometers uint32 gyro_device_id # unique device ID for the selected rate gyros diff --git a/platforms/common/px4_work_queue/WorkQueue.cpp b/platforms/common/px4_work_queue/WorkQueue.cpp index 48b88c7290..e112673959 100644 --- a/platforms/common/px4_work_queue/WorkQueue.cpp +++ b/platforms/common/px4_work_queue/WorkQueue.cpp @@ -111,6 +111,7 @@ void WorkQueue::Add(WorkItem *item) if (_lockstep_component == -1) { _lockstep_component = px4_lockstep_register_component(); + //fprintf(stderr, "WQ %s px4_lockstep_register_component:%d\n", item->ItemName(), _lockstep_component); } #endif // ENABLE_LOCKSTEP_SCHEDULER @@ -170,6 +171,10 @@ void WorkQueue::Run() #if defined(ENABLE_LOCKSTEP_SCHEDULER) if (_q.empty()) { + if (_lockstep_component > 0) { + //fprintf(stderr, "WQ px4_lockstep_unregister_component:%d\n", _lockstep_component); + } + px4_lockstep_unregister_component(_lockstep_component); _lockstep_component = -1; } diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index 0493c68fd8..81ed3946d1 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -66,29 +66,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; -} - uORB::DeviceMaster *uORB::Manager::get_device_master() { if (!_device_master) { @@ -170,30 +147,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, unsigned int queue_size) { -#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); @@ -242,14 +195,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); } @@ -264,54 +209,11 @@ int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned return node_open(meta, false, &inst); } -int uORB::Manager::orb_unsubscribe(int fd) -{ - return px4_close(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); } -int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer) -{ - int ret; - - ret = px4_read(handle, buffer, meta->o_size); - - if (ret < 0) { - return PX4_ERROR; - } - - if (ret != (int)meta->o_size) { - errno = EIO; - return PX4_ERROR; - } - - return PX4_OK; -} - -int uORB::Manager::orb_check(int handle, bool *updated) -{ - /* Set to false here so that if `px4_ioctl` fails to false. */ - *updated = false; - return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); -} - -int uORB::Manager::orb_set_interval(int handle, unsigned interval) -{ - return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); -} - int uORB::Manager::orb_get_interval(int handle, unsigned *interval) { int ret = px4_ioctl(handle, ORBIOCGETINTERVAL, (unsigned long)interval); @@ -508,142 +410,3 @@ bool uORB::Manager::is_remote_subscriber_present(const char *messageName) #endif } #endif /* 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 b6e0ae784f..57af3a456f 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -256,7 +256,7 @@ public: * @param handle A handle returned from orb_subscribe. * @return OK on success, PX4_ERROR otherwise with errno set accordingly. */ - int orb_unsubscribe(int handle); + int orb_unsubscribe(int handle) { return px4_close(handle); } /** * Fetch data from a topic. @@ -274,7 +274,21 @@ public: * using the data. * @return OK on success, PX4_ERROR otherwise with errno set accordingly. */ - int orb_copy(const struct orb_metadata *meta, int handle, void *buffer); + int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) + { + int ret = px4_read(handle, buffer, meta->o_size); + + if (ret < 0) { + return PX4_ERROR; + } + + if (ret != (int)meta->o_size) { + errno = EIO; + return PX4_ERROR; + } + + return PX4_OK; + } /** * Check whether a topic has been published to since the last orb_copy. @@ -292,7 +306,12 @@ public: * @return OK if the check was successful, PX4_ERROR otherwise with * errno set accordingly. */ - int orb_check(int handle, bool *updated); + int orb_check(int handle, bool *updated) + { + /* Set to false here so that if `px4_ioctl` fails to false. */ + *updated = false; + return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); + } /** * Check if a topic has already been created and published (advertised) @@ -321,8 +340,7 @@ public: * @param interval An interval period in milliseconds. * @return OK on success, PX4_ERROR otherwise with ERRNO set accordingly. */ - int orb_set_interval(int handle, unsigned interval); - + int orb_set_interval(int handle, unsigned interval) { return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); } /** * Get the minimum interval between which updates are seen for a subscription. @@ -381,9 +399,12 @@ private: // data members DeviceMaster *_device_master{nullptr}; -private: //class methods - Manager(); - virtual ~Manager(); +private: // class methods + Manager() = default; + virtual ~Manager() + { + delete _device_master; + } #ifdef ORB_COMMUNICATOR /** @@ -442,46 +463,6 @@ private: //class methods */ virtual int16_t process_received_message(const char *messageName, int32_t length, uint8_t *data); #endif /* 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/posix/src/px4/common/drv_hrt.cpp b/platforms/posix/src/px4/common/drv_hrt.cpp index fa93293389..b69a2b6fdb 100644 --- a/platforms/posix/src/px4/common/drv_hrt.cpp +++ b/platforms/posix/src/px4/common/drv_hrt.cpp @@ -77,7 +77,7 @@ __EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; static px4_sem_t _hrt_lock; static struct work_s _hrt_work; -static hrt_abstime px4_timestart_monotonic = 0; +static hrt_abstime px4_timestart_monotonic = UINT64_MAX; #if defined(ENABLE_LOCKSTEP_SCHEDULER) static LockstepScheduler *lockstep_scheduler = new LockstepScheduler(); @@ -90,7 +90,7 @@ static void hrt_call_invoke(); hrt_abstime hrt_absolute_time_offset() { - return px4_timestart_monotonic; + return (px4_timestart_monotonic == UINT64_MAX) ? 0 : px4_timestart_monotonic; } static void hrt_lock() @@ -136,7 +136,9 @@ hrt_abstime hrt_absolute_time() #if defined(ENABLE_LOCKSTEP_SCHEDULER) // optimized case (avoid ts_to_abstime) if lockstep scheduler is used const uint64_t abstime = lockstep_scheduler->get_absolute_time(); - return abstime - px4_timestart_monotonic; + uint64_t timestart_monotonic = (px4_timestart_monotonic == UINT64_MAX) ? 0 : px4_timestart_monotonic; + return abstime - timestart_monotonic; + #else // defined(ENABLE_LOCKSTEP_SCHEDULER) struct timespec ts; px4_clock_gettime(CLOCK_MONOTONIC, &ts); @@ -149,9 +151,7 @@ hrt_abstime hrt_absolute_time() */ hrt_abstime ts_to_abstime(const struct timespec *ts) { - hrt_abstime result; - - result = (hrt_abstime)(ts->tv_sec) * 1000000; + hrt_abstime result = (hrt_abstime)(ts->tv_sec) * 1000000; result += ts->tv_nsec / 1000; return result; @@ -510,7 +510,8 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) if (clk_id == CLOCK_MONOTONIC) { #if defined(ENABLE_LOCKSTEP_SCHEDULER) const uint64_t abstime = lockstep_scheduler->get_absolute_time(); - abstime_to_ts(tp, abstime - px4_timestart_monotonic); + uint64_t timestart_monotonic = (px4_timestart_monotonic == UINT64_MAX) ? 0 : px4_timestart_monotonic; + abstime_to_ts(tp, abstime - timestart_monotonic); return 0; #else // defined(ENABLE_LOCKSTEP_SCHEDULER) #if defined(__PX4_DARWIN) @@ -536,7 +537,7 @@ int px4_clock_settime(clockid_t clk_id, const struct timespec *ts) } else { const uint64_t time_us = ts_to_abstime(ts); - if (px4_timestart_monotonic == 0) { + if (px4_timestart_monotonic == UINT64_MAX) { px4_timestart_monotonic = time_us; } @@ -545,10 +546,9 @@ int px4_clock_settime(clockid_t clk_id, const struct timespec *ts) } } - int px4_usleep(useconds_t usec) { - if (px4_timestart_monotonic == 0) { + if (px4_timestart_monotonic == UINT64_MAX) { // Until the time is set by the simulator, we fallback to the normal // usleep; return system_usleep(usec); @@ -561,7 +561,7 @@ int px4_usleep(useconds_t usec) unsigned int px4_sleep(unsigned int seconds) { - if (px4_timestart_monotonic == 0) { + if (px4_timestart_monotonic == UINT64_MAX) { // Until the time is set by the simulator, we fallback to the normal // sleep; return system_sleep(seconds); @@ -577,8 +577,10 @@ int px4_pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, const struct timespec *ts) { + uint64_t timestart_monotonic = (px4_timestart_monotonic == UINT64_MAX) ? 0 : px4_timestart_monotonic; + const uint64_t time_us = ts_to_abstime(ts); - const uint64_t scheduled = time_us + px4_timestart_monotonic; + const uint64_t scheduled = time_us + timestart_monotonic; return lockstep_scheduler->cond_timedwait(cond, mutex, scheduled); } diff --git a/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.cpp b/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.cpp index c0754ddc34..960c9bb487 100644 --- a/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.cpp +++ b/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.cpp @@ -49,10 +49,10 @@ MagnetometerBiasEstimator::MagnetometerBiasEstimator(const matrix::Dcmf &board_r _board_rotation(board_rotation) {} -void MagnetometerBiasEstimator::extractBias(vehicle_magnetometer_s &mag_raw, const sensor_combined_s &gyro_raw) +void MagnetometerBiasEstimator::extractBias(vehicle_magnetometer_s &mag_raw, const vehicle_angular_velocity_s &gyro) { // fill in vectors - Vector3f gyro(gyro_raw.gyro_rad); + Vector3f gyro(gyro.xyz); Vector3f mag(mag_raw.magnetometer_ga); // prepare delta time in seconds diff --git a/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.hpp b/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.hpp index 4fb0867463..e2ec48a98e 100644 --- a/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.hpp +++ b/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.hpp @@ -47,9 +47,9 @@ #include #include -#include -#include #include +#include +#include #include @@ -64,7 +64,7 @@ public: * @param mag_raw struct containing the magnetometer data to operate on (gets adjusted with current estimate) * @param raw struct containing the gyroscope data to use */ - void extractBias(vehicle_magnetometer_s &mag_raw, const sensor_combined_s &gyro_raw); + void extractBias(vehicle_magnetometer_s &mag_raw, const vehicle_angular_velocity_s &gyro_raw); private: void updateEstimate(const matrix::Vector3f &gyro, const matrix::Vector3f &mag, const float dt); diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index 2ec7b87652..84d72e6375 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -58,12 +58,11 @@ bool TerrainEstimator::is_distance_valid(float distance) return (distance < 40.0f && distance > 0.00001f); } -void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude, - const struct sensor_combined_s *sensor, - const struct distance_sensor_s *distance) +void TerrainEstimator::predict(float dt, const vehicle_attitude_s *attitude, const vehicle_imu_s *imu, + const distance_sensor_s *distance) { matrix::Dcmf R_att = matrix::Quatf(attitude->q); - matrix::Vector3f a{sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2]}; + matrix::Vector3f a{Vector3f{imu->delta_velocity} * 1.e6f / (float)imu->delta_velocity_dt}; matrix::Vector u; u = R_att * a; _u_z = u(2) + CONSTANTS_ONE_G; // compensate for gravity @@ -97,9 +96,8 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu B * R * B.transpose() + Q) * dt; } -void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, - const struct distance_sensor_s *distance, - const struct vehicle_attitude_s *attitude) +void TerrainEstimator::measurement_update(uint64_t time_ref, const vehicle_gps_position_s *gps, + const distance_sensor_s *distance, const vehicle_attitude_s *attitude) { // terrain estimate is invalid if we have range sensor timeout if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) { diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index ab5db616fa..0752537e92 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -39,11 +39,10 @@ #include #include -#include -#include -#include #include - +#include +#include +#include /* * This class can be used to estimate distance to the ground using a laser range finder. @@ -66,11 +65,10 @@ public: float get_distance_to_ground() {return -_x(0);} float get_velocity() {return _x(1);} - void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, - const struct distance_sensor_s *distance); - void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, - const struct distance_sensor_s *distance, - const struct vehicle_attitude_s *attitude); + void predict(float dt, const vehicle_attitude_s *attitude, const vehicle_imu_s *sensor, + const distance_sensor_s *distance); + void measurement_update(uint64_t time_ref, const vehicle_gps_position_s *gps, const distance_sensor_s *distance, + const vehicle_attitude_s *attitude); private: enum {n_x = 3}; diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index a11af425ed..e9d7954540 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -55,9 +55,9 @@ #include #include #include -#include #include #include +#include #include #include #include @@ -106,7 +106,7 @@ private: const float _dt_min = 0.00001f; const float _dt_max = 0.02f; - uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)}; + uORB::SubscriptionCallbackWorkItem _imu_sub{this, ORB_ID(vehicle_imu)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)}; @@ -180,7 +180,7 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : bool AttitudeEstimatorQ::init() { - if (!_sensors_sub.registerCallback()) { + if (!_imu_sub.registerCallback()) { PX4_ERR("sensor combined callback registration failed!"); return false; } @@ -192,33 +192,26 @@ void AttitudeEstimatorQ::Run() { if (should_exit()) { - _sensors_sub.unregisterCallback(); + _imu_sub.unregisterCallback(); exit_and_cleanup(); return; } - sensor_combined_s sensors; + vehicle_imu_s imu; - if (_sensors_sub.update(&sensors)) { + if (_imu_sub.update(&imu)) { update_parameters(); - // Feed validator with recent sensor data - if (sensors.timestamp > 0) { - _gyro(0) = sensors.gyro_rad[0]; - _gyro(1) = sensors.gyro_rad[1]; - _gyro(2) = sensors.gyro_rad[2]; - } + const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt; + _accel = Vector3f{imu.delta_velocity} * accel_dt_inv; - if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { - _accel(0) = sensors.accelerometer_m_s2[0]; - _accel(1) = sensors.accelerometer_m_s2[1]; - _accel(2) = sensors.accelerometer_m_s2[2]; + const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt; + _gyro = Vector3f{imu.delta_angle} * gyro_dt_inv; - if (_accel.length() < 0.01f) { - PX4_ERR("degenerate accel!"); - return; - } + if (_accel.length() < 0.01f) { + PX4_ERR("degenerate accel!"); + return; } // Update magnetometer @@ -348,8 +341,8 @@ AttitudeEstimatorQ::Run() _last_time = now; if (update(dt)) { - vehicle_attitude_s att = {}; - att.timestamp_sample = sensors.timestamp; + vehicle_attitude_s att{}; + att.timestamp_sample = imu.timestamp; _q.copyTo(att.q); /* the instance count is not used here */ diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m index 6b5d1b6879..32ef6f3bcb 100755 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m @@ -19,11 +19,11 @@ end clear imu_data; n_samples = length(timestamp); imu_data.time_us = timestamp + accelerometer_timestamp_relative; -imu_data.gyro_dt = gyro_integral_dt ./ 1e6; -imu_data.del_ang = [gyro_rad0.*imu_data.gyro_dt, gyro_rad1.*imu_data.gyro_dt, gyro_rad2.*imu_data.gyro_dt]; +imu_data.gyro_dt = delta_angle_dt ./ 1e6; +imu_data.del_ang = [delta_angle0, delta_angle1, delta_angle2]; -imu_data.accel_dt = accelerometer_integral_dt ./ 1e6; -imu_data.del_vel = [accelerometer_m_s20.*imu_data.accel_dt, accelerometer_m_s21.*imu_data.accel_dt, accelerometer_m_s22.*imu_data.accel_dt]; +imu_data.accel_dt = delta_velocity_dt ./ 1e6; +imu_data.del_vel = [delta_velocity0, delta_velocity1, delta_velocity2]; %% convert magnetometer data clear mag_data; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/px4_replay_import.m b/src/modules/ekf2/EKF/matlab/EKF_replay/px4_replay_import.m index 6d6644f7cd..8df0403ca2 100644 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/px4_replay_import.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/px4_replay_import.m @@ -38,7 +38,7 @@ opts.DataLines = [2, Inf]; opts.Delimiter = ","; % Specify column names and types -opts.VariableNames = ["timestamp", "gyro_rad0", "gyro_rad1", "gyro_rad2", "gyro_integral_dt", "accelerometer_timestamp_relative", "accelerometer_m_s20", "accelerometer_m_s21", "accelerometer_m_s22", "accelerometer_integral_dt"]; +opts.VariableNames = ["timestamp", "delta_angle0", "delta_angle1", "delta_angle2", "delta_angle_dt", "delta_velocity0", "delta_velocity1", "delta_velocity2", "delta_velocity_dt"]; opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double"]; opts.ExtraColumnsRule = "ignore"; opts.EmptyLineRule = "read"; @@ -48,15 +48,14 @@ tbl = readtable(sensors_file, opts); % Convert to output type timestamp = tbl.timestamp; -gyro_rad0 = tbl.gyro_rad0; -gyro_rad1 = tbl.gyro_rad1; -gyro_rad2 = tbl.gyro_rad2; -gyro_integral_dt = tbl.gyro_integral_dt; -accelerometer_timestamp_relative = tbl.accelerometer_timestamp_relative; -accelerometer_m_s20 = tbl.accelerometer_m_s20; -accelerometer_m_s21 = tbl.accelerometer_m_s21; -accelerometer_m_s22 = tbl.accelerometer_m_s22; -accelerometer_integral_dt = tbl.accelerometer_integral_dt; +delta_angle0 = tbl.delta_angle0; +delta_angle1 = tbl.delta_angle1; +delta_angle2 = tbl.delta_angle2; +gyro_integral_dt = tbl.delta_angle_dt; +delta_velocity0 = tbl.delta_velocity0; +delta_velocity1 = tbl.delta_velocity1; +delta_velocity2 = tbl.delta_velocity2; +accelerometer_integral_dt = tbl.delta_velocity_dt; clear opts tbl @@ -166,7 +165,7 @@ cd ../; %% ------ SECTION 3: Ground Truth Data (STIL only, optional) ------ if exist('attitude_file','var') && exist('localpos_file','var') && exist('globalpos_file','var') - + %- Import Attitude data from text file opts = delimitedTextImportOptions("NumVariables", 13); opts.DataLines = [2, Inf]; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 398266df86..77356476ee 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -45,10 +45,9 @@ static px4::atomic _objects[EKF2_MAX_INSTANCES] {}; static px4::atomic _ekf2_selector {nullptr}; #endif // !CONSTRAINED_FLASH -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)), @@ -190,7 +189,6 @@ bool EKF2::multi_init(int imu, int mag) _odometry_pub.advertise(); _wind_pub.advertise(); - _ekf2_timestamps_pub.advertise(); _ekf_gps_drift_pub.advertise(); _estimator_baro_bias_pub.advertise(); _estimator_innovation_test_ratios_pub.advertise(); @@ -247,9 +245,7 @@ int EKF2::print_status() void EKF2::Run() { if (should_exit()) { - _sensor_combined_sub.unregisterCallback(); _vehicle_imu_sub.unregisterCallback(); - return; } @@ -273,12 +269,7 @@ void EKF2::Run() } if (!_callback_registered) { - if (_multi_mode) { - _callback_registered = _vehicle_imu_sub.registerCallback(); - - } else { - _callback_registered = _sensor_combined_sub.registerCallback(); - } + _callback_registered = _vehicle_imu_sub.registerCallback(); if (!_callback_registered) { PX4_WARN("%d - failed to register callback, retrying", _instance); @@ -309,107 +300,80 @@ void EKF2::Run() } } - bool imu_updated = false; - imuSample imu_sample_new {}; - - hrt_abstime imu_dt = 0; // for tracking time slip later - - if (_multi_mode) { - const unsigned last_generation = _vehicle_imu_sub.get_last_generation(); - vehicle_imu_s imu; - imu_updated = _vehicle_imu_sub.update(&imu); - - if (imu_updated && (_vehicle_imu_sub.get_last_generation() != last_generation + 1)) { - perf_count(_msg_missed_imu_perf); - } - - imu_sample_new.time_us = imu.timestamp_sample; - imu_sample_new.delta_ang_dt = imu.delta_angle_dt * 1.e-6f; - imu_sample_new.delta_ang = Vector3f{imu.delta_angle}; - imu_sample_new.delta_vel_dt = imu.delta_velocity_dt * 1.e-6f; - imu_sample_new.delta_vel = Vector3f{imu.delta_velocity}; - - if (imu.delta_velocity_clipping > 0) { - imu_sample_new.delta_vel_clipping[0] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_X; - imu_sample_new.delta_vel_clipping[1] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Y; - imu_sample_new.delta_vel_clipping[2] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Z; - } - - imu_dt = imu.delta_angle_dt; - - if ((_device_id_accel == 0) || (_device_id_gyro == 0)) { - _device_id_accel = imu.accel_device_id; - _device_id_gyro = imu.gyro_device_id; - _imu_calibration_count = imu.calibration_count; - - } else if ((imu.calibration_count > _imu_calibration_count) - || (imu.accel_device_id != _device_id_accel) - || (imu.gyro_device_id != _device_id_gyro)) { - - PX4_DEBUG("%d - resetting IMU bias", _instance); - _device_id_accel = imu.accel_device_id; - _device_id_gyro = imu.gyro_device_id; - - _ekf.resetImuBias(); - _imu_calibration_count = imu.calibration_count; - - SelectImuStatus(); - } - - } else { - const unsigned last_generation = _vehicle_imu_sub.get_last_generation(); - sensor_combined_s sensor_combined; - imu_updated = _sensor_combined_sub.update(&sensor_combined); - - if (imu_updated && (_sensor_combined_sub.get_last_generation() != last_generation + 1)) { - perf_count(_msg_missed_imu_perf); - } - - imu_sample_new.time_us = sensor_combined.timestamp; - imu_sample_new.delta_ang_dt = sensor_combined.gyro_integral_dt * 1.e-6f; - imu_sample_new.delta_ang = Vector3f{sensor_combined.gyro_rad} * imu_sample_new.delta_ang_dt; - imu_sample_new.delta_vel_dt = sensor_combined.accelerometer_integral_dt * 1.e-6f; - imu_sample_new.delta_vel = Vector3f{sensor_combined.accelerometer_m_s2} * imu_sample_new.delta_vel_dt; - - if (sensor_combined.accelerometer_clipping > 0) { - imu_sample_new.delta_vel_clipping[0] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_X; - imu_sample_new.delta_vel_clipping[1] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Y; - imu_sample_new.delta_vel_clipping[2] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Z; - } - - imu_dt = sensor_combined.gyro_integral_dt; - + if (!_multi_mode) { if (_sensor_selection_sub.updated() || (_device_id_accel == 0 || _device_id_gyro == 0)) { + bool imu_changed = false; sensor_selection_s sensor_selection; if (_sensor_selection_sub.copy(&sensor_selection)) { if (_device_id_accel != sensor_selection.accel_device_id) { _ekf.resetAccelBias(); _device_id_accel = sensor_selection.accel_device_id; - SelectImuStatus(); + imu_changed = true; } if (_device_id_gyro != sensor_selection.gyro_device_id) { _ekf.resetGyroBias(); _device_id_gyro = sensor_selection.gyro_device_id; - SelectImuStatus(); + imu_changed = true; + } + + if (imu_changed) { + SelectImu(); } } } } - if (imu_updated) { - const hrt_abstime now = imu_sample_new.time_us; + const unsigned last_generation = _vehicle_imu_sub.get_last_generation(); + vehicle_imu_s vehicle_imu; + + if (_vehicle_imu_sub.update(&vehicle_imu)) { + + const hrt_abstime now = vehicle_imu.timestamp_sample; + + if (_vehicle_imu_sub.get_last_generation() != last_generation + 1) { + perf_count(_msg_missed_imu_perf); + } + + if ((_device_id_accel == 0) || (_device_id_gyro == 0)) { + _device_id_accel = vehicle_imu.accel_device_id; + _device_id_gyro = vehicle_imu.gyro_device_id; + _imu_calibration_count = vehicle_imu.calibration_count; + + } else if ((vehicle_imu.calibration_count > _imu_calibration_count) + || (vehicle_imu.accel_device_id != _device_id_accel) + || (vehicle_imu.gyro_device_id != _device_id_gyro)) { + + PX4_DEBUG("%d - resetting IMU bias", _instance); + _device_id_accel = vehicle_imu.accel_device_id; + _device_id_gyro = vehicle_imu.gyro_device_id; + + _ekf.resetImuBias(); + _imu_calibration_count = vehicle_imu.calibration_count; + + SelectImu(); + } UpdateImuStatus(); // push imu data into estimator + imuSample imu_sample_new; + imu_sample_new.time_us = vehicle_imu.timestamp_sample; + imu_sample_new.delta_ang_dt = vehicle_imu.delta_angle_dt * 1.e-6f; + imu_sample_new.delta_ang = Vector3f{vehicle_imu.delta_angle}; + imu_sample_new.delta_vel_dt = vehicle_imu.delta_velocity_dt * 1.e-6f; + imu_sample_new.delta_vel = Vector3f{vehicle_imu.delta_velocity}; + imu_sample_new.delta_vel_clipping[0] = vehicle_imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_X; + imu_sample_new.delta_vel_clipping[0] = vehicle_imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Y; + imu_sample_new.delta_vel_clipping[0] = vehicle_imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Z; + _ekf.setIMUData(imu_sample_new); PublishAttitude(now); // publish attitude immediately (uses quaternion from output predictor) // integrate time to monitor time slippage if (_start_time_us > 0) { - _integrated_time_us += imu_dt; + _integrated_time_us += vehicle_imu.delta_angle_dt; _last_time_slip_us = (imu_sample_new.time_us - _start_time_us) - _integrated_time_us; } else { @@ -465,29 +429,18 @@ void EKF2::Run() } } - // 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, - }; - - UpdateAirspeedSample(ekf2_timestamps); - UpdateAuxVelSample(ekf2_timestamps); - UpdateBaroSample(ekf2_timestamps); - UpdateGpsSample(ekf2_timestamps); - UpdateMagSample(ekf2_timestamps); - UpdateRangeSample(ekf2_timestamps); + UpdateAirspeedSample(); + UpdateAuxVelSample(); + UpdateBaroSample(); + UpdateGpsSample(); + UpdateMagSample(); + UpdateRangeSample(); vehicle_odometry_s ev_odom; - const bool new_ev_odom = UpdateExtVisionSample(ekf2_timestamps, ev_odom); + const bool new_ev_odom = UpdateExtVisionSample(ev_odom); optical_flow_s optical_flow; - const bool new_optical_flow = UpdateFlowSample(ekf2_timestamps, optical_flow); + const bool new_optical_flow = UpdateFlowSample(optical_flow); // run the EKF update and output @@ -529,9 +482,6 @@ void EKF2::Run() if (new_optical_flow) { PublishOpticalFlowVel(now, optical_flow); } - - // publish ekf2_timestamps - _ekf2_timestamps_pub.publish(ekf2_timestamps); } } @@ -545,13 +495,7 @@ void EKF2::PublishAttitude(const hrt_abstime ×tamp) q.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); } } @@ -570,7 +514,7 @@ void EKF2::PublishBaroBias(const hrt_abstime ×tamp) baro_bias.innov = status.innov; baro_bias.innov_var = status.innov_var; baro_bias.innov_test_ratio = status.innov_test_ratio; - baro_bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + baro_bias.timestamp = hrt_absolute_time(); _estimator_baro_bias_pub.publish(baro_bias); _last_baro_bias_published = status.bias; @@ -590,7 +534,7 @@ void EKF2::PublishEkfDriftMetrics(const hrt_abstime ×tamp) drift_data.vpos_drift_rate = gps_drift[1]; drift_data.hspd = gps_drift[2]; drift_data.blocked = blocked; - drift_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + drift_data.timestamp = hrt_absolute_time(); _ekf_gps_drift_pub.publish(drift_data); } @@ -649,7 +593,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.publish(event_flags); } @@ -694,7 +638,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) } global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning - global_pos.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + global_pos.timestamp = hrt_absolute_time(); _global_position_pub.publish(global_pos); } } @@ -719,7 +663,7 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp, const imuSample &imu // Not yet supported innovations.aux_vvel = NAN; - 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 @@ -755,7 +699,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) // Not yet supported test_ratios.aux_vvel = NAN; - test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + test_ratios.timestamp = hrt_absolute_time(); _estimator_innovation_test_ratios_pub.publish(test_ratios); } @@ -779,7 +723,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) // Not yet supported variances.aux_vvel = NAN; - variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + variances.timestamp = hrt_absolute_time(); _estimator_innovation_variances_pub.publish(variances); } @@ -890,7 +834,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); } @@ -957,7 +901,7 @@ void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu) odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6]; // publish vehicle odometry data - odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + odom.timestamp = hrt_absolute_time(); _odometry_pub.publish(odom); } @@ -1050,7 +994,7 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) _last_mag_bias_published = mag_bias; } - bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + bias.timestamp = hrt_absolute_time(); _estimator_sensor_bias_pub.publish(bias); } } @@ -1063,7 +1007,7 @@ void EKF2::PublishStates(const hrt_abstime ×tamp) states.n_states = Ekf::_k_num_states; _ekf.getStateAtFusionHorizonAsVector().copyTo(states.states); _ekf.covariances_diagonal().copyTo(states.covariances); - states.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + states.timestamp = hrt_absolute_time(); _estimator_states_pub.publish(states); } @@ -1117,7 +1061,7 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) status.gyro_device_id = _device_id_gyro; status.mag_device_id = _device_id_mag; - status.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + status.timestamp = hrt_absolute_time(); _estimator_status_pub.publish(status); } @@ -1216,7 +1160,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_flag_update = status_flags.timestamp; @@ -1236,7 +1180,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime ×tamp) yaw_est_test_data.weight)) { yaw_est_test_data.timestamp_sample = timestamp; - 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); } @@ -1260,7 +1204,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); } @@ -1276,7 +1220,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp, const optical_flo _ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral); _ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral); _ekf.getFlowGyro().copyTo(flow_vel.gyro_rate_integral); - flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + flow_vel.timestamp = hrt_absolute_time(); _estimator_optical_flow_vel_pub.publish(flow_vel); } @@ -1302,17 +1246,31 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt) return amsl_hgt + _wgs84_hgt_offset; } -void EKF2::SelectImuStatus() +void EKF2::SelectImu() { + if (!_multi_mode) { + for (uint8_t imu_instance = 0; imu_instance < MAX_NUM_IMUS; imu_instance++) { + uORB::Subscription vehicle_imu_sub{ORB_ID(vehicle_imu), imu_instance}; + + vehicle_imu_s vehicle_imu{}; + vehicle_imu_sub.copy(&vehicle_imu); + + if (vehicle_imu.accel_device_id == _device_id_accel) { + _vehicle_imu_sub.ChangeInstance(imu_instance); + break; + } + } + } + for (uint8_t imu_instance = 0; imu_instance < MAX_NUM_IMUS; imu_instance++) { - uORB::Subscription imu_status_sub{ORB_ID(vehicle_imu_status), imu_instance}; + uORB::Subscription vehicle_imu_status_sub{ORB_ID(vehicle_imu_status), imu_instance}; - vehicle_imu_status_s imu_status{}; - imu_status_sub.copy(&imu_status); + vehicle_imu_status_s vehicle_imu_status{}; + vehicle_imu_status_sub.copy(&vehicle_imu_status); - if (imu_status.accel_device_id == _device_id_accel) { + if (vehicle_imu_status.accel_device_id == _device_id_accel) { _vehicle_imu_status_sub.ChangeInstance(imu_instance); - return; + break; } } @@ -1326,7 +1284,7 @@ void EKF2::UpdateImuStatus() if (_vehicle_imu_status_sub.update(&imu_status)) { if (imu_status.accel_device_id != _device_id_accel) { - SelectImuStatus(); + SelectImu(); return; } @@ -1339,7 +1297,7 @@ void EKF2::UpdateImuStatus() } } -void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) +void EKF2::UpdateAirspeedSample() { // EKF airspeed sample const unsigned last_generation = _airspeed_sub.get_last_generation(); @@ -1370,13 +1328,10 @@ 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); } } -void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps) +void EKF2::UpdateAuxVelSample() { // EKF auxillary velocity sample // - use the landing target pose estimate as another source of velocity data @@ -1404,7 +1359,7 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps) } } -void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps) +void EKF2::UpdateBaroSample() { // EKF baro sample const unsigned last_generation = _airdata_sub.get_last_generation(); @@ -1423,13 +1378,10 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps) _ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter}); _device_id_baro = airdata.baro_device_id; - - ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 - - (int64_t)ekf2_timestamps.timestamp / 100); } } -bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom) +bool EKF2::UpdateExtVisionSample(vehicle_odometry_s &ev_odom) { // EKF external vision sample bool new_ev_odom = false; @@ -1519,15 +1471,12 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo _ekf.setExtVisionData(ev_data); new_ev_odom = true; - - ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)ev_odom.timestamp / 100 - - (int64_t)ekf2_timestamps.timestamp / 100); } return new_ev_odom; } -bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &optical_flow) +bool EKF2::UpdateFlowSample(optical_flow_s &optical_flow) { // EKF flow sample bool new_optical_flow = false; @@ -1563,15 +1512,12 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s & new_optical_flow = true; } - - ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 - - (int64_t)ekf2_timestamps.timestamp / 100); } return new_optical_flow; } -void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) +void EKF2::UpdateGpsSample() { // EKF GPS message const unsigned last_generation = _vehicle_gps_position_sub.get_last_generation(); @@ -1614,7 +1560,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) } } -void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) +void EKF2::UpdateMagSample() { const unsigned last_generation = _magnetometer_sub.get_last_generation(); vehicle_magnetometer_s magnetometer; @@ -1655,13 +1601,10 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) } _ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}}); - - ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 - - (int64_t)ekf2_timestamps.timestamp / 100); } } -void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) +void EKF2::UpdateRangeSample() { if (!_distance_sensor_selected) { // get subscription index of first downward-facing range sensor @@ -1718,9 +1661,6 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) _last_range_sensor_update = distance_sensor.timestamp; return; } - - ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)distance_sensor.timestamp / 100 - - (int64_t)ekf2_timestamps.timestamp / 100); } if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) { @@ -1768,7 +1708,7 @@ void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) _param_ekf2_mag_decl.set(declination_deg); _mag_decl_saved = true; - if (!_multi_mode) { + if (!_multi_mode || (_multi_mode && _instance == 0)) { _param_ekf2_mag_decl.commit_no_notification(); } } @@ -1784,12 +1724,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(CONSTRAINED_FLASH) bool multi_mode = false; @@ -1847,6 +1781,7 @@ int EKF2::task_spawn(int argc, char *argv[]) } const hrt_abstime time_started = hrt_absolute_time(); + //PX4_INFO("start time %lu", time_started); const int multi_instances = math::min(imu_instances * mag_instances, static_cast(EKF2_MAX_INSTANCES)); int multi_instances_allocated = 0; @@ -1857,9 +1792,11 @@ 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))) { + //PX4_INFO("start time %lu elapsed: %lu", time_started, hrt_elapsed_time(&time_started)); + vehicle_status_sub.update(); for (uint8_t mag = 0; mag < mag_instances; mag++) { @@ -1876,7 +1813,7 @@ int EKF2::task_spawn(int argc, char *argv[]) && (vehicle_imu_sub.get().gyro_device_id != 0)) { 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 @@ -1924,13 +1861,15 @@ int EKF2::task_spawn(int argc, char *argv[]) } } - } + //PX4_INFO("start time %lu elapsed: %lu", time_started, hrt_elapsed_time(&time_started)); + + } else #endif // !CONSTRAINED_FLASH - else { + { // 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); @@ -1955,14 +1894,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/master/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_DEFAULT_COMMANDS(); #if !defined(CONSTRAINED_FLASH) PRINT_MODULE_USAGE_COMMAND_DESCR("select_instance", "Request switch to new estimator instance"); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 57c339d95e..3f89c5bfd0 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -65,7 +65,6 @@ #include #include #include -#include #include #include #include @@ -78,7 +77,6 @@ #include #include #include -#include #include #include #include @@ -102,7 +100,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 */ @@ -154,16 +152,16 @@ private: void PublishWindEstimate(const hrt_abstime ×tamp); void PublishYawEstimatorStatus(const hrt_abstime ×tamp); - void SelectImuStatus(); + void SelectImu(); - void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps); - void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps); - void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps); - bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom); - bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &optical_flow); - void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); - void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps); - void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps); + void UpdateAirspeedSample(); + void UpdateAuxVelSample(); + void UpdateBaroSample(); + bool UpdateExtVisionSample(vehicle_odometry_s &ev_odom); + bool UpdateFlowSample(optical_flow_s &optical_flow); + void UpdateGpsSample(); + void UpdateMagSample(); + void UpdateRangeSample(); void UpdateImuStatus(); void UpdateMagCalibration(const hrt_abstime ×tamp); @@ -175,7 +173,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}; @@ -247,7 +244,6 @@ private: uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; - uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; bool _callback_registered{false}; @@ -269,7 +265,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::PublicationMulti _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; uORB::PublicationMulti _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)}; uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; diff --git a/src/modules/ekf2/test/sensor_simulator/convertULogToSensorData.py b/src/modules/ekf2/test/sensor_simulator/convertULogToSensorData.py index 0e656a684a..0ffcca7abc 100644 --- a/src/modules/ekf2/test/sensor_simulator/convertULogToSensorData.py +++ b/src/modules/ekf2/test/sensor_simulator/convertULogToSensorData.py @@ -122,15 +122,15 @@ def getMagnetometerData(ulog: ULog) -> pd.DataFrame: def getImuData(ulog: ULog) -> pd.DataFrame: - sensor_combined = ulog.get_dataset("sensor_combined").data - imu = pd.DataFrame({'timestamp': sensor_combined['timestamp'], + vehicle_imu = ulog.get_dataset("vehicle_imu").data + imu = pd.DataFrame({'timestamp': vehicle_imu['timestamp'], 'sensor' : 'imu', - 'accel_m_s2[0]': sensor_combined["accelerometer_m_s2[0]"], - 'accel_m_s2[1]': sensor_combined["accelerometer_m_s2[1]"], - 'accel_m_s2[2]': sensor_combined["accelerometer_m_s2[2]"], - 'gyro_rad[0]': sensor_combined["gyro_rad[0]"], - 'gyro_rad[1]': sensor_combined["gyro_rad[1]"], - 'gyro_rad[2]': sensor_combined["gyro_rad[2]"]}) + 'delta_velocity[0]': vehicle_imu["delta_velocity[0]"], + 'delta_velocity[1]': vehicle_imu["delta_velocity[1]"], + 'delta_velocity[2]': vehicle_imu["delta_velocity[2]"], + 'delta_angle[0]': vehicle_imu["delta_angle[0]"], + 'delta_angle[1]': vehicle_imu["delta_angle[1]"], + 'delta_angle[2]': vehicle_imu["delta_angle[2]"]}) return imu def getVehicleLandingStatus(ulog: ULog) -> pd.DataFrame: diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 7ee630e29c..ba61f5a1bd 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -184,7 +184,7 @@ void BlockLocalPositionEstimator::Run() } } - sensor_combined_s imu; + vehicle_imu_s imu; if (!_sensors_sub.update(&imu)) { return; @@ -914,11 +914,11 @@ void BlockLocalPositionEstimator::updateSSParams() m_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density; } -void BlockLocalPositionEstimator::predict(const sensor_combined_s &imu) +void BlockLocalPositionEstimator::predict(const vehicle_imu_s &imu) { // get acceleration _R_att = matrix::Dcm(matrix::Quatf(_sub_att.get().q)); - Vector3f a(imu.accelerometer_m_s2); + const Vector3f a{Vector3f{imu.delta_velocity} * 1.e6f / (float)imu.delta_velocity_dt}; // note, bias is removed in dynamics function _u = _R_att * a; _u(U_az) += CONSTANTS_ONE_G; // add g diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 9d1af11e96..f75bd74e90 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include #include @@ -184,7 +184,7 @@ private: void updateSSParams(); // predict the next state - void predict(const sensor_combined_s &imu); + void predict(const vehicle_imu_s &imu); // lidar int lidarMeasure(Vector &y); @@ -262,7 +262,7 @@ private: // ---------------------------- // subscriptions - uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)}; + uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(vehicle_imu)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index e0ce45bff3..f2fc72f6bf 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -82,7 +82,6 @@ void LoggedTopics::add_default_topics() add_topic("rpm", 500); add_topic("rtl_flight_time", 1000); add_topic("safety"); - add_topic("sensor_combined"); add_topic("sensor_correction"); add_topic("sensor_gyro_fft", 50); add_topic("sensor_selection"); @@ -92,7 +91,7 @@ void LoggedTopics::add_default_topics() add_topic("tecs_status", 200); add_topic("trajectory_setpoint", 200); add_topic("transponder_report"); - add_topic("vehicle_acceleration", 50); + add_topic("vehicle_acceleration", 20); add_topic("vehicle_air_data", 200); add_topic("vehicle_angular_velocity", 20); add_topic("vehicle_attitude", 50); @@ -163,7 +162,7 @@ void LoggedTopics::add_default_topics() add_topic_multi("sensor_gps", 1000, 2); add_topic_multi("sensor_gyro", 1000, 4); add_topic_multi("sensor_mag", 1000, 4); - add_topic_multi("vehicle_imu", 500, 4); + add_topic_multi("vehicle_imu", 10, 4); add_topic_multi("vehicle_imu_status", 1000, 4); add_topic_multi("vehicle_magnetometer", 500, 4); @@ -195,12 +194,12 @@ void LoggedTopics::add_high_rate_topics() add_topic("actuator_outputs"); add_topic("manual_control_setpoint"); add_topic("rate_ctrl_status", 20); - add_topic("sensor_combined"); add_topic("vehicle_angular_acceleration"); add_topic("vehicle_angular_velocity"); add_topic("vehicle_attitude"); add_topic("vehicle_attitude_setpoint"); add_topic("vehicle_rates_setpoint"); + add_topic_multi("vehicle_imu", 0, 4); } void LoggedTopics::add_debug_topics() @@ -217,21 +216,23 @@ 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("landing_target_pose"); add_topic("optical_flow"); - add_topic("sensor_combined"); + add_topic("parameter_update"); add_topic("sensor_selection"); + add_topic("sensors_status_imu"); 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_status"); add_topic("vehicle_visual_odometry"); - add_topic_multi("distance_sensor"); + add_topic_multi("distance_sensor", 0, 2); + add_topic_multi("vehicle_imu", 0, 4); + add_topic_multi("vehicle_imu_status", 0, 4); + add_topic_multi("vehicle_magnetometer", 0, 4); } void LoggedTopics::add_thermal_calibration_topics() @@ -274,9 +275,9 @@ void LoggedTopics::add_system_identification_topics() // for system id need to log imu and controls at full rate add_topic("actuator_controls_0"); add_topic("actuator_controls_1"); - add_topic("sensor_combined"); add_topic("vehicle_angular_acceleration"); add_topic("vehicle_angular_acceleration_setpoint"); + add_topic("vehicle_angular_velocity"); add_topic("vehicle_torque_setpoint"); } diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 8d959690ec..659cfd8818 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -663,6 +663,8 @@ void Logger::run() if (polling_topic_sub >= 0) { _lockstep_component = px4_lockstep_register_component(); + + fprintf(stderr, "logger: px4_lockstep_register_component:%d\n", _lockstep_component); } bool was_started = false; diff --git a/src/modules/mavlink/streams/SCALED_IMU.hpp b/src/modules/mavlink/streams/SCALED_IMU.hpp index 7b81d44a78..16bb552172 100644 --- a/src/modules/mavlink/streams/SCALED_IMU.hpp +++ b/src/modules/mavlink/streams/SCALED_IMU.hpp @@ -84,7 +84,7 @@ private: msg.zacc = (int16_t)accel(2); // Gyroscope in mrad/s - const float gyro_dt_inv = 1.e6f / (float)imu.delta_velocity_dt; + const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt; const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f; msg.xgyro = gyro(0); msg.ygyro = gyro(1); diff --git a/src/modules/mavlink/streams/SCALED_IMU2.hpp b/src/modules/mavlink/streams/SCALED_IMU2.hpp index ab45751665..58caa3e106 100644 --- a/src/modules/mavlink/streams/SCALED_IMU2.hpp +++ b/src/modules/mavlink/streams/SCALED_IMU2.hpp @@ -84,7 +84,7 @@ private: msg.zacc = (int16_t)accel(2); // Gyroscope in mrad/s - const float gyro_dt_inv = 1.e6f / (float)imu.delta_velocity_dt; + const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt; const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f; msg.xgyro = gyro(0); msg.ygyro = gyro(1); diff --git a/src/modules/mavlink/streams/SCALED_IMU3.hpp b/src/modules/mavlink/streams/SCALED_IMU3.hpp index 5512b10cb3..9d7cc40ad6 100644 --- a/src/modules/mavlink/streams/SCALED_IMU3.hpp +++ b/src/modules/mavlink/streams/SCALED_IMU3.hpp @@ -84,7 +84,7 @@ private: msg.zacc = (int16_t)accel(2); // Gyroscope in mrad/s - const float gyro_dt_inv = 1.e6f / (float)imu.delta_velocity_dt; + const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt; const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f; msg.xgyro = gyro(0); msg.ygyro = gyro(1); diff --git a/src/modules/replay/CMakeLists.txt b/src/modules/replay/CMakeLists.txt index 1e83c882e2..9051a22050 100644 --- a/src/modules/replay/CMakeLists.txt +++ b/src/modules/replay/CMakeLists.txt @@ -39,6 +39,4 @@ px4_add_module( 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 cd4503a6e6..3755ec6639 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-2021 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 @@ -62,7 +62,6 @@ #include #include "Replay.hpp" -#include "ReplayEkf2.hpp" #define PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params.txt" @@ -74,16 +73,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) { @@ -93,27 +82,6 @@ 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) { @@ -237,6 +205,8 @@ Replay::readFileDefinitions(std::ifstream &file) break; case (int)ULogMessageType::PARAMETER: + + // case (int)ULogMessageType::PARAMETER_DEFAULT: // TODO if (!readAndApplyParameter(file, message_header.msg_size)) { return false; } @@ -249,6 +219,7 @@ Replay::readFileDefinitions(std::ifstream &file) case (int)ULogMessageType::INFO: //skip case (int)ULogMessageType::INFO_MULTIPLE: //skip + case (int)ULogMessageType::PARAMETER_DEFAULT: // TODO file.seekg(message_header.msg_size, ios::cur); break; @@ -365,51 +336,21 @@ 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 string file_format = _file_formats[topic_name]; if (file_format != orb_meta->o_fields) { - // check if we have a compatibility conversion available - if (topic_name == "sensor_combined") { - if (string(orb_meta->o_fields) == "uint64_t timestamp;float[3] gyro_rad;uint32_t gyro_integral_dt;" - "int32_t accelerometer_timestamp_relative;float[3] accelerometer_m_s2;" - "uint32_t accelerometer_integral_dt" && - file_format == "uint64_t timestamp;float[3] gyro_rad;float gyro_integral_dt;" - "int32_t accelerometer_timestamp_relative;float[3] accelerometer_m_s2;" - "float accelerometer_integral_dt;") { - - int gyro_integral_dt_offset_log; - 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_meta->o_fields, "gyro_integral_dt", gyro_integral_dt_offset_intern, unused) && - findFieldOffset(file_format, "accelerometer_integral_dt", accelerometer_integral_dt_offset_log, unused) && - findFieldOffset(orb_meta->o_fields, "accelerometer_integral_dt", accelerometer_integral_dt_offset_intern, unused)) { - - compat = new CompatSensorCombinedDtType(gyro_integral_dt_offset_log, gyro_integral_dt_offset_intern, - accelerometer_integral_dt_offset_log, accelerometer_integral_dt_offset_intern); - } - } - } - - if (!compat) { - PX4_ERR("Formats for %s don't match. Will ignore it.", topic_name.c_str()); - PX4_WARN(" Internal format: %s", orb_meta->o_fields); - PX4_WARN(" File format : %s", file_format.c_str()); - return true; // not a fatal error - } + PX4_ERR("Formats for %s don't match. Will ignore it.", topic_name.c_str()); + PX4_WARN(" Internal format: %s", orb_meta->o_fields); + PX4_WARN(" File format : %s", file_format.c_str()); + 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; @@ -764,6 +705,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)) { @@ -777,10 +727,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; @@ -794,14 +740,17 @@ Replay::run() 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) { + int imu_publish_count = 0; + bool imu_published = false; - //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; + + 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; @@ -823,36 +772,99 @@ 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) { - //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 + + // ekf2 replay + if (sub.orb_meta && + ((strcmp(sub.orb_meta->o_name, "estimator_status") == 0) + || (strcmp(sub.orb_meta->o_name, "estimator_local_position") == 0) + || (strcmp(sub.orb_meta->o_name, "estimator_global_position") == 0) + || (strcmp(sub.orb_meta->o_name, "estimator_attitude") == 0) + || (strcmp(sub.orb_meta->o_name, "estimator_sensor_bias") == 0) + || (strcmp(sub.orb_meta->o_name, "ekf_gps_drift") == 0) + || (strcmp(sub.orb_meta->o_name, "vehicle_attitude") == 0) + || (strcmp(sub.orb_meta->o_name, "vehicle_local_position") == 0) + || (strcmp(sub.orb_meta->o_name, "vehicle_global_position") == 0)) + ) { + // skipping blocked message + nextDataMessage(replay_file, sub, next_msg_id); + continue; + } + + // 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); last_additional_message_pos = next_additional_message_pos; - const uint64_t publish_timestamp = handleTopicDelay(next_file_time, timestamp_offset); + const uint64_t publish_timestamp = sub.next_timestamp; + + // 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(); + } // It's time to publish readTopicDataToBuffer(sub, replay_file); - memcpy(_read_buffer.data() + sub.timestamp_offset, &publish_timestamp, sizeof(uint64_t)); //adjust the timestamp - if (handleTopicUpdate(sub, _read_buffer.data(), replay_file)) { + if (publish_timestamp >= timestamp_last) { + 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 %lu (last timestamp %lu)", sub.orb_meta->o_name, sub.multi_id, publish_timestamp, + timestamp_last); + } + + if (publishTopic(sub, _read_buffer.data())) { ++nr_published_messages; } + timestamp_last = publish_timestamp; + nextDataMessage(replay_file, sub, next_msg_id); // TODO: output status (eg. every sec), including total duration... + + + // Wait for other modules, such as logger or ekf2 + if (imu_published) { + px4_lockstep_wait_for_components(); + + } else { + if (sub.orb_meta) { + if (strcmp(sub.orb_meta->o_name, "vehicle_imu") == 0) { + imu_publish_count++; + + if (imu_publish_count > 1000) { + //imu_published = true; + } + } + } + + //system_usleep(1); + } + + if (hrt_elapsed_time(&last_update) >= 1_s) { + PX4_INFO("[%.6fs] %d messages published", timestamp_last * 1e-6, nr_published_messages); + last_update = timestamp_last; + } } for (auto &subscription : _subscriptions) { @@ -860,11 +872,6 @@ Replay::run() continue; } - if (subscription->compat) { - delete subscription->compat; - subscription->compat = nullptr; - } - if (subscription->orb_advert) { orb_unadvertise(subscription->orb_advert); subscription->orb_advert = nullptr; @@ -876,8 +883,6 @@ Replay::run() (double)hrt_elapsed_time(&_replay_start_time) / 1.e6); } - onExitMainLoop(); - if (!should_exit()) { replay_file.close(); px4_shutdown_request(); @@ -903,50 +908,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; @@ -988,6 +954,142 @@ 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); + return lenstr < lenpre ? false : strncmp(pre, str, lenpre) == 0; +} + +bool Replay::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 Replay::strTrim(const char **str) +{ + while (**str == ' ' || **str == '\t') { ++(*str); } +} + +int Replay::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; +} + int Replay::custom_command(int argc, char *argv[]) { @@ -999,6 +1101,8 @@ Replay::custom_command(int argc, char *argv[]) return Replay::task_spawn(argc, argv); } + // ignore? + return print_usage("unknown command"); } @@ -1065,17 +1169,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; } diff --git a/src/modules/replay/Replay.hpp b/src/modules/replay/Replay.hpp index 6b52c120ad..b24a5862ff 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-2021 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 @@ -43,7 +43,6 @@ #include #include -#include namespace px4 { @@ -95,36 +94,6 @@ 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 @@ -137,8 +106,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; @@ -162,34 +129,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 @@ -206,7 +155,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 @@ -278,6 +227,46 @@ private: void setUserParams(const char *filename); static char *_replay_file; + + + + + 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; + + }; } //namespace px4 diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp deleted file mode 100644 index 941718323b..0000000000 --- a/src/modules/replay/ReplayEkf2.cpp +++ /dev/null @@ -1,214 +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 - -// for ekf2 replay -#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(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; - } - - // 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); - - // 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 -} - -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, "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"); -} - -} // namespace px4 diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp deleted file mode 100644 index 43c7de1913..0000000000 --- a/src/modules/replay/ReplayEkf2.hpp +++ /dev/null @@ -1,93 +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; -}; - -} //namespace px4 diff --git a/src/modules/replay/replay_main.cpp b/src/modules/replay/replay_main.cpp index 9f18eb21d8..1fc71a05e3 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-2021 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/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d44a622c18..cf2e50ffad 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -115,9 +115,6 @@ private: bool _armed{false}; /**< arming status of the vehicle */ hrt_abstime _last_config_update{0}; - hrt_abstime _sensor_combined_prev_timestamp{0}; - - sensor_combined_s _sensor_combined{}; uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[MAX_SENSOR_COUNT] { {this, ORB_ID(vehicle_imu), 0}, @@ -133,7 +130,6 @@ private: uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; - uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -632,7 +628,7 @@ void Sensors::Run() InitializeVehicleIMU(); InitializeVehicleGPSPosition(); InitializeVehicleMagnetometer(); - _voted_sensors_update.init(_sensor_combined); + _voted_sensors_update.init(); parameter_update_poll(true); } @@ -650,20 +646,13 @@ void Sensors::Run() } } - _voted_sensors_update.sensorsPoll(_sensor_combined); + _voted_sensors_update.sensorsPoll(); // check analog airspeed adc_poll(); diff_pres_poll(); - if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) { - - _voted_sensors_update.setRelativeTimestamps(_sensor_combined); - _sensor_pub.publish(_sensor_combined); - _sensor_combined_prev_timestamp = _sensor_combined.timestamp; - } - // keep adding sensors as long as we are not armed, // when not adding sensors poll for param updates if (!_armed && hrt_elapsed_time(&_last_config_update) > 1000_ms) { @@ -814,8 +803,7 @@ it into a more usable form, and publishes it for the rest of the system. The provided functionality includes: - Read the output from the sensor drivers (`sensor_gyro`, etc.). If there are multiple of the same type, do voting and failover handling. - Then apply the board rotation and temperature calibration (if enabled). And finally publish the data; one of the - topics is `sensor_combined`, used by many parts of the system. + Then apply the board rotation and temperature calibration (if enabled). And finally publish the selection. - Make sure the sensor drivers get the updated calibration parameters (scale & offset) when the parameters change or on startup. The sensor drivers use the ioctl interface for parameter updates. For this to work properly, the sensor drivers must already be running when `sensors` is started. diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 35d1fe6cbf..6e891379b6 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -60,11 +60,8 @@ VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled, } } -int VotedSensorsUpdate::init(sensor_combined_s &raw) +int VotedSensorsUpdate::init() { - raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; - raw.timestamp = 0; - initializeSensors(); _selection_changed = true; @@ -138,7 +135,7 @@ void VotedSensorsUpdate::parametersUpdate() } } -void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) +void VotedSensorsUpdate::imuPoll() { for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { vehicle_imu_s imu_report; @@ -151,36 +148,21 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) _vehicle_imu_status_subs[uorb_index].copy(&imu_status); _accel_device_id[uorb_index] = imu_report.accel_device_id; - _gyro_device_id[uorb_index] = imu_report.gyro_device_id; - // convert the delta velocities to an equivalent acceleration const float accel_dt_inv = 1.e6f / (float)imu_report.delta_velocity_dt; - Vector3f accel_data = Vector3f{imu_report.delta_velocity} * accel_dt_inv; - + _accel_last[uorb_index] = Vector3f{imu_report.delta_velocity} * accel_dt_inv; + float accel_array[3]; + _accel_last[uorb_index].copyTo(accel_array); + _accel.voter.put(uorb_index, imu_report.timestamp, accel_array, imu_status.accel_error_count, + _accel.priority[uorb_index]); + _gyro_device_id[uorb_index] = imu_report.gyro_device_id; // convert the delta angles to an equivalent angular rate const float gyro_dt_inv = 1.e6f / (float)imu_report.delta_angle_dt; - Vector3f gyro_rate = Vector3f{imu_report.delta_angle} * gyro_dt_inv; - - _last_sensor_data[uorb_index].timestamp = imu_report.timestamp_sample; - _last_sensor_data[uorb_index].accelerometer_m_s2[0] = accel_data(0); - _last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1); - _last_sensor_data[uorb_index].accelerometer_m_s2[2] = accel_data(2); - _last_sensor_data[uorb_index].accelerometer_integral_dt = imu_report.delta_velocity_dt; - _last_sensor_data[uorb_index].accelerometer_clipping = imu_report.delta_velocity_clipping; - _last_sensor_data[uorb_index].gyro_rad[0] = gyro_rate(0); - _last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1); - _last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2); - _last_sensor_data[uorb_index].gyro_integral_dt = imu_report.delta_angle_dt; - - - _last_accel_timestamp[uorb_index] = imu_report.timestamp_sample; - - _accel.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2, - imu_status.accel_error_count, _accel.priority[uorb_index]); - - _gyro.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].gyro_rad, - imu_status.gyro_error_count, _gyro.priority[uorb_index]); + _gyro_last[uorb_index] = Vector3f{imu_report.delta_angle} * gyro_dt_inv; + float gyro_array[3]; + _gyro_last[uorb_index].copyTo(gyro_array); + _gyro.voter.put(uorb_index, imu_report.timestamp, gyro_array, imu_status.gyro_error_count, _gyro.priority[uorb_index]); } } @@ -221,14 +203,6 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) // write data for the best sensor to output variables if ((accel_best_index >= 0) && (gyro_best_index >= 0)) { - raw.timestamp = _last_sensor_data[gyro_best_index].timestamp; - memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2, - sizeof(raw.accelerometer_m_s2)); - memcpy(&raw.gyro_rad, &_last_sensor_data[gyro_best_index].gyro_rad, sizeof(raw.gyro_rad)); - raw.accelerometer_integral_dt = _last_sensor_data[accel_best_index].accelerometer_integral_dt; - raw.gyro_integral_dt = _last_sensor_data[gyro_best_index].gyro_integral_dt; - raw.accelerometer_clipping = _last_sensor_data[accel_best_index].accelerometer_clipping; - if ((accel_best_index != _accel.last_best_vote) || (_selection.accel_device_id != _accel_device_id[accel_best_index])) { _accel.last_best_vote = (uint8_t)accel_best_index; _selection.accel_device_id = _accel_device_id[accel_best_index]; @@ -386,9 +360,9 @@ void VotedSensorsUpdate::printStatus() _accel.voter.print(); } -void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw) +void VotedSensorsUpdate::sensorsPoll() { - imuPoll(raw); + imuPoll(); calcAccelInconsistency(); calcGyroInconsistency(); @@ -423,14 +397,6 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw) _sensors_status_imu_pub.publish(status); } -void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw) -{ - if (_last_accel_timestamp[_accel.last_best_vote]) { - raw.accelerometer_timestamp_relative = (int32_t)((int64_t)_last_accel_timestamp[_accel.last_best_vote] - - (int64_t)raw.timestamp); - } -} - void VotedSensorsUpdate::calcAccelInconsistency() { Vector3f accel_mean{}; @@ -440,7 +406,7 @@ void VotedSensorsUpdate::calcAccelInconsistency() for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) { accel_count++; - accel_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].accelerometer_m_s2}; + accel_all[sensor_index] = _accel_last[sensor_index]; accel_mean += accel_all[sensor_index]; } } @@ -465,7 +431,7 @@ void VotedSensorsUpdate::calcGyroInconsistency() for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) { gyro_count++; - gyro_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].gyro_rad}; + gyro_all[sensor_index] = _gyro_last[sensor_index]; gyro_mean += gyro_all[sensor_index]; } } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 4ae773a867..e9e40ebe97 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -53,7 +53,6 @@ #include #include #include -#include #include #include #include @@ -82,7 +81,7 @@ public: * initialize subscriptions etc. * @return 0 on success, <0 otherwise */ - int init(sensor_combined_s &raw); + int init(); /** * This tries to find new sensor instances. This is called from init(), then it can be called periodically. @@ -100,13 +99,7 @@ public: /** * read new sensor data */ - void sensorsPoll(sensor_combined_s &raw); - - /** - * set the relative timestamps of each sensor timestamp, based on the last sensorsPoll, - * so that the data can be published. - */ - void setRelativeTimestamps(sensor_combined_s &raw); + void sensorsPoll(); private: @@ -134,7 +127,7 @@ private: * @param raw Combined sensor data structure into which * data should be returned. */ - void imuPoll(sensor_combined_s &raw); + void imuPoll(); /** * Check & handle failover of a sensor @@ -166,20 +159,19 @@ private: uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; - sensor_combined_s _last_sensor_data[MAX_SENSOR_COUNT] {}; /**< latest sensor data from all sensors instances */ - const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */ bool _selection_changed{true}; /**< true when a sensor selection has changed and not been published */ + matrix::Vector3f _accel_last[MAX_SENSOR_COUNT] {}; matrix::Vector3f _accel_diff[MAX_SENSOR_COUNT] {}; /**< filtered accel differences between IMU units (m/s/s) */ + + matrix::Vector3f _gyro_last[MAX_SENSOR_COUNT] {}; matrix::Vector3f _gyro_diff[MAX_SENSOR_COUNT] {}; /**< filtered gyro differences between IMU uinits (rad/s) */ uint32_t _accel_device_id[MAX_SENSOR_COUNT] {}; /**< accel driver device id for each uorb instance */ uint32_t _gyro_device_id[MAX_SENSOR_COUNT] {}; /**< gyro driver device id for each uorb instance */ - uint64_t _last_accel_timestamp[MAX_SENSOR_COUNT] {}; /**< latest full timestamp */ - sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */ DEFINE_PARAMETERS( 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 diff --git a/src/templates/template_module/template_module.cpp b/src/templates/template_module/template_module.cpp index ad087d40ea..b257b721ac 100644 --- a/src/templates/template_module/template_module.cpp +++ b/src/templates/template_module/template_module.cpp @@ -38,8 +38,6 @@ #include #include -#include - int TemplateModule::print_status() { @@ -137,11 +135,11 @@ TemplateModule::TemplateModule(int example_param, bool example_flag) void TemplateModule::run() { - // Example: run the loop synchronized to the sensor_combined topic publication - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + // Example: run the loop synchronized to the vehicle_angular_velocity topic publication + int vehicle_angular_velocity_sub = orb_subscribe(ORB_ID(vehicle_angular_velocity)); px4_pollfd_struct_t fds[1]; - fds[0].fd = sensor_combined_sub; + fds[0].fd = vehicle_angular_velocity_sub; fds[0].events = POLLIN; // initialize parameters @@ -163,8 +161,8 @@ void TemplateModule::run() } else if (fds[0].revents & POLLIN) { - struct sensor_combined_s sensor_combined; - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor_combined); + vehicle_angular_velocity_s vehicle_angular_velocity; + orb_copy(ORB_ID(vehicle_angular_velocity), vehicle_angular_velocity_sub, &vehicle_angular_velocity); // TODO: do something with the data... } @@ -172,7 +170,7 @@ void TemplateModule::run() parameters_update(); } - orb_unsubscribe(sensor_combined_sub); + orb_unsubscribe(vehicle_angular_velocity_sub); } void TemplateModule::parameters_update(bool force)