From 4f62355decd13b1b273338a9991052657fdca045 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 29 Dec 2020 11:27:21 -0500 Subject: [PATCH] msg: new estimator_status_flags message for more accessible ekf2 status logging - log all estimator (ekf2) flags as separate booleans in a new dedicated low rate message (only publishes at 1 Hz or immediately on any change) - this is a bit verbose, but it avoids the duplicate bit definitions we currently have across PX4 msgs, ecl analysis script, flight review, and many other custom tools and it's much easier for casual log review in FlightPlot, PlotJuggler, csv, etc - for compatibility I've left estimator_status filter_fault_flags, innovation_check_flags, and solution_status_flags in place, but they can gradually be removed as tooling is updated Co-authored-by: Mathieu Bresciani --- .ci/Jenkinsfile-hardware | 2 + msg/CMakeLists.txt | 1 + msg/estimator_status_flags.msg | 72 +++++++++++++++++++ msg/tools/uorb_rtps_message_ids.yaml | 2 + src/modules/ekf2/EKF2.cpp | 103 +++++++++++++++++++++++++++ src/modules/ekf2/EKF2.hpp | 13 ++++ src/modules/logger/logged_topics.cpp | 1 + 7 files changed, 194 insertions(+) create mode 100644 msg/estimator_status_flags.msg diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index f0b6d6fd87..36b457b377 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -837,6 +837,7 @@ void statusFTDI() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_selector_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_sensor_bias"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_status_flags"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener logger_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel_fifo"' @@ -915,6 +916,7 @@ void statusSEGGER() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_selector_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_sensor_bias"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_status_flags"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener logger_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_accel"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_accel_fifo"' diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index c5c44dc09a..307ae06196 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -61,6 +61,7 @@ set(msg_files estimator_sensor_bias.msg estimator_states.msg estimator_status.msg + estimator_status_flags.msg follow_target.msg generator_status.msg geofence_result.msg diff --git a/msg/estimator_status_flags.msg b/msg/estimator_status_flags.msg new file mode 100644 index 0000000000..8ddaee34a7 --- /dev/null +++ b/msg/estimator_status_flags.msg @@ -0,0 +1,72 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + + +# filter control status +uint32 control_status_changes # number of filter control status changes +bool control_status_tilt_align # 0 - true if the filter tilt alignment is complete +bool control_status_yaw_align # 1 - true if the filter yaw alignment is complete +bool control_status_gps # 2 - true if GPS measurement fusion is intended +bool control_status_opt_flow # 3 - true if optical flow measurements fusion is intended +bool control_status_mag_hdg # 4 - true if a simple magnetic yaw heading fusion is intended +bool control_status_mag_3d # 5 - true if 3-axis magnetometer measurement fusion is inteded +bool control_status_mag_dec # 6 - true if synthetic magnetic declination measurements fusion is intended +bool control_status_in_air # 7 - true when the vehicle is airborne +bool control_status_wind # 8 - true when wind velocity is being estimated +bool control_status_baro_hgt # 9 - true when baro height is being fused as a primary height reference +bool control_status_rng_hgt # 10 - true when range finder height is being fused as a primary height reference +bool control_status_gps_hgt # 11 - true when GPS height is being fused as a primary height reference +bool control_status_ev_pos # 12 - true when local position data fusion from external vision is intended +bool control_status_ev_yaw # 13 - true when yaw data from external vision measurements fusion is intended +bool control_status_ev_hgt # 14 - true when height data from external vision measurements is being fused +bool control_status_fuse_beta # 15 - true when synthetic sideslip measurements are being fused +bool control_status_mag_field_disturbed # 16 - true when the mag field does not match the expected strength +bool control_status_fixed_wing # 17 - true when the vehicle is operating as a fixed wing vehicle +bool control_status_mag_fault # 18 - true when the magnetometer has been declared faulty and is no longer being used +bool control_status_fuse_aspd # 19 - true when airspeed measurements are being fused +bool control_status_gnd_effect # 20 - true when protection from ground effect induced static pressure rise is active +bool control_status_rng_stuck # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough +bool control_status_gps_yaw # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended +bool control_status_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment has been completed +bool control_status_ev_vel # 24 - true when local frame velocity data fusion from external vision measurements is intended +bool control_status_synthetic_mag_z # 25 - true when we are using a synthesized measurement for the magnetometer Z component +bool control_status_vehicle_at_rest # 26 - true when the vehicle is at rest + + +# fault status +uint32 fault_status_changes # number of filter fault status changes +bool fault_status_bad_mag_x # 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error +bool fault_status_bad_mag_y # 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error +bool fault_status_bad_mag_z # 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error +bool fault_status_bad_hdg # 3 - true if the fusion of the heading angle has encountered a numerical error +bool fault_status_bad_mag_decl # 4 - true if the fusion of the magnetic declination has encountered a numerical error +bool fault_status_bad_airspeed # 5 - true if fusion of the airspeed has encountered a numerical error +bool fault_status_bad_sideslip # 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error +bool fault_status_bad_optflow_x # 7 - true if fusion of the optical flow X axis has encountered a numerical error +bool fault_status_bad_optflow_y # 8 - true if fusion of the optical flow Y axis has encountered a numerical error +bool fault_status_bad_vel_n # 9 - true if fusion of the North velocity has encountered a numerical error +bool fault_status_bad_vel_e # 10 - true if fusion of the East velocity has encountered a numerical error +bool fault_status_bad_vel_d # 11 - true if fusion of the Down velocity has encountered a numerical error +bool fault_status_bad_pos_n # 12 - true if fusion of the North position has encountered a numerical error +bool fault_status_bad_pos_e # 13 - true if fusion of the East position has encountered a numerical error +bool fault_status_bad_pos_d # 14 - true if fusion of the Down position has encountered a numerical error +bool fault_status_bad_acc_bias # 15 - true if bad delta velocity bias estimates have been detected +bool fault_status_bad_acc_vertical # 16 - true if bad vertical accelerometer data has been detected +bool fault_status_bad_acc_clipping # 17 - true if delta velocity data contains clipping (asymmetric railing) + + +# innovation test failures +uint32 innovation_fault_status_changes # number of filter fault status changes +bool reject_hor_vel # 0 - true if horizontal velocity observations have been rejected +bool reject_ver_vel # 1 - true if vertical velocity observations have been rejected +bool reject_hor_pos # 2 - true if horizontal position observations have been rejected +bool reject_ver_pos # 3 - true if vertical position observations have been rejected +bool reject_mag_x # 4 - true if the X magnetometer observation has been rejected +bool reject_mag_y # 5 - true if the Y magnetometer observation has been rejected +bool reject_mag_z # 6 - true if the Z magnetometer observation has been rejected +bool reject_yaw # 7 - true if the yaw observation has been rejected +bool reject_airspeed # 8 - true if the airspeed observation has been rejected +bool reject_sideslip # 9 - true if the synthetic sideslip observation has been rejected +bool reject_hagl # 10 - true if the height above ground observation has been rejected +bool reject_optflow_x # 11 - true if the X optical flow observation has been rejected +bool reject_optflow_y # 12 - true if the Y optical flow observation has been rejected diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 4effe7d83d..6a127ae0e3 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -291,6 +291,8 @@ rtps: id: 139 - msg: manual_control_switches id: 140 + - msg: estimator_status_flags + id: 141 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index dbc4f40589..b183d66558 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -181,6 +181,7 @@ EKF2::EKF2(int instance, const px4::wq_config_t &config, int imu, int mag, bool _estimator_sensor_bias_pub.advertise(); _estimator_states_pub.advertise(); _estimator_status_pub.advertise(); + _estimator_status_flags_pub.advertise(); _estimator_visual_odometry_aligned_pub.advertised(); _wind_pub.advertise(); _yaw_est_pub.advertise(); @@ -443,6 +444,7 @@ void EKF2::Run() PublishEkfDriftMetrics(now); PublishStates(now); PublishStatus(now); + PublishStatusFlags(now); PublishInnovations(now, imu_sample_new); PublishInnovationTestRatios(now); PublishInnovationVariances(now); @@ -973,6 +975,107 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) _estimator_status_pub.publish(status); } +void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) +{ + // publish at ~ 1 Hz (or immediately if filter control status or fault status changes) + bool update = (hrt_elapsed_time(&_last_status_flag_update) >= 1_s); + + // filter control status + if (_ekf.control_status().value != _filter_control_status) { + update = true; + _filter_control_status = _ekf.control_status().value; + _filter_control_status_changes++; + } + + // filter fault status + if (_ekf.fault_status().value != _filter_fault_status) { + update = true; + _filter_fault_status = _ekf.fault_status().value; + _filter_fault_status_changes++; + } + + // innovation check fail status + if (_ekf.innov_check_fail_status().value != _innov_check_fail_status) { + update = true; + _innov_check_fail_status = _ekf.innov_check_fail_status().value; + _innov_check_fail_status_changes++; + } + + if (update) { + estimator_status_flags_s status_flags{}; + status_flags.timestamp_sample = timestamp; + + status_flags.control_status_changes = _filter_control_status_changes; + status_flags.control_status_tilt_align = _ekf.control_status_flags().tilt_align; + status_flags.control_status_yaw_align = _ekf.control_status_flags().yaw_align; + status_flags.control_status_gps = _ekf.control_status_flags().gps; + status_flags.control_status_opt_flow = _ekf.control_status_flags().opt_flow; + status_flags.control_status_mag_hdg = _ekf.control_status_flags().mag_hdg; + status_flags.control_status_mag_3d = _ekf.control_status_flags().mag_3D; + status_flags.control_status_mag_dec = _ekf.control_status_flags().mag_dec; + status_flags.control_status_in_air = _ekf.control_status_flags().in_air; + status_flags.control_status_wind = _ekf.control_status_flags().wind; + status_flags.control_status_baro_hgt = _ekf.control_status_flags().baro_hgt; + status_flags.control_status_rng_hgt = _ekf.control_status_flags().rng_hgt; + status_flags.control_status_gps_hgt = _ekf.control_status_flags().gps_hgt; + status_flags.control_status_ev_pos = _ekf.control_status_flags().ev_pos; + status_flags.control_status_ev_yaw = _ekf.control_status_flags().ev_yaw; + status_flags.control_status_ev_hgt = _ekf.control_status_flags().ev_hgt; + status_flags.control_status_fuse_beta = _ekf.control_status_flags().fuse_beta; + status_flags.control_status_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed; + status_flags.control_status_fixed_wing = _ekf.control_status_flags().fixed_wing; + status_flags.control_status_mag_fault = _ekf.control_status_flags().mag_fault; + status_flags.control_status_fuse_aspd = _ekf.control_status_flags().fuse_aspd; + status_flags.control_status_gnd_effect = _ekf.control_status_flags().gnd_effect; + status_flags.control_status_rng_stuck = _ekf.control_status_flags().rng_stuck; + status_flags.control_status_gps_yaw = _ekf.control_status_flags().gps_yaw; + status_flags.control_status_mag_aligned_in_flight = _ekf.control_status_flags().mag_aligned_in_flight; + status_flags.control_status_ev_vel = _ekf.control_status_flags().ev_vel; + status_flags.control_status_synthetic_mag_z = _ekf.control_status_flags().synthetic_mag_z; + status_flags.control_status_vehicle_at_rest = _ekf.control_status_flags().vehicle_at_rest; + + status_flags.fault_status_changes = _filter_fault_status_changes; + status_flags.fault_status_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; + status_flags.fault_status_bad_mag_y = _ekf.fault_status_flags().bad_mag_y; + status_flags.fault_status_bad_mag_z = _ekf.fault_status_flags().bad_mag_z; + status_flags.fault_status_bad_hdg = _ekf.fault_status_flags().bad_hdg; + status_flags.fault_status_bad_mag_decl = _ekf.fault_status_flags().bad_mag_decl; + status_flags.fault_status_bad_airspeed = _ekf.fault_status_flags().bad_airspeed; + status_flags.fault_status_bad_sideslip = _ekf.fault_status_flags().bad_sideslip; + status_flags.fault_status_bad_optflow_x = _ekf.fault_status_flags().bad_optflow_X; + status_flags.fault_status_bad_optflow_y = _ekf.fault_status_flags().bad_optflow_Y; + status_flags.fault_status_bad_vel_n = _ekf.fault_status_flags().bad_vel_N; + status_flags.fault_status_bad_vel_e = _ekf.fault_status_flags().bad_vel_E; + status_flags.fault_status_bad_vel_d = _ekf.fault_status_flags().bad_vel_D; + status_flags.fault_status_bad_pos_n = _ekf.fault_status_flags().bad_pos_N; + status_flags.fault_status_bad_pos_e = _ekf.fault_status_flags().bad_pos_E; + status_flags.fault_status_bad_pos_d = _ekf.fault_status_flags().bad_pos_D; + status_flags.fault_status_bad_acc_bias = _ekf.fault_status_flags().bad_acc_bias; + status_flags.fault_status_bad_acc_vertical = _ekf.fault_status_flags().bad_acc_vertical; + status_flags.fault_status_bad_acc_clipping = _ekf.fault_status_flags().bad_acc_clipping; + + status_flags.innovation_fault_status_changes = _innov_check_fail_status_changes; + status_flags.reject_hor_vel = _ekf.innov_check_fail_status_flags().reject_hor_vel; + status_flags.reject_ver_vel = _ekf.innov_check_fail_status_flags().reject_ver_vel; + status_flags.reject_hor_pos = _ekf.innov_check_fail_status_flags().reject_hor_pos; + status_flags.reject_ver_pos = _ekf.innov_check_fail_status_flags().reject_ver_pos; + status_flags.reject_mag_x = _ekf.innov_check_fail_status_flags().reject_mag_x; + status_flags.reject_mag_y = _ekf.innov_check_fail_status_flags().reject_mag_y; + status_flags.reject_mag_z = _ekf.innov_check_fail_status_flags().reject_mag_z; + status_flags.reject_yaw = _ekf.innov_check_fail_status_flags().reject_yaw; + status_flags.reject_airspeed = _ekf.innov_check_fail_status_flags().reject_airspeed; + status_flags.reject_sideslip = _ekf.innov_check_fail_status_flags().reject_sideslip; + status_flags.reject_hagl = _ekf.innov_check_fail_status_flags().reject_hagl; + 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(); + _estimator_status_flags_pub.publish(status_flags); + + _last_status_flag_update = status_flags.timestamp; + } +} + void EKF2::PublishYawEstimatorStatus(const hrt_abstime ×tamp) { static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF, diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index ef2d1295aa..2dc1ce866c 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -136,6 +137,7 @@ private: void PublishSensorBias(const hrt_abstime ×tamp); void PublishStates(const hrt_abstime ×tamp); void PublishStatus(const hrt_abstime ×tamp); + void PublishStatusFlags(const hrt_abstime ×tamp); void PublishWindEstimate(const hrt_abstime ×tamp); void PublishYawEstimatorStatus(const hrt_abstime ×tamp); @@ -228,6 +230,16 @@ private: bool _armed{false}; bool _standby{false}; // standby arming state + hrt_abstime _last_status_flag_update{0}; + + uint32_t _filter_control_status{0}; + uint32_t _filter_fault_status{0}; + uint32_t _innov_check_fail_status{0}; + + uint32_t _filter_control_status_changes{0}; + uint32_t _filter_fault_status_changes{0}; + uint32_t _innov_check_fail_status_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_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; @@ -237,6 +249,7 @@ private: uORB::PublicationMulti _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; uORB::PublicationMulti _estimator_states_pub{ORB_ID(estimator_states)}; uORB::PublicationMulti _estimator_status_pub{ORB_ID(estimator_status)}; + uORB::PublicationMulti _estimator_status_flags_pub{ORB_ID(estimator_status_flags)}; uORB::PublicationMulti _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)}; uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; uORB::PublicationMulti _wind_pub{ORB_ID(wind_estimate)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 8118a6ad7c..f5c3932164 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -126,6 +126,7 @@ void LoggedTopics::add_default_topics() add_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_status", 500, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_visual_odometry_aligned", 200, MAX_ESTIMATOR_INSTANCES); add_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("wind_estimate", 1000); // published by both ekf2 and airspeed_selector