From 0e32b155f3649c333f8f548a634421150bbbf7bd Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 23 May 2025 09:59:33 +0200 Subject: [PATCH] ekf2: simplify names of scoped variables --- .../ekf2/EKF/aid_sources/gnss/gnss_checks.cpp | 133 +++++++++--------- .../ekf2/EKF/aid_sources/gnss/gnss_checks.hpp | 62 ++++---- .../ekf2/EKF/aid_sources/gnss/gps_control.cpp | 2 +- 3 files changed, 96 insertions(+), 101 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp index cf216b5c65..6d41f482d5 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp @@ -40,93 +40,92 @@ namespace estimator { -bool GnssChecks::runGnssChecks(const gnssSample &gnss, uint64_t time_us) +bool GnssChecks::run(const gnssSample &gnss, uint64_t time_us) { // assume failed first time through - if (_last_gps_fail_us == 0) { - _last_gps_fail_us = time_us; + if (_time_last_fail_us == 0) { + _time_last_fail_us = time_us; } bool passed = false; if (_initial_checks_passed) { if (runInitialFixChecks(gnss)) { - _last_gps_pass_us = time_us; - passed = isTimedOut(_last_gps_fail_us, time_us, math::max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10)); + _time_last_pass_us = time_us; + passed = isTimedOut(_time_last_fail_us, time_us, math::max((uint64_t)1e6, (uint64_t)_params.min_health_time_us / 10)); } else { - _last_gps_fail_us = time_us; + _time_last_fail_us = time_us; } } else { if (runInitialFixChecks(gnss)) { - _last_gps_pass_us = time_us; + _time_last_pass_us = time_us; - if (isTimedOut(_last_gps_fail_us, time_us, (uint64_t)_min_gps_health_time_us)) { + if (isTimedOut(_time_last_fail_us, time_us, (uint64_t)_params.min_health_time_us)) { _initial_checks_passed = true; passed = true; } } else { - _last_gps_fail_us = time_us; + _time_last_fail_us = time_us; } } - // save GPS fix for next time - _gnss_pos_prev.initReference(gnss.lat, gnss.lon, gnss.time_us); - _gnss_alt_prev = gnss.alt; + lat_lon_prev.initReference(gnss.lat, gnss.lon, gnss.time_us); + _alt_prev = gnss.alt; - _checks_passed = passed; + _passed = passed; return passed; } bool GnssChecks::runInitialFixChecks(const gnssSample &gnss) { // Check the fix type - _gps_check_fail_status.flags.fix = (gnss.fix_type < 3); + _check_fail_status.flags.fix = (gnss.fix_type < 3); // Check the number of satellites - _gps_check_fail_status.flags.nsats = (gnss.nsats < _params.req_nsats); + _check_fail_status.flags.nsats = (gnss.nsats < _params.req_nsats); // Check the position dilution of precision - _gps_check_fail_status.flags.pdop = (gnss.pdop > _params.req_pdop); + _check_fail_status.flags.pdop = (gnss.pdop > _params.req_pdop); // Check the reported horizontal and vertical position accuracy - _gps_check_fail_status.flags.hacc = (gnss.hacc > _params.req_hacc); - _gps_check_fail_status.flags.vacc = (gnss.vacc > _params.req_vacc); + _check_fail_status.flags.hacc = (gnss.hacc > _params.req_hacc); + _check_fail_status.flags.vacc = (gnss.vacc > _params.req_vacc); // Check the reported speed accuracy - _gps_check_fail_status.flags.sacc = (gnss.sacc > _params.req_sacc); + _check_fail_status.flags.sacc = (gnss.sacc > _params.req_sacc); - _gps_check_fail_status.flags.spoofed = gnss.spoofed; + _check_fail_status.flags.spoofed = gnss.spoofed; runOnGroundGnssChecks(gnss); // force horizontal speed failure if above the limit if (gnss.vel.xy().longerThan(_params.velocity_limit)) { - _gps_check_fail_status.flags.hspeed = true; + _check_fail_status.flags.hspeed = true; } // force vertical speed failure if above the limit if (fabsf(gnss.vel(2)) > _params.velocity_limit) { - _gps_check_fail_status.flags.vspeed = true; + _check_fail_status.flags.vspeed = true; } bool passed = true; // if any user selected checks have failed, record the fail time if ( - _gps_check_fail_status.flags.fix || - (_gps_check_fail_status.flags.nsats && isGnssCheckEnabled(GnssChecksMask::kNsats)) || - (_gps_check_fail_status.flags.pdop && isGnssCheckEnabled(GnssChecksMask::kPdop)) || - (_gps_check_fail_status.flags.hacc && isGnssCheckEnabled(GnssChecksMask::kHacc)) || - (_gps_check_fail_status.flags.vacc && isGnssCheckEnabled(GnssChecksMask::kVacc)) || - (_gps_check_fail_status.flags.sacc && isGnssCheckEnabled(GnssChecksMask::kSacc)) || - (_gps_check_fail_status.flags.hdrift && isGnssCheckEnabled(GnssChecksMask::kHdrift)) || - (_gps_check_fail_status.flags.vdrift && isGnssCheckEnabled(GnssChecksMask::kVdrift)) || - (_gps_check_fail_status.flags.hspeed && isGnssCheckEnabled(GnssChecksMask::kHspd)) || - (_gps_check_fail_status.flags.vspeed && isGnssCheckEnabled(GnssChecksMask::kVspd)) || - (_gps_check_fail_status.flags.spoofed && isGnssCheckEnabled(GnssChecksMask::kSpoofed)) + _check_fail_status.flags.fix || + (_check_fail_status.flags.nsats && isCheckEnabled(GnssChecksMask::kNsats)) || + (_check_fail_status.flags.pdop && isCheckEnabled(GnssChecksMask::kPdop)) || + (_check_fail_status.flags.hacc && isCheckEnabled(GnssChecksMask::kHacc)) || + (_check_fail_status.flags.vacc && isCheckEnabled(GnssChecksMask::kVacc)) || + (_check_fail_status.flags.sacc && isCheckEnabled(GnssChecksMask::kSacc)) || + (_check_fail_status.flags.hdrift && isCheckEnabled(GnssChecksMask::kHdrift)) || + (_check_fail_status.flags.vdrift && isCheckEnabled(GnssChecksMask::kVdrift)) || + (_check_fail_status.flags.hspeed && isCheckEnabled(GnssChecksMask::kHspd)) || + (_check_fail_status.flags.vspeed && isCheckEnabled(GnssChecksMask::kVspd)) || + (_check_fail_status.flags.spoofed && isCheckEnabled(GnssChecksMask::kSpoofed)) ) { passed = false; } @@ -139,12 +138,12 @@ void GnssChecks::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; + _check_fail_status.flags.hdrift = false; + _check_fail_status.flags.vdrift = false; + _check_fail_status.flags.hspeed = false; + _check_fail_status.flags.vspeed = false; - resetGpsDriftCheckFilters(); + resetDriftFilters(); return; } @@ -152,7 +151,7 @@ void GnssChecks::runOnGroundGnssChecks(const gnssSample &gnss) // 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())) + lat_lon_prev.getProjectionReferenceTimestamp())) * 1e-6f, 0.001f, filt_time_const); const float filter_coef = dt / filt_time_const; @@ -160,64 +159,64 @@ void GnssChecks::runOnGroundGnssChecks(const gnssSample &gnss) 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); + // calculate position movement since last fix + if (lat_lon_prev.getProjectionReferenceTimestamp() > 0) { + lat_lon_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; + lat_lon_prev.initReference(gnss.lat, gnss.lon, gnss.time_us); + _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)); + Vector3f delta_pos(delta_pos_n, delta_pos_e, (_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); + _lat_lon_alt_deriv_filt = delta_pos / dt * filter_coef + _lat_lon_alt_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); + _lat_lon_alt_deriv_filt = matrix::constrain(_lat_lon_alt_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); + _horizontal_position_drift_rate_m_s = Vector2f(_lat_lon_alt_deriv_filt.xy()).norm(); + _check_fail_status.flags.hdrift = (_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); + _vertical_position_drift_rate_m_s = fabsf(_lat_lon_alt_deriv_filt(2)); + _check_fail_status.flags.vdrift = (_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); + const Vector2f vel_ne = matrix::constrain(Vector2f(gnss.vel.xy()), + -10.0f * _params.req_hdrift, + 10.0f * _params.req_hdrift); + _vel_ne_filt = vel_ne * filter_coef + _vel_ne_filt * (1.0f - filter_coef); + _filtered_horizontal_velocity_m_s = _vel_ne_filt.norm(); + _check_fail_status.flags.hspeed = (_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); + _vel_d_filt = gnss_vz * filter_coef + _vel_d_filt * (1.f - filter_coef); - _gps_check_fail_status.flags.vspeed = (fabsf(_gps_vel_d_filt) > _params.req_vdrift); + _check_fail_status.flags.vspeed = (fabsf(_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(); + resetDriftFilters(); } } -void GnssChecks::resetGpsDriftCheckFilters() +void GnssChecks::resetDriftFilters() { - _gps_velNE_filt.setZero(); - _gps_vel_d_filt = 0.f; + _vel_ne_filt.setZero(); + _vel_d_filt = 0.f; - _gps_pos_deriv_filt.setZero(); + _lat_lon_alt_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; + _horizontal_position_drift_rate_m_s = NAN; + _vertical_position_drift_rate_m_s = NAN; + _filtered_horizontal_velocity_m_s = NAN; } }; // namespace estimator diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp index fa8ac7d2d4..af64678271 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp @@ -46,8 +46,7 @@ public: GnssChecks(int32_t &check_mask, int32_t &req_nsats, float &req_pdop, float &req_hacc, float &req_vacc, float &req_sacc, float &req_hdrift, float &req_vdrift, float &velocity_limit, uint32_t &min_health_time_us, filter_control_status_u &control_status): - _params{check_mask, req_nsats, req_pdop, req_hacc, req_vacc, req_sacc, req_hdrift, req_vdrift, velocity_limit}, - _min_gps_health_time_us(min_health_time_us), + _params{check_mask, req_nsats, req_pdop, req_hacc, req_vacc, req_sacc, req_hdrift, req_vdrift, velocity_limit, min_health_time_us}, _control_status(control_status) {}; @@ -76,28 +75,26 @@ public: void reset() { - _checks_passed = false; - _last_gps_pass_us = 0; - _last_gps_fail_us = 0; - resetGpsDriftCheckFilters(); + _passed = false; + _time_last_pass_us = 0; + _time_last_fail_us = 0; + resetDriftFilters(); } /* - * Return true if the GPS solution quality is adequate. - * Checks are activated using the EKF2_GPS_CHECK bitmask parameter - * Checks are adjusted using the EKF2_REQ_* parameters + * Return true if the GNSS solution quality is adequate. */ - bool runGnssChecks(const gnssSample &gps, uint64_t time_us); - bool passed() const { return _checks_passed; } + bool run(const gnssSample &gnss, uint64_t time_us); + bool passed() const { return _passed; } bool initialChecksPassed() const { return _initial_checks_passed; } - uint64_t getLastPassUs() const { return _last_gps_pass_us; } - uint64_t getLastFailUs() const { return _last_gps_fail_us; } + uint64_t getLastPassUs() const { return _time_last_pass_us; } + uint64_t getLastFailUs() const { return _time_last_fail_us; } - const gps_check_fail_status_u &getFailStatus() const { return _gps_check_fail_status; } + const gps_check_fail_status_u &getFailStatus() const { return _check_fail_status; } - float horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; } - float vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; } - float filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; } + float horizontal_position_drift_rate_m_s() const { return _horizontal_position_drift_rate_m_s; } + float vertical_position_drift_rate_m_s() const { return _vertical_position_drift_rate_m_s; } + float filtered_horizontal_velocity_m_s() const { return _filtered_horizontal_velocity_m_s; } private: enum class GnssChecksMask : int32_t { @@ -113,36 +110,35 @@ private: kSpoofed = (1 << 9) }; - bool isGnssCheckEnabled(GnssChecksMask check) { return (_params.check_mask & static_cast(check)); } + bool isCheckEnabled(GnssChecksMask check) { return (_params.check_mask & static_cast(check)); } bool runInitialFixChecks(const gnssSample &gnss); void runOnGroundGnssChecks(const gnssSample &gnss); - void resetGpsDriftCheckFilters(); + void resetDriftFilters(); bool isTimedOut(uint64_t timestamp_to_check_us, uint64_t now_us, uint64_t timeout_period) const { return (timestamp_to_check_us == 0) || (timestamp_to_check_us + timeout_period < now_us); } - gps_check_fail_status_u _gps_check_fail_status{}; + gps_check_fail_status_u _check_fail_status{}; - float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s) - 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) + float _horizontal_position_drift_rate_m_s{NAN}; + float _vertical_position_drift_rate_m_s{NAN}; + float _filtered_horizontal_velocity_m_s{NAN}; - 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) + MapProjection lat_lon_prev{}; + float _alt_prev{0.0f}; - // variables used for the GPS quality checks - Vector3f _gps_pos_deriv_filt{}; - Vector2f _gps_velNE_filt{}; ///< filtered GPS North and East velocity (m/sec) + Vector3f _lat_lon_alt_deriv_filt{}; + Vector2f _vel_ne_filt{}; - float _gps_vel_d_filt{0.0f}; ///< GNSS filtered Down velocity (m/sec) - uint64_t _last_gps_fail_us{0}; - uint64_t _last_gps_pass_us{0}; + float _vel_d_filt{0.0f}; ///< GNSS filtered Down velocity (m/sec) + uint64_t _time_last_fail_us{0}; + uint64_t _time_last_pass_us{0}; bool _initial_checks_passed{false}; - bool _checks_passed{false}; + bool _passed{false}; struct Params { const int32_t &check_mask; @@ -154,10 +150,10 @@ private: const float &req_hdrift; const float &req_vdrift; const float &velocity_limit; + const uint32_t &min_health_time_us; }; const Params _params; - const uint32_t &_min_gps_health_time_us; ///< GPS is marked as healthy only after this amount of time const filter_control_status_u &_control_status; }; }; // namespace estimator diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index 2373c76481..01d15601c0 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -65,7 +65,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) const bool initial_checks_passed_prev = _gnss_checks.initialChecksPassed(); - if (_gnss_checks.runGnssChecks(gnss_sample, _time_delayed_us)) { + if (_gnss_checks.run(gnss_sample, _time_delayed_us)) { if (_gnss_checks.initialChecksPassed() && !initial_checks_passed_prev) { // First time checks are passing, latching. _information_events.flags.gps_checks_passed = true;