mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
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:
parent
c29b4ff87e
commit
75bb339d94
@ -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
|
|
||||||
|
|||||||
@ -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");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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)
|
||||||
|
|||||||
@ -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");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1107,16 +1107,7 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp)
|
|||||||
_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 ×tamp)
|
|||||||
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 ×tamp)
|
|||||||
&& (_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();
|
||||||
|
|
||||||
|
|||||||
@ -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)};
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user