From 230276540f89bd8d361a1deeed3e1ba4fd04c227 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 22 Jan 2026 10:16:28 +0100 Subject: [PATCH] estimator_status_flags: remove useless logged flags Those flags are not so useful for log analysis and can be found in the aid_src topics --- msg/EstimatorStatusFlags.msg | 14 ------------- .../aid_sources/airspeed/airspeed_fusion.cpp | 3 --- .../EKF/aid_sources/gnss/gnss_yaw_control.cpp | 2 -- .../aid_sources/magnetometer/mag_control.cpp | 5 ----- .../optical_flow/optical_flow_control.cpp | 10 ---------- .../optical_flow/optical_flow_fusion.cpp | 5 ----- .../range_finder/range_height_fusion.cpp | 4 ---- .../aid_sources/sideslip/sideslip_fusion.cpp | 1 - src/modules/ekf2/EKF/common.h | 20 ------------------- src/modules/ekf2/EKF/ekf.cpp | 1 - src/modules/ekf2/EKF/estimator_interface.h | 5 ----- src/modules/ekf2/EKF/yaw_fusion.cpp | 5 ----- src/modules/ekf2/EKF2.cpp | 19 ------------------ src/modules/ekf2/EKF2.hpp | 2 -- 14 files changed, 96 deletions(-) diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 3f8435b6e5..6de62f2c06 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -66,17 +66,3 @@ bool fs_bad_optflow_x # 7 - true if fusion of the optical flow X axis h bool fs_bad_optflow_y # 8 - true if fusion of the optical flow Y axis has encountered a numerical error bool fs_bad_acc_vertical # 10 - true if bad vertical accelerometer data has been detected bool fs_bad_acc_clipping # 11 - true if delta velocity data contains clipping (asymmetric railing) - - -# innovation test failures -uint32 innovation_fault_status_changes # number of innovation fault status (reject) 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_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/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp index baaa8079d5..49d5b4f396 100644 --- a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -93,9 +93,6 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) updateAirspeed(airspeed_sample, _aid_src_airspeed); - _innov_check_fail_status.flags.reject_airspeed = - _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag - const bool continuing_conditions_passing = _control_status.flags.in_air && (_control_status.flags.fixed_wing || _control_status.flags.in_transition_to_fw) && !_control_status.flags.fake_pos; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp index a0d2c4cb66..709fb76211 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp @@ -159,7 +159,6 @@ void Ekf::fuseGnssYaw(float antenna_yaw_offset) auto &aid_src = _aid_src_gnss_yaw; if (aid_src.innovation_rejected) { - _innov_check_fail_status.flags.reject_yaw = true; return; } @@ -189,7 +188,6 @@ void Ekf::fuseGnssYaw(float antenna_yaw_offset) } _fault_status.flags.bad_hdg = false; - _innov_check_fail_status.flags.reject_yaw = false; if ((fabsf(aid_src.test_ratio_filtered) > 0.2f) && !_control_status.flags.in_air && isTimedOut(aid_src.time_last_fuse, (uint64_t)1e6) diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index d03229f7c2..6fdb679c8b 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -149,11 +149,6 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) innov_var, // innovation variance math::max(_params.ekf2_mag_gate, 1.f)); // innovation gate - // Perform an innovation consistency check and report the result - _innov_check_fail_status.flags.reject_mag_x = (aid_src.test_ratio[0] > 1.f); - _innov_check_fail_status.flags.reject_mag_y = (aid_src.test_ratio[1] > 1.f); - _innov_check_fail_status.flags.reject_mag_z = (aid_src.test_ratio[2] > 1.f); - // determine if we should use mag fusion bool continuing_conditions_passing = ((_params.ekf2_mag_type == MagFuseType::INIT) || (_params.ekf2_mag_type == MagFuseType::AUTO) diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index 7774147610..fa6f70ab4e 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -233,9 +233,6 @@ void Ekf::resetFlowFusion(const flowSample &flow_sample) resetHorizontalVelocityTo(getFilteredFlowVelNE(), flow_vel_var); resetAidSourceStatusZeroInnovation(_aid_src_optical_flow); - - _innov_check_fail_status.flags.reject_optflow_X = false; - _innov_check_fail_status.flags.reject_optflow_Y = false; } void Ekf::resetTerrainToFlow() @@ -265,10 +262,6 @@ void Ekf::resetTerrainToFlow() resetAidSourceStatusZeroInnovation(_aid_src_optical_flow); - _innov_check_fail_status.flags.reject_optflow_X = false; - _innov_check_fail_status.flags.reject_optflow_Y = false; - - // record the state change if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) { _state_reset_status.hagl_change = delta_terrain; @@ -291,9 +284,6 @@ void Ekf::stopFlowFusion() _fault_status.flags.bad_optflow_X = false; _fault_status.flags.bad_optflow_Y = false; - _innov_check_fail_status.flags.reject_optflow_X = false; - _innov_check_fail_status.flags.reject_optflow_Y = false; - _flow_counter = 0; } } diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp index 8e193cc46b..e2c72be9be 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp @@ -48,8 +48,6 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain) // if either axis fails we abort the fusion if (_aid_src_optical_flow.innovation_rejected) { - _innov_check_fail_status.flags.reject_optflow_X = true; - _innov_check_fail_status.flags.reject_optflow_Y = true; return false; } @@ -100,9 +98,6 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain) _fault_status.flags.bad_optflow_X = false; _fault_status.flags.bad_optflow_Y = false; - _innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f); - _innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f); - _aid_src_optical_flow.time_last_fuse = _time_delayed_us; _aid_src_optical_flow.fused = true; diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_fusion.cpp index c2fd636720..2703e48259 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_fusion.cpp @@ -37,7 +37,6 @@ bool Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain) { if (aid_src.innovation_rejected) { - _innov_check_fail_status.flags.reject_hagl = true; return false; } @@ -60,9 +59,6 @@ bool Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, boo measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation); - // record last successful fusion event - _innov_check_fail_status.flags.reject_hagl = false; - aid_src.time_last_fuse = _time_delayed_us; aid_src.fused = true; diff --git a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp index 0684f164ca..48e5cb7670 100644 --- a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp @@ -62,7 +62,6 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed) if (beta_fusion_time_triggered) { updateSideslip(_aid_src_sideslip); - _innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected; if (fuseSideslip(_aid_src_sideslip)) { _control_status.flags.wind = true; diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index ad950dda77..982cb4c1af 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -527,26 +527,6 @@ bool bad_sideslip : uint32_t value; }; -// define structure used to communicate innovation test failures -union innovation_fault_status_u { - struct { - bool reject_hor_vel : 1; ///< 0 - true if horizontal velocity observations have been rejected - bool reject_ver_vel : 1; ///< 1 - true if vertical velocity observations have been rejected - bool reject_hor_pos : 1; ///< 2 - true if horizontal position observations have been rejected - bool reject_ver_pos : 1; ///< 3 - true if true if vertical position observations have been rejected - bool reject_mag_x : 1; ///< 4 - true if the X magnetometer observation has been rejected - bool reject_mag_y : 1; ///< 5 - true if the Y magnetometer observation has been rejected - bool reject_mag_z : 1; ///< 6 - true if the Z magnetometer observation has been rejected - bool reject_yaw : 1; ///< 7 - true if the yaw observation has been rejected - bool reject_airspeed : 1; ///< 8 - true if the airspeed observation has been rejected - bool reject_sideslip : 1; ///< 9 - true if the synthetic sideslip observation has been rejected - bool reject_hagl : 1; ///< 10 - unused - bool reject_optflow_X : 1; ///< 11 - true if the X optical flow observation has been rejected - bool reject_optflow_Y : 1; ///< 12 - true if the Y optical flow observation has been rejected - } flags; - uint16_t value; -}; - // bitmask containing filter control status union filter_control_status_u { struct { diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index af63fc2b72..ac04d5e9c3 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -91,7 +91,6 @@ void Ekf::reset() _control_status_prev.flags.in_air = true; _fault_status.value = 0; - _innov_check_fail_status.value = 0; #if defined(CONFIG_EKF2_GNSS) _gnss_checks.resetHard(); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index b4bb3a0f9b..e67c558410 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -313,9 +313,6 @@ public: const fault_status_u &fault_status() const { return _fault_status; } const decltype(fault_status_u::flags) &fault_status_flags() const { return _fault_status.flags; } - const innovation_fault_status_u &innov_check_fail_status() const { return _innov_check_fail_status; } - const decltype(innovation_fault_status_u::flags) &innov_check_fail_status_flags() const { return _innov_check_fail_status.flags; } - const information_event_status_u &information_event_status() const { return _information_events; } const decltype(information_event_status_u::flags) &information_event_flags() const { return _information_events.flags; } void clear_information_events() { _information_events.value = 0; } @@ -433,8 +430,6 @@ protected: dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) #endif // CONFIG_EKF2_DRAG_FUSION - innovation_fault_status_u _innov_check_fail_status{}; - bool _horizontal_deadreckon_time_exceeded{true}; bool _vertical_position_deadreckon_time_exceeded{true}; bool _vertical_velocity_deadreckon_time_exceeded{true}; diff --git a/src/modules/ekf2/EKF/yaw_fusion.cpp b/src/modules/ekf2/EKF/yaw_fusion.cpp index 56bd1c1112..816d3f842a 100644 --- a/src/modules/ekf2/EKF/yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -75,8 +75,6 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H // set the heading unhealthy if the test fails if (aid_src_status.innovation_rejected) { - _innov_check_fail_status.flags.reject_yaw = true; - // if we are in air we don't want to fuse the measurement // we allow to use it when on the ground because the large innovation could be caused // by interference or a large initial gyro bias @@ -98,9 +96,6 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H } else { return false; } - - } else { - _innov_check_fail_status.flags.reject_yaw = false; } measurementUpdate(Kfusion, H_YAW, aid_src_status.observation_variance, aid_src_status.innovation); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index d959df31bc..4a3ddfc32f 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1898,13 +1898,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) _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 = _ekf.time_delayed_us(); @@ -1972,18 +1965,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.fs_bad_acc_vertical = _ekf.fault_status_flags().bad_acc_vertical; status_flags.fs_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_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); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 4c39ea1188..ff372df248 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -414,11 +414,9 @@ private: uint64_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}; uint32_t _filter_information_event_changes{0}; uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};