dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

* 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-08 08:16:48 +11:00 committed by GitHub
parent 18a8d89fa4
commit 4465c4fbf6
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 105 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.ci/Jenkinsfile-hardware
View File

@ -1004,6 +1004,7 @@ void statusFTDI() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener battery_status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_event_flags"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_local_position"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_selector_status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_sensor_bias"'

1
msg/CMakeLists.txt
View File

@ -57,6 +57,7 @@ set(msg_files
ekf_gps_drift.msg
esc_report.msg
esc_status.msg
estimator_event_flags.msg
estimator_innovations.msg
estimator_optical_flow_vel.msg
estimator_selector_status.msg

32
msg/estimator_event_flags.msg Normal file
View File

@ -0,0 +1,32 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
# information events
uint32 information_event_changes # number of information event changes
bool gps_checks_passed # 0 - true when gps quality checks are passing passed
bool reset_vel_to_gps # 1 - true when the velocity states are reset to the gps measurement
bool reset_vel_to_flow # 2 - true when the velocity states are reset using the optical flow measurement
bool reset_vel_to_vision # 3 - true when the velocity states are reset to the vision system measurement
bool reset_vel_to_zero # 4 - true when the velocity states are reset to zero
bool reset_pos_to_last_known # 5 - true when the position states are reset to the last known position
bool reset_pos_to_gps # 6 - true when the position states are reset to the gps measurement
bool reset_pos_to_vision # 7 - true when the position states are reset to the vision system measurement
bool starting_gps_fusion # 8 - true when the filter starts using gps measurements to correct the state estimates
bool starting_vision_pos_fusion # 9 - true when the filter starts using vision system position measurements to correct the state estimates
bool starting_vision_vel_fusion # 10 - true when the filter starts using vision system velocity measurements to correct the state estimates
bool starting_vision_yaw_fusion # 11 - true when the filter starts using vision system yaw measurements to correct the state estimates
bool yaw_aligned_to_imu_gps # 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data
# 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 fiter has detected an invalid yaw esitmate 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 magnetomer 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 magnetomer data

2
msg/tools/uorb_rtps_message_ids.yaml
View File

@ -331,6 +331,8 @@ rtps:
id: 157
- msg: airspeed_wind
id: 158
- msg: estimator_event_flags
id: 159
########## multi topics: begin ##########
- msg: actuator_controls_0
id: 170

2
src/lib/ecl

@ -1 +1 @@
Subproject commit 098d5ce5c034f630b1b440a02ba78a13d092b8c7
Subproject commit 4bad2a272cfbcfd2ef2c77bb4af788f5b569aaaa

61
src/modules/ekf2/EKF2.cpp
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()) {

5
src/modules/ekf2/EKF2.hpp
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)};

3
src/modules/logger/logged_topics.cpp
View File

@ -140,12 +140,13 @@ void LoggedTopics::add_default_topics()
add_topic("estimator_selector_status");
add_topic_multi("ekf_gps_drift", 1000, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_innovations", 500, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES);