From 591b7b6934e32c5459a9e5507cf1d33e4e4eabd7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 18 Feb 2022 11:56:43 -0500 Subject: [PATCH] ekf2: add new estimator_gps_status.msg - includes the estimator status check fail bits broken out as descriptive booleans - absorbs ekf_gps_drift.msg --- .ci/Jenkinsfile-hardware | 2 +- msg/CMakeLists.txt | 2 +- msg/ekf_gps_drift.msg | 6 --- msg/estimator_gps_status.msg | 19 +++++++ src/modules/ekf2/EKF/ekf.h | 8 +-- src/modules/ekf2/EKF/ekf_helper.cpp | 25 ++------- src/modules/ekf2/EKF/estimator_interface.h | 17 +++--- src/modules/ekf2/EKF/gps_checks.cpp | 17 +++--- src/modules/ekf2/EKF2.cpp | 63 +++++++++++++--------- src/modules/ekf2/EKF2.hpp | 11 ++-- src/modules/logger/logged_topics.cpp | 6 +-- 11 files changed, 93 insertions(+), 83 deletions(-) delete mode 100644 msg/ekf_gps_drift.msg create mode 100644 msg/estimator_gps_status.msg diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index 210a54b674..7c8408332e 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -861,7 +861,7 @@ void printTopics() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf2_timestamps" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf_gps_drift" || true' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_gps_status" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_baro_bias" || true' diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 07939461d4..433c36b08f 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -64,7 +64,7 @@ set(msg_files differential_pressure.msg distance_sensor.msg ekf2_timestamps.msg - ekf_gps_drift.msg + estimator_gps_status.msg esc_report.msg esc_status.msg estimator_baro_bias.msg diff --git a/msg/ekf_gps_drift.msg b/msg/ekf_gps_drift.msg deleted file mode 100644 index 68163d7910..0000000000 --- a/msg/ekf_gps_drift.msg +++ /dev/null @@ -1,6 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -float32 hpos_drift_rate # Horizontal position rate magnitude checked using EKF2_REQ_HDRIFT (m/s) -float32 vpos_drift_rate # Vertical position rate magnitude checked using EKF2_REQ_VDRIFT (m/s) -float32 hspd # Filtered horizontal velocity magnitude checked using EKF2_REQ_HDRIFT (m/s) - -bool blocked # true when drift calculation is blocked due to IMU movement check diff --git a/msg/estimator_gps_status.msg b/msg/estimator_gps_status.msg new file mode 100644 index 0000000000..2d2462ee5c --- /dev/null +++ b/msg/estimator_gps_status.msg @@ -0,0 +1,19 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +bool checks_passed + +bool check_fail_gps_fix # 0 : insufficient fix type (no 3D solution) +bool check_fail_min_sat_count # 1 : minimum required sat count fail +bool check_fail_max_pdop # 2 : maximum allowed PDOP fail +bool check_fail_max_horz_err # 3 : maximum allowed horizontal position error fail +bool check_fail_max_vert_err # 4 : maximum allowed vertical position error fail +bool check_fail_max_spd_err # 5 : maximum allowed speed error fail +bool check_fail_max_horz_drift # 6 : maximum allowed horizontal position drift fail - requires stationary vehicle +bool check_fail_max_vert_drift # 7 : maximum allowed vertical position drift fail - requires stationary vehicle +bool check_fail_max_horz_spd_err # 8 : maximum allowed horizontal speed fail - requires stationary vehicle +bool check_fail_max_vert_spd_err # 9 : maximum allowed vertical velocity discrepancy fail + +float32 position_drift_rate_horizontal_m_s # Horizontal position rate magnitude (m/s) +float32 position_drift_rate_vertical_m_s # Vertical position rate magnitude (m/s) +float32 filtered_horizontal_speed_m_s # Filtered horizontal velocity magnitude (m/s) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 27e26e9c17..a895c43227 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -246,9 +246,6 @@ public: bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; } - // get GPS check status - void get_gps_check_status(uint16_t *val) const { *val = _gps_check_fail_status.value; } - const auto &state_reset_status() const { return _state_reset_status; } // return the amount the local vertical position changed in the last reset and the number of reset events @@ -311,6 +308,11 @@ public: // set minimum continuous period without GPS fail required to mark a healthy GPS status void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; } + const gps_check_fail_status_u &gps_check_fail_status() const { return _gps_check_fail_status; } + const decltype(gps_check_fail_status_u::flags) &gps_check_fail_status_flags() const { return _gps_check_fail_status.flags; } + + bool gps_checks_passed() const { return _gps_checks_passed; }; + // get solution data from the EKF-GSF emergency yaw estimator // returns false when data is not available bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index ee10288437..9867fdb5e8 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -750,27 +750,6 @@ void Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons } } -/* - First argument returns GPS drift metrics in the following array locations - 0 : Horizontal position drift rate (m/s) - 1 : Vertical position drift rate (m/s) - 2 : Filtered horizontal velocity (m/s) - Second argument returns true when IMU movement is blocking the drift calculation - Function returns true if the metrics have been updated and not returned previously by this function -*/ -bool Ekf::get_gps_drift_metrics(float drift[3], bool *blocked) -{ - memcpy(drift, _gps_drift_metrics, 3 * sizeof(float)); - *blocked = !_control_status.flags.vehicle_at_rest; - - if (_gps_drift_updated) { - _gps_drift_updated = false; - return true; - } - - return false; -} - // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const { @@ -1823,4 +1802,8 @@ void Ekf::resetGpsDriftCheckFilters() { _gps_velNE_filt.setZero(); _gps_pos_deriv_filt.setZero(); + + _gps_horizontal_position_drift_rate_m_s = NAN; + _gps_vertical_position_drift_rate_m_s = NAN; + _gps_filtered_horizontal_velocity_m_s = NAN; } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index b394784d4f..33f6b711e3 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -245,17 +245,20 @@ public: // Getter for the average EKF update period in s float get_dt_ekf_avg() const { return _dt_ekf_avg; } - // Getter for the imu sample on the delayed time horizon + // Getters for samples on the delayed time horizon const imuSample &get_imu_sample_delayed() const { return _imu_sample_delayed; } - - // Getter for the baro sample on the delayed time horizon const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; } + const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } const bool &global_origin_valid() const { return _NED_origin_initialised; } const MapProjection &global_origin() const { return _pos_ref; } void print_status(); + float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; } + float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; } + float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; } + protected: EstimatorInterface() = default; @@ -346,14 +349,12 @@ protected: bool _deadreckon_time_exceeded{true}; // true if the horizontal nav solution has been deadreckoning for too long and is invalid bool _is_wind_dead_reckoning{false}; // true if we are navigationg reliant on wind relative measurements - float _gps_drift_metrics[3] {}; // Array containing GPS drift metrics - // [0] Horizontal position drift rate (m/s) - // [1] Vertical position drift rate (m/s) - // [2] Filtered horizontal velocity (m/s) + float _gps_horizontal_position_drift_rate_m_s{0}; // Horizontal position drift rate (m/s) + float _gps_vertical_position_drift_rate_m_s{0}; // Vertical position drift rate (m/s) + float _gps_filtered_horizontal_velocity_m_s{0}; // Filtered horizontal velocity (m/s) uint64_t _time_last_on_ground_us{0}; ///< last time we were on the ground (uSec) uint64_t _time_last_in_air{0}; ///< last time we were in air (uSec) - bool _gps_drift_updated{false}; // true when _gps_drift_metrics has been updated and is ready for retrieval // data buffer instances RingBuffer _imu_buffer{12}; // buffer length 12 with default parameters diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index 1c3527cc15..9abb3bed1a 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -201,22 +201,20 @@ bool Ekf::gps_is_good(const gps_message &gps) _gps_pos_deriv_filt = pos_derived * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef); // Calculate the horizontal drift speed and fail if too high - _gps_drift_metrics[0] = Vector2f(_gps_pos_deriv_filt.xy()).norm(); - _gps_check_fail_status.flags.hdrift = (_gps_drift_metrics[0] > _params.req_hdrift); + _gps_horizontal_position_drift_rate_m_s = Vector2f(_gps_pos_deriv_filt.xy()).norm(); + _gps_check_fail_status.flags.hdrift = (_gps_horizontal_position_drift_rate_m_s > _params.req_hdrift); // Fail if the vertical drift speed is too high - _gps_drift_metrics[1] = fabsf(_gps_pos_deriv_filt(2)); - _gps_check_fail_status.flags.vdrift = (_gps_drift_metrics[1] > _params.req_vdrift); + _gps_vertical_position_drift_rate_m_s = fabsf(_gps_pos_deriv_filt(2)); + _gps_check_fail_status.flags.vdrift = (_gps_vertical_position_drift_rate_m_s > _params.req_vdrift); // Check the magnitude of the filtered horizontal GPS velocity const Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel_ned.xy()), -10.0f * _params.req_hdrift, 10.0f * _params.req_hdrift); _gps_velNE_filt = gps_velNE * filter_coef + _gps_velNE_filt * (1.0f - filter_coef); - _gps_drift_metrics[2] = _gps_velNE_filt.norm(); - _gps_check_fail_status.flags.hspeed = (_gps_drift_metrics[2] > _params.req_hdrift); - - _gps_drift_updated = true; + _gps_filtered_horizontal_velocity_m_s = _gps_velNE_filt.norm(); + _gps_check_fail_status.flags.hspeed = (_gps_filtered_horizontal_velocity_m_s > _params.req_hdrift); } else if (_control_status.flags.in_air) { // These checks are always declared as passed when flying @@ -224,14 +222,11 @@ bool Ekf::gps_is_good(const gps_message &gps) _gps_check_fail_status.flags.hdrift = false; _gps_check_fail_status.flags.vdrift = false; _gps_check_fail_status.flags.hspeed = false; - _gps_drift_updated = false; resetGpsDriftCheckFilters(); } else { // This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation - _gps_drift_updated = true; - resetGpsDriftCheckFilters(); } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 5ae684d7af..a32e7b0c5e 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -191,17 +191,17 @@ bool EKF2::multi_init(int imu, int mag) _wind_pub.advertise(); _ekf2_timestamps_pub.advertise(); - _ekf_gps_drift_pub.advertise(); _estimator_baro_bias_pub.advertise(); _estimator_event_flags_pub.advertise(); + _estimator_gps_status_pub.advertise(); _estimator_innovation_test_ratios_pub.advertise(); _estimator_innovation_variances_pub.advertise(); _estimator_innovations_pub.advertise(); _estimator_optical_flow_vel_pub.advertise(); _estimator_sensor_bias_pub.advertise(); _estimator_states_pub.advertise(); - _estimator_status_pub.advertise(); _estimator_status_flags_pub.advertise(); + _estimator_status_pub.advertise(); _estimator_visual_odometry_aligned_pub.advertised(); _yaw_est_pub.advertise(); @@ -573,8 +573,8 @@ void EKF2::Run() // publish status/logging messages PublishBaroBias(now); - PublishEkfDriftMetrics(now); PublishEventFlags(now); + PublishGpsStatus(now); PublishInnovations(now); PublishInnovationTestRatios(now); PublishInnovationVariances(now); @@ -647,24 +647,6 @@ void EKF2::PublishBaroBias(const hrt_abstime ×tamp) } } -void EKF2::PublishEkfDriftMetrics(const hrt_abstime ×tamp) -{ - // publish GPS drift data only when updated to minimise overhead - float gps_drift[3]; - bool blocked; - - if (_ekf.get_gps_drift_metrics(gps_drift, &blocked)) { - ekf_gps_drift_s drift_data; - drift_data.hpos_drift_rate = gps_drift[0]; - drift_data.vpos_drift_rate = gps_drift[1]; - drift_data.hspd = gps_drift[2]; - drift_data.blocked = blocked; - drift_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); - - _ekf_gps_drift_pub.publish(drift_data); - } -} - void EKF2::PublishEventFlags(const hrt_abstime ×tamp) { // information events @@ -768,6 +750,41 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) } } +void EKF2::PublishGpsStatus(const hrt_abstime ×tamp) +{ + const hrt_abstime timestamp_sample = _ekf.get_gps_sample_delayed().time_us; + + if (timestamp_sample == _last_gps_status_published) { + return; + } + + estimator_gps_status_s estimator_gps_status{}; + estimator_gps_status.timestamp_sample = timestamp_sample; + + estimator_gps_status.position_drift_rate_horizontal_m_s = _ekf.gps_horizontal_position_drift_rate_m_s(); + estimator_gps_status.position_drift_rate_vertical_m_s = _ekf.gps_vertical_position_drift_rate_m_s(); + estimator_gps_status.filtered_horizontal_speed_m_s = _ekf.gps_filtered_horizontal_velocity_m_s(); + + estimator_gps_status.checks_passed = _ekf.gps_checks_passed(); + + estimator_gps_status.check_fail_gps_fix = _ekf.gps_check_fail_status_flags().fix; + estimator_gps_status.check_fail_min_sat_count = _ekf.gps_check_fail_status_flags().nsats; + estimator_gps_status.check_fail_max_pdop = _ekf.gps_check_fail_status_flags().pdop; + estimator_gps_status.check_fail_max_horz_err = _ekf.gps_check_fail_status_flags().hacc; + estimator_gps_status.check_fail_max_vert_err = _ekf.gps_check_fail_status_flags().vacc; + estimator_gps_status.check_fail_max_spd_err = _ekf.gps_check_fail_status_flags().sacc; + estimator_gps_status.check_fail_max_horz_drift = _ekf.gps_check_fail_status_flags().hdrift; + estimator_gps_status.check_fail_max_vert_drift = _ekf.gps_check_fail_status_flags().vdrift; + estimator_gps_status.check_fail_max_horz_spd_err = _ekf.gps_check_fail_status_flags().hspeed; + estimator_gps_status.check_fail_max_vert_spd_err = _ekf.gps_check_fail_status_flags().vspeed; + + estimator_gps_status.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _estimator_gps_status_pub.publish(estimator_gps_status); + + + _last_gps_status_published = timestamp_sample; +} + void EKF2::PublishInnovations(const hrt_abstime ×tamp) { // publish estimator innovation data @@ -1152,11 +1169,9 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) _ekf.getOutputTrackingError().copyTo(status.output_tracking_error); - _ekf.get_gps_check_status(&status.gps_check_fail_flags); - // only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include // the GPS Fix bit, which is always checked) - status.gps_check_fail_flags &= ((uint16_t)_params->gps_check_mask << 1) | 1; + status.gps_check_fail_flags = _ekf.gps_check_fail_status().value & (((uint16_t)_params->gps_check_mask << 1) | 1); status.control_mode_flags = _ekf.control_status().value; status.filter_fault_flags = _ekf.fault_status().value; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index b508036ff5..72341e33b5 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -67,9 +67,9 @@ #include #include #include -#include #include #include +#include #include #include #include @@ -137,9 +137,9 @@ private: void PublishAttitude(const hrt_abstime ×tamp); void PublishBaroBias(const hrt_abstime ×tamp); - void PublishEkfDriftMetrics(const hrt_abstime ×tamp); void PublishEventFlags(const hrt_abstime ×tamp); void PublishGlobalPosition(const hrt_abstime ×tamp); + void PublishGpsStatus(const hrt_abstime ×tamp); void PublishInnovations(const hrt_abstime ×tamp); void PublishInnovationTestRatios(const hrt_abstime ×tamp); void PublishInnovationVariances(const hrt_abstime ×tamp); @@ -238,6 +238,7 @@ private: Vector3f _last_mag_calibration_published{}; hrt_abstime _last_sensor_bias_published{0}; + hrt_abstime _last_gps_status_published{0}; float _last_baro_bias_published{}; @@ -285,17 +286,17 @@ private: uint32_t _filter_information_event_changes{0}; uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; - uORB::PublicationMulti _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; uORB::PublicationMulti _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)}; + uORB::PublicationMulti _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; + uORB::PublicationMulti _estimator_gps_status_pub{ORB_ID(estimator_gps_status)}; uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; uORB::PublicationMulti _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; uORB::PublicationMulti _estimator_innovations_pub{ORB_ID(estimator_innovations)}; uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; uORB::PublicationMulti _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; uORB::PublicationMulti _estimator_states_pub{ORB_ID(estimator_states)}; - uORB::PublicationMulti _estimator_status_pub{ORB_ID(estimator_status)}; uORB::PublicationMulti _estimator_status_flags_pub{ORB_ID(estimator_status_flags)}; - uORB::PublicationMulti _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; + uORB::PublicationMulti _estimator_status_pub{ORB_ID(estimator_status)}; uORB::PublicationMulti _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)}; uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index e7d6d8fb21..69e7495495 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -138,9 +138,9 @@ void LoggedTopics::add_default_topics() #endif // always add the first instance - add_topic("ekf_gps_drift", 1000); add_topic("estimator_baro_bias", 500); add_topic("estimator_event_flags", 0); + add_topic("estimator_gps_status", 1000); add_topic("estimator_innovation_test_ratios", 500); add_topic("estimator_innovation_variances", 500); add_topic("estimator_innovations", 500); @@ -152,9 +152,9 @@ void LoggedTopics::add_default_topics() add_topic("estimator_visual_odometry_aligned", 200); add_topic("yaw_estimator_status", 1000); - add_optional_topic_multi("ekf_gps_drift", 1000, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_gps_status", 1000, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_innovations", 500, MAX_ESTIMATOR_INSTANCES); @@ -194,9 +194,9 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_local_position_groundtruth", 100); // EKF replay - add_topic("ekf_gps_drift"); add_topic("estimator_baro_bias"); add_topic("estimator_event_flags"); + add_topic("estimator_gps_status"); add_topic("estimator_innovation_test_ratios"); add_topic("estimator_innovation_variances"); add_topic("estimator_innovations");