fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

estimator_status_flags: remove useless logged flags

Browse Source 
Those flags are not so useful for log analysis and can be found in the
aid_src topics
This commit is contained in:
bresch 2026-01-22 10:16:28 +01:00 committed by Niklas Hauser
parent 52308735a7
commit 230276540f
14 changed files with 0 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
msg/EstimatorStatusFlags.msg
View File

@ -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

3
src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp
View File

@ -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;

2
src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp
View File

@ -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)

5
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
View File

@ -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)

10
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
View File

@ -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;
}
}

5
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
View File

@ -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;

4
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_fusion.cpp
View File

@ -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;

1
src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp
View File

@ -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;

20
src/modules/ekf2/EKF/common.h
View File

@ -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 {

1
src/modules/ekf2/EKF/ekf.cpp
View File

@ -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();

5
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -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};

5
src/modules/ekf2/EKF/yaw_fusion.cpp
View File

@ -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);

19
src/modules/ekf2/EKF2.cpp
View File

@ -1898,13 +1898,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
_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 &timestamp)
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);

2
src/modules/ekf2/EKF2.hpp
View File

@ -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_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};