From 06702da0032d07ed31db3c6edcbca0672cf0dd21 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 8 Nov 2022 14:59:29 +0100 Subject: [PATCH] ekf2: add GNSS yaw max interval yaw data usually comes at lower rate than vel/pos --- src/modules/ekf2/EKF/common.h | 9 +++++---- src/modules/ekf2/EKF/control.cpp | 4 ++-- src/modules/ekf2/EKF/gnss_height_control.cpp | 4 ++-- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 6f484f3195..68334b4fc7 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -66,10 +66,11 @@ using math::Utilities::sq; using math::Utilities::updateYawInRotMat; // maximum sensor intervals in usec -#define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec) -#define EV_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec) -#define GPS_MAX_INTERVAL (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec) -#define RNG_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between range finder measurements (uSec) +#define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec) +#define EV_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec) +#define GNSS_MAX_INTERVAL (uint64_t)5e5 ///< Maximum allowable time interval between GNSS measurements (uSec) +#define GNSS_YAW_MAX_INTERVAL (uint64_t)15e5 ///< Maximum allowable time interval between GNSS yaw measurements (uSec) +#define RNG_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between range finder measurements (uSec) // bad accelerometer detection and mitigation #define BADACC_PROBATION (uint64_t)10e6 ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index b4dbd3b6be..3ae57ef44b 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -86,7 +86,7 @@ void Ekf::controlFusionModes() } if (_gps_buffer) { - _gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL); + _gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL); // check for arrival of new sensor data at the fusion time horizon _time_prev_gps_us = _gps_sample_delayed.time_us; @@ -418,7 +418,7 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi const bool continuing_conditions_passing = !gps_checks_failing; - const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GPS_MAX_INTERVAL); + const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL); const bool starting_conditions_passing = continuing_conditions_passing && _control_status.flags.tilt_align diff --git a/src/modules/ekf2/EKF/gnss_height_control.cpp b/src/modules/ekf2/EKF/gnss_height_control.cpp index ce13617e42..2c7c55e4c3 100644 --- a/src/modules/ekf2/EKF/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/gnss_height_control.cpp @@ -92,7 +92,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) && _gps_checks_passed; const bool starting_conditions_passing = continuing_conditions_passing - && isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL) + && isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL) && _gps_checks_passed && gps_checks_passing && !gps_checks_failing; @@ -158,7 +158,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) } } else if (_control_status.flags.gps_hgt - && !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL)) { + && !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL)) { // No data anymore. Stop until it comes back. ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME); stopGpsHgtFusion();