fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 11:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: Publish and log EKF warning and information events (NEW msg estimator_event_flags)

Browse Source 
* msg: Add estinator information and warning events message (estimator_event_flags)
 * ekf2: publish information and warning events
 * logger: log estimator_event_flags
 * update ecl submodule to latest

Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
Paul Riseborough
2021-03-07 16:16:48 -05:00
committed by GitHub
co-authored by Daniel Agar
parent 18a8d89fa4
commit 4465c4fbf6
8 changed files with 105 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+61
View File
@@ -472,6 +472,7 @@ void EKF2::Run()
// publish status/logging messages
PublishEkfDriftMetrics(now);
PublishEventFlags(now);
PublishStates(now);
PublishStatus(now);
PublishStatusFlags(now);
@@ -540,6 +541,66 @@ void EKF2::PublishEkfDriftMetrics(const hrt_abstime &timestamp)
}
}
void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
{
// information events
uint32_t information_events = _ekf.information_event_status().value;
bool information_event_updated = false;
if (information_events != 0) {
information_event_updated = true;
_filter_information_event_changes++;
}
// warning events
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{};
event_flags.timestamp_sample = timestamp;
event_flags.information_event_changes = _filter_information_event_changes;
event_flags.gps_checks_passed = _ekf.information_event_flags().gps_checks_passed;
event_flags.reset_vel_to_gps = _ekf.information_event_flags().reset_vel_to_gps;
event_flags.reset_vel_to_flow = _ekf.information_event_flags().reset_vel_to_flow;
event_flags.reset_vel_to_vision = _ekf.information_event_flags().reset_vel_to_vision;
event_flags.reset_vel_to_zero = _ekf.information_event_flags().reset_vel_to_zero;
event_flags.reset_pos_to_last_known = _ekf.information_event_flags().reset_pos_to_last_known;
event_flags.reset_pos_to_gps = _ekf.information_event_flags().reset_pos_to_gps;
event_flags.reset_pos_to_vision = _ekf.information_event_flags().reset_pos_to_vision;
event_flags.starting_gps_fusion = _ekf.information_event_flags().starting_gps_fusion;
event_flags.starting_vision_pos_fusion = _ekf.information_event_flags().starting_vision_pos_fusion;
event_flags.starting_vision_vel_fusion = _ekf.information_event_flags().starting_vision_vel_fusion;
event_flags.starting_vision_yaw_fusion = _ekf.information_event_flags().starting_vision_yaw_fusion;
event_flags.yaw_aligned_to_imu_gps = _ekf.information_event_flags().yaw_aligned_to_imu_gps;
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.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_event_flags_pub.publish(event_flags);
}
_ekf.clear_information_events();
_ekf.clear_warning_events();
}
void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
{
if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) {