ekf2: remove warning events logging

- some of these warning flags aren't even being used, and most of the rest we can figure out from other sources
This commit is contained in:
Daniel Agar 2024-07-05 15:33:34 -04:00
parent c29b4ff87e
commit 75bb339d94
9 changed files with 2 additions and 84 deletions

View File

@ -20,18 +20,3 @@ bool reset_hgt_to_baro # 13 - true when the vertical position s
bool reset_hgt_to_gps # 14 - true when the vertical position state is reset to the gps measurement bool reset_hgt_to_gps # 14 - true when the vertical position state is reset to the gps measurement
bool reset_hgt_to_rng # 15 - true when the vertical position state is reset to the rng measurement bool reset_hgt_to_rng # 15 - true when the vertical position state is reset to the rng measurement
bool reset_hgt_to_ev # 16 - true when the vertical position state is reset to the ev measurement bool reset_hgt_to_ev # 16 - true when the vertical position state is reset to the ev measurement
# warning events
uint32 warning_event_changes # number of warning event changes
bool gps_quality_poor # 0 - true when the gps is failing quality checks
bool gps_fusion_timout # 1 - true when the gps data has not been used to correct the state estimates for a significant time period
bool gps_data_stopped # 2 - true when the gps data has stopped for a significant time period
bool gps_data_stopped_using_alternate # 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation
bool height_sensor_timeout # 4 - true when the height sensor has not been used to correct the state estimates for a significant time period
bool stopping_navigation # 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation
bool invalid_accel_bias_cov_reset # 6 - true when the filter has detected bad acceerometer bias state esitmstes and has reset the corresponding covariance matrix elements
bool bad_yaw_using_gps_course # 7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course
bool stopping_mag_use # 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data
bool vision_data_stopped # 9 - true when the vision system data has stopped for a significant time period
bool emergency_yaw_reset_mag_stopped # 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetometer data
bool emergency_yaw_reset_gps_yaw_stopped # 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data

View File

@ -83,7 +83,6 @@ void Ekf::controlExternalVisionFusion()
_ev_q_error_initialized = false; _ev_q_error_initialized = false;
_warning_events.flags.vision_data_stopped = true;
ECL_WARN("vision data stopped"); ECL_WARN("vision data stopped");
} }
} }

View File

@ -109,7 +109,6 @@ void Ekf::controlFakePosFusion()
if (_control_status.flags.tilt_align) { if (_control_status.flags.tilt_align) {
// The fake position fusion is not started for initial alignement // The fake position fusion is not started for initial alignement
_warning_events.flags.stopping_navigation = true;
ECL_WARN("stopping navigation"); ECL_WARN("stopping navigation");
} }
} }

View File

@ -78,7 +78,6 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
if (_control_status.flags.gps && isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) { if (_control_status.flags.gps && isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) {
stopGpsFusion(); stopGpsFusion();
_warning_events.flags.gps_quality_poor = true;
ECL_WARN("GPS quality poor - stopping use"); ECL_WARN("GPS quality poor - stopping use");
} }
} }
@ -92,7 +91,6 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
} else if (_control_status.flags.gps) { } else if (_control_status.flags.gps) {
if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) { if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) {
stopGpsFusion(); stopGpsFusion();
_warning_events.flags.gps_data_stopped = true;
ECL_WARN("GPS data stopped"); ECL_WARN("GPS data stopped");
} }
} }
@ -276,14 +274,12 @@ bool Ekf::tryYawEmergencyReset()
// stop using the magnetometer in the main EKF otherwise its fusion could drag the yaw around // stop using the magnetometer in the main EKF otherwise its fusion could drag the yaw around
// and cause another navigation failure // and cause another navigation failure
_control_status.flags.mag_fault = true; _control_status.flags.mag_fault = true;
_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
} }
#if defined(CONFIG_EKF2_GNSS_YAW) #if defined(CONFIG_EKF2_GNSS_YAW)
if (_control_status.flags.gps_yaw) { if (_control_status.flags.gps_yaw) {
_control_status.flags.gps_yaw_fault = true; _control_status.flags.gps_yaw_fault = true;
_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
} }
#endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS_YAW

View File

@ -666,34 +666,5 @@ bool reset_pos_to_ext_obs :
uint32_t value; uint32_t value;
}; };
// define structure used to communicate information events
union warning_event_status_u {
struct {
bool gps_quality_poor : 1; ///< 0 - true when the gps is failing quality checks
bool gps_fusion_timout :
1; ///< 1 - true when the gps data has not been used to correct the state estimates for a significant time period
bool gps_data_stopped : 1; ///< 2 - true when the gps data has stopped for a significant time period
bool gps_data_stopped_using_alternate :
1; ///< 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation
bool height_sensor_timeout :
1; ///< 4 - true when the height sensor has not been used to correct the state estimates for a significant time period
bool stopping_navigation :
1; ///< 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation
bool invalid_accel_bias_cov_reset :
1; ///< 6 - true when the filter has detected bad acceerometer bias state estimates and has reset the corresponding covariance matrix elements
bool bad_yaw_using_gps_course :
1; ///< 7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course
bool stopping_mag_use :
1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data
bool vision_data_stopped :
1; ///< 9 - true when the vision system data has stopped for a significant time period
bool emergency_yaw_reset_mag_stopped :
1; ///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetometer data
bool emergency_yaw_reset_gps_yaw_stopped:
1; ///< 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data
} flags;
uint32_t value;
};
} }
#endif // !EKF_COMMON_H #endif // !EKF_COMMON_H

View File

