From 5332010b13d7cd9f1c6b5a4cda420bbd13e05763 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 21 May 2025 15:20:29 +0200 Subject: [PATCH] ekf2: move on ground GNSS checks to separate function --- .../ekf2/EKF/aid_sources/gnss/gps_checks.cpp | 172 +++++++++--------- src/modules/ekf2/EKF/ekf.h | 1 + src/modules/ekf2/EKF/estimator_interface.h | 4 +- 3 files changed, 91 insertions(+), 86 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index 81b3fcc76b..cf28524c79 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -55,112 +55,41 @@ #define MASK_GPS_VSPD (1<<8) #define MASK_GPS_SPOOFED (1<<9) -bool Ekf::runGnssChecks(const gnssSample &gps) +bool Ekf::runGnssChecks(const gnssSample &gnss) { - _gps_check_fail_status.flags.spoofed = gps.spoofed; + _gps_check_fail_status.flags.spoofed = gnss.spoofed; // Check the fix type - _gps_check_fail_status.flags.fix = (gps.fix_type < 3); + _gps_check_fail_status.flags.fix = (gnss.fix_type < 3); // Check the number of satellites - _gps_check_fail_status.flags.nsats = (gps.nsats < _params.req_nsats); + _gps_check_fail_status.flags.nsats = (gnss.nsats < _params.req_nsats); // Check the position dilution of precision - _gps_check_fail_status.flags.pdop = (gps.pdop > _params.req_pdop); + _gps_check_fail_status.flags.pdop = (gnss.pdop > _params.req_pdop); // Check the reported horizontal and vertical position accuracy - _gps_check_fail_status.flags.hacc = (gps.hacc > _params.req_hacc); - _gps_check_fail_status.flags.vacc = (gps.vacc > _params.req_vacc); + _gps_check_fail_status.flags.hacc = (gnss.hacc > _params.req_hacc); + _gps_check_fail_status.flags.vacc = (gnss.vacc > _params.req_vacc); // Check the reported speed accuracy - _gps_check_fail_status.flags.sacc = (gps.sacc > _params.req_sacc); + _gps_check_fail_status.flags.sacc = (gnss.sacc > _params.req_sacc); - // Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient - constexpr float filt_time_const = 10.0f; - const float dt = math::constrain(float(int64_t(gps.time_us) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp())) - * 1e-6f, 0.001f, filt_time_const); - const float filter_coef = dt / filt_time_const; - - // The following checks are only valid when the vehicle is at rest - const double lat = gps.lat; - const double lon = gps.lon; - - if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) { - // Calculate position movement since last measurement - float delta_pos_n = 0.0f; - float delta_pos_e = 0.0f; - - // calculate position movement since last GPS fix - if (_gps_pos_prev.getProjectionReferenceTimestamp() > 0) { - _gps_pos_prev.project(lat, lon, delta_pos_n, delta_pos_e); - - } else { - // no previous position has been set - _gps_pos_prev.initReference(lat, lon, gps.time_us); - _gps_alt_prev = gps.alt; - } - - // Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold - const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift); - Vector3f delta_pos(delta_pos_n, delta_pos_e, (_gps_alt_prev - gps.alt)); - - // Apply a low pass filter - _gps_pos_deriv_filt = delta_pos / dt * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef); - - // Apply anti-windup to the state instead of the input to avoid generating a bias on asymmetric signals - _gps_pos_deriv_filt = matrix::constrain(_gps_pos_deriv_filt, -10.0f * vel_limit, 10.0f * vel_limit); - - // hdrift: calculate the horizontal drift speed and fail if too high - _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); - - // vdrift: fail if the vertical drift speed is too high - _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); - - // hspeed: check the magnitude of the filtered horizontal GNSS velocity - const Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel.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_filtered_horizontal_velocity_m_s = _gps_velNE_filt.norm(); - _gps_check_fail_status.flags.hspeed = (_gps_filtered_horizontal_velocity_m_s > _params.req_hdrift); - - // vspeed: check the magnitude of the filtered vertical GNSS velocity - const float gnss_vz_limit = 10.f * _params.req_vdrift; - const float gnss_vz = math::constrain(gps.vel(2), -gnss_vz_limit, gnss_vz_limit); - _gps_vel_d_filt = gnss_vz * filter_coef + _gps_vel_d_filt * (1.f - filter_coef); - - _gps_check_fail_status.flags.vspeed = (fabsf(_gps_vel_d_filt) > _params.req_vdrift); - - } else if (_control_status.flags.in_air) { - // These checks are always declared as passed when flying - // If on ground and moving, the last result before movement commenced is kept - _gps_check_fail_status.flags.hdrift = false; - _gps_check_fail_status.flags.vdrift = false; - _gps_check_fail_status.flags.hspeed = false; - _gps_check_fail_status.flags.vspeed = false; - - resetGpsDriftCheckFilters(); - - } else { - // This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation - resetGpsDriftCheckFilters(); - } + runOnGroundGnssChecks(gnss); // force horizontal speed failure if above the limit - if (gps.vel.xy().longerThan(_params.velocity_limit)) { + if (gnss.vel.xy().longerThan(_params.velocity_limit)) { _gps_check_fail_status.flags.hspeed = true; } // force vertical speed failure if above the limit - if (fabsf(gps.vel(2)) > _params.velocity_limit) { + if (fabsf(gnss.vel(2)) > _params.velocity_limit) { _gps_check_fail_status.flags.vspeed = true; } // save GPS fix for next time - _gps_pos_prev.initReference(lat, lon, gps.time_us); - _gps_alt_prev = gps.alt; + _gnss_pos_prev.initReference(gnss.lat, gnss.lon, gnss.time_us); + _gnss_alt_prev = gnss.alt; // assume failed first time through if (_last_gps_fail_us == 0) { @@ -190,6 +119,81 @@ bool Ekf::runGnssChecks(const gnssSample &gps) } } +void Ekf::runOnGroundGnssChecks(const gnssSample &gnss) +{ + if (_control_status.flags.in_air) { + // These checks are always declared as passed when flying + // If on ground and moving, the last result before movement commenced is kept + _gps_check_fail_status.flags.hdrift = false; + _gps_check_fail_status.flags.vdrift = false; + _gps_check_fail_status.flags.hspeed = false; + _gps_check_fail_status.flags.vspeed = false; + + resetGpsDriftCheckFilters(); + return; + } + + if (_control_status.flags.vehicle_at_rest) { + // Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient + constexpr float filt_time_const = 10.0f; + const float dt = math::constrain(float(int64_t(gnss.time_us) - int64_t( + _gnss_pos_prev.getProjectionReferenceTimestamp())) + * 1e-6f, 0.001f, filt_time_const); + const float filter_coef = dt / filt_time_const; + + // Calculate position movement since last measurement + float delta_pos_n = 0.0f; + float delta_pos_e = 0.0f; + + // calculate position movement since last GPS fix + if (_gnss_pos_prev.getProjectionReferenceTimestamp() > 0) { + _gnss_pos_prev.project(gnss.lat, gnss.lon, delta_pos_n, delta_pos_e); + + } else { + // no previous position has been set + _gnss_pos_prev.initReference(gnss.lat, gnss.lon, gnss.time_us); + _gnss_alt_prev = gnss.alt; + } + + // Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold + const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift); + Vector3f delta_pos(delta_pos_n, delta_pos_e, (_gnss_alt_prev - gnss.alt)); + + // Apply a low pass filter + _gps_pos_deriv_filt = delta_pos / dt * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef); + + // Apply anti-windup to the state instead of the input to avoid generating a bias on asymmetric signals + _gps_pos_deriv_filt = matrix::constrain(_gps_pos_deriv_filt, -10.0f * vel_limit, 10.0f * vel_limit); + + // hdrift: calculate the horizontal drift speed and fail if too high + _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); + + // vdrift: fail if the vertical drift speed is too high + _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); + + // hspeed: check the magnitude of the filtered horizontal GNSS velocity + const Vector2f gps_velNE = matrix::constrain(Vector2f(gnss.vel.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_filtered_horizontal_velocity_m_s = _gps_velNE_filt.norm(); + _gps_check_fail_status.flags.hspeed = (_gps_filtered_horizontal_velocity_m_s > _params.req_hdrift); + + // vspeed: check the magnitude of the filtered vertical GNSS velocity + const float gnss_vz_limit = 10.f * _params.req_vdrift; + const float gnss_vz = math::constrain(gnss.vel(2), -gnss_vz_limit, gnss_vz_limit); + _gps_vel_d_filt = gnss_vz * filter_coef + _gps_vel_d_filt * (1.f - filter_coef); + + _gps_check_fail_status.flags.vspeed = (fabsf(_gps_vel_d_filt) > _params.req_vdrift); + + } else { + // This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation + resetGpsDriftCheckFilters(); + } +} + void Ekf::resetGpsDriftCheckFilters() { _gps_velNE_filt.setZero(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 1dad06d304..8f40beae2a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -883,6 +883,7 @@ private: * Checks are adjusted using the EKF2_REQ_* parameters */ bool runGnssChecks(const gnssSample &gps); + void runOnGroundGnssChecks(const gnssSample &gnss); void controlGnssHeightFusion(const gnssSample &gps_sample); void stopGpsHgtFusion(); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 592b786a6e..10c1832c67 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -402,8 +402,8 @@ protected: float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s) float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s) - MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message - float _gps_alt_prev{0.0f}; // height from the previous GPS message (m) + MapProjection _gnss_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message + float _gnss_alt_prev{0.0f}; // height from the previous GPS message (m) # if defined(CONFIG_EKF2_GNSS_YAW) // innovation consistency check monitoring ratios