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