@ -293,10 +293,6 @@ public:
const innovation_fault_status_u &innov_check_fail_status() const { return _innov_check_fail_status; } 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 decltype(innovation_fault_status_u::flags) &innov_check_fail_status_flags() const { return _innov_check_fail_status.flags; }
const warning_event_status_u &warning_event_status() const { return _warning_events; }
const decltype(warning_event_status_u::flags) &warning_event_flags() const { return _warning_events.flags; }
void clear_warning_events() { _warning_events.value = 0; }
const information_event_status_u &information_event_status() const { return _information_events; } 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; } const decltype(information_event_status_u::flags) &information_event_flags() const { return _information_events.flags; }
void clear_information_events() { _information_events.value = 0; } void clear_information_events() { _information_events.value = 0; }
@ -473,7 +469,6 @@ protected:
// these are used to record single frame events for external monitoring and should NOT be used for // these are used to record single frame events for external monitoring and should NOT be used for
// state logic becasue they will be cleared externally after being read. // state logic becasue they will be cleared externally after being read.
warning_event_status_u _warning_events{};
information_event_status_u _information_events{}; information_event_status_u _information_events{};
unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec) unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)

View File

@ -216,7 +216,6 @@ void Ekf::checkVerticalAccelerationBias(const imuSample &imu_delayed)
_time_acc_bias_check = imu_delayed.time_us; _time_acc_bias_check = imu_delayed.time_us;
_fault_status.flags.bad_acc_bias = false; _fault_status.flags.bad_acc_bias = false;
_warning_events.flags.invalid_accel_bias_cov_reset = true;
ECL_WARN("invalid accel bias - covariance reset"); ECL_WARN("invalid accel bias - covariance reset");
} }
} }

View File

@ -1107,16 +1107,7 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
_filter_information_event_changes++; _filter_information_event_changes++;
} }
// warning events if (information_event_updated) {
uint32_t warning_events = _ekf.warning_event_status().value;
bool warning_event_updated = false;
if (warning_events != 0) {
warning_event_updated = true;
_filter_warning_event_changes++;
}
if (information_event_updated || warning_event_updated) {
estimator_event_flags_s event_flags{}; estimator_event_flags_s event_flags{};
event_flags.timestamp_sample = _ekf.time_delayed_us(); event_flags.timestamp_sample = _ekf.time_delayed_us();
@ -1139,27 +1130,12 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
event_flags.reset_hgt_to_rng = _ekf.information_event_flags().reset_hgt_to_rng; event_flags.reset_hgt_to_rng = _ekf.information_event_flags().reset_hgt_to_rng;
event_flags.reset_hgt_to_ev = _ekf.information_event_flags().reset_hgt_to_ev; event_flags.reset_hgt_to_ev = _ekf.information_event_flags().reset_hgt_to_ev;
event_flags.warning_event_changes = _filter_warning_event_changes;
event_flags.gps_quality_poor = _ekf.warning_event_flags().gps_quality_poor;
event_flags.gps_fusion_timout = _ekf.warning_event_flags().gps_fusion_timout;
event_flags.gps_data_stopped = _ekf.warning_event_flags().gps_data_stopped;
event_flags.gps_data_stopped_using_alternate = _ekf.warning_event_flags().gps_data_stopped_using_alternate;
event_flags.height_sensor_timeout = _ekf.warning_event_flags().height_sensor_timeout;
event_flags.stopping_navigation = _ekf.warning_event_flags().stopping_mag_use;
event_flags.invalid_accel_bias_cov_reset = _ekf.warning_event_flags().invalid_accel_bias_cov_reset;
event_flags.bad_yaw_using_gps_course = _ekf.warning_event_flags().bad_yaw_using_gps_course;
event_flags.stopping_mag_use = _ekf.warning_event_flags().stopping_mag_use;
event_flags.vision_data_stopped = _ekf.warning_event_flags().vision_data_stopped;
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 = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_event_flags_pub.update(event_flags); _estimator_event_flags_pub.update(event_flags);
_last_event_flags_publish = event_flags.timestamp; _last_event_flags_publish = event_flags.timestamp;
_ekf.clear_information_events(); _ekf.clear_information_events();
_ekf.clear_warning_events();
} else if ((_last_event_flags_publish != 0) && (timestamp >= _last_event_flags_publish + 1_s)) { } else if ((_last_event_flags_publish != 0) && (timestamp >= _last_event_flags_publish + 1_s)) {
// continue publishing periodically // continue publishing periodically
@ -2676,8 +2652,7 @@ void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
&& (_ekf.fault_status().value == 0) && (_ekf.fault_status().value == 0)
&& !_ekf.fault_status_flags().bad_acc_bias && !_ekf.fault_status_flags().bad_acc_bias
&& !_ekf.fault_status_flags().bad_acc_clipping && !_ekf.fault_status_flags().bad_acc_clipping
&& !_ekf.fault_status_flags().bad_acc_vertical && !_ekf.fault_status_flags().bad_acc_vertical;
&& !_ekf.warning_event_flags().invalid_accel_bias_cov_reset;
const bool learning_valid = bias_valid && !_ekf.accel_bias_inhibited(); const bool learning_valid = bias_valid && !_ekf.accel_bias_inhibited();

View File

@ -418,7 +418,6 @@ private:
uint32_t _filter_control_status_changes{0}; uint32_t _filter_control_status_changes{0};
uint32_t _filter_fault_status_changes{0}; uint32_t _filter_fault_status_changes{0};
uint32_t _innov_check_fail_status_changes{0}; uint32_t _innov_check_fail_status_changes{0};
uint32_t _filter_warning_event_changes{0};
uint32_t _filter_information_event_changes{0}; uint32_t _filter_information_event_changes{0};
uORB::PublicationMulti<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; uORB::PublicationMulti<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};