fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 19:43:33 +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()) {
src/modules/ekf2/EKF2.hpp
+5
View File
@@ -64,6 +64,7 @@
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/ekf_gps_drift.h>
#include <uORB/topics/estimator_event_flags.h>
#include <uORB/topics/estimator_innovations.h>
#include <uORB/topics/estimator_optical_flow_vel.h>
#include <uORB/topics/estimator_sensor_bias.h>
@@ -128,6 +129,7 @@ private:
void PublishAttitude(const hrt_abstime &timestamp);
void PublishEkfDriftMetrics(const hrt_abstime &timestamp);
void PublishEventFlags(const hrt_abstime &timestamp);
void PublishGlobalPosition(const hrt_abstime &timestamp);
void PublishInnovations(const hrt_abstime &timestamp, const imuSample &imu);
void PublishInnovationTestRatios(const hrt_abstime &timestamp);
@@ -243,6 +245,8 @@ private:
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_warning_event_changes{0};
uint32_t _filter_information_event_changes{0};
uORB::PublicationMulti<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
uORB::PublicationMulti<ekf_gps_drift_s> _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
@@ -254,6 +258,7 @@ private:
uORB::PublicationMulti<estimator_states_s> _estimator_states_pub{ORB_ID(estimator_states)};
uORB::PublicationMulti<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
uORB::PublicationMulti<estimator_status_flags_s> _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
uORB::PublicationMulti<estimator_event_flags_s> _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
uORB::PublicationMulti<vehicle_odometry_s> _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};