diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 181978792c..a7b8e18f8a 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -173,8 +173,8 @@ endif() if(CONFIG_EKF2_GNSS) list(APPEND EKF_SRCS + EKF/aid_sources/gnss/gnss_checks.cpp EKF/aid_sources/gnss/gnss_height_control.cpp - EKF/aid_sources/gnss/gps_checks.cpp EKF/aid_sources/gnss/gps_control.cpp ) diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 2c4c898d41..381e265380 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -90,8 +90,8 @@ endif() if(CONFIG_EKF2_GNSS) list(APPEND EKF_SRCS + aid_sources/gnss/gnss_checks.cpp aid_sources/gnss/gnss_height_control.cpp - aid_sources/gnss/gps_checks.cpp aid_sources/gnss/gps_control.cpp ) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp similarity index 86% rename from src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp rename to src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp index c1e37e0893..cf216b5c65 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,26 +32,56 @@ ****************************************************************************/ /** - * @file gps_checks.cpp - * Perform pre-flight and in-flight GPS quality checks - * - * @author Paul Riseborough - * + * @file gnss_checks.cpp + * Perform pre-flight and in-flight GNSS quality checks */ -#include "ekf.h" +#include "aid_sources/gnss/gnss_checks.hpp" -#include - -bool Ekf::isGnssCheckEnabled(GnssChecksMask check) +namespace estimator { - return (_params.gps_check_mask & static_cast(check)); +bool GnssChecks::runGnssChecks(const gnssSample &gnss, uint64_t time_us) +{ + // assume failed first time through + if (_last_gps_fail_us == 0) { + _last_gps_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)); + + } else { + _last_gps_fail_us = time_us; + } + + } else { + if (runInitialFixChecks(gnss)) { + _last_gps_pass_us = time_us; + + if (isTimedOut(_last_gps_fail_us, time_us, (uint64_t)_min_gps_health_time_us)) { + _initial_checks_passed = true; + passed = true; + } + + } else { + _last_gps_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; + + _checks_passed = passed; + return passed; } -bool Ekf::runGnssChecks(const gnssSample &gnss) +bool GnssChecks::runInitialFixChecks(const gnssSample &gnss) { - _gps_check_fail_status.flags.spoofed = gnss.spoofed; - // Check the fix type _gps_check_fail_status.flags.fix = (gnss.fix_type < 3); @@ -68,6 +98,8 @@ bool Ekf::runGnssChecks(const gnssSample &gnss) // Check the reported speed accuracy _gps_check_fail_status.flags.sacc = (gnss.sacc > _params.req_sacc); + _gps_check_fail_status.flags.spoofed = gnss.spoofed; + runOnGroundGnssChecks(gnss); // force horizontal speed failure if above the limit @@ -80,14 +112,7 @@ bool Ekf::runGnssChecks(const gnssSample &gnss) _gps_check_fail_status.flags.vspeed = true; } - // save GPS fix for next time - _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) { - _last_gps_fail_us = _time_delayed_us; - } + bool passed = true; // if any user selected checks have failed, record the fail time if ( @@ -103,16 +128,13 @@ bool Ekf::runGnssChecks(const gnssSample &gnss) (_gps_check_fail_status.flags.vspeed && isGnssCheckEnabled(GnssChecksMask::kVspd)) || (_gps_check_fail_status.flags.spoofed && isGnssCheckEnabled(GnssChecksMask::kSpoofed)) ) { - _last_gps_fail_us = _time_delayed_us; - return false; - - } else { - _last_gps_pass_us = _time_delayed_us; - return true; + passed = false; } + + return passed; } -void Ekf::runOnGroundGnssChecks(const gnssSample &gnss) +void GnssChecks::runOnGroundGnssChecks(const gnssSample &gnss) { if (_control_status.flags.in_air) { // These checks are always declared as passed when flying @@ -187,7 +209,7 @@ void Ekf::runOnGroundGnssChecks(const gnssSample &gnss) } } -void Ekf::resetGpsDriftCheckFilters() +void GnssChecks::resetGpsDriftCheckFilters() { _gps_velNE_filt.setZero(); _gps_vel_d_filt = 0.f; @@ -198,3 +220,4 @@ void Ekf::resetGpsDriftCheckFilters() _gps_vertical_position_drift_rate_m_s = NAN; _gps_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 new file mode 100644 index 0000000000..fa8ac7d2d4 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp @@ -0,0 +1,165 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef EKF_GNSS_CHECKS_H +#define EKF_GNSS_CHECKS_H + +#include + +#include "../../common.h" + +namespace estimator +{ +class GnssChecks final +{ +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), + _control_status(control_status) + {}; + + union gps_check_fail_status_u { + struct { + uint16_t fix : 1; ///< 0 - true if the fix type is insufficient (no 3D solution) + uint16_t nsats : 1; ///< 1 - true if number of satellites used is insufficient + uint16_t pdop : 1; ///< 2 - true if position dilution of precision is insufficient + uint16_t hacc : 1; ///< 3 - true if reported horizontal accuracy is insufficient + uint16_t vacc : 1; ///< 4 - true if reported vertical accuracy is insufficient + uint16_t sacc : 1; ///< 5 - true if reported speed accuracy is insufficient + uint16_t hdrift : 1; ///< 6 - true if horizontal drift is excessive (can only be used when stationary on ground) + uint16_t vdrift : 1; ///< 7 - true if vertical drift is excessive (can only be used when stationary on ground) + uint16_t hspeed : 1; ///< 8 - true if horizontal speed is excessive (can only be used when stationary on ground) + uint16_t vspeed : 1; ///< 9 - true if vertical speed error is excessive + uint16_t spoofed: 1; ///< 10 - true if the GNSS data is spoofed + } flags; + uint16_t value; + }; + + void resetHard() + { + _initial_checks_passed = false; + reset(); + } + + void reset() + { + _checks_passed = false; + _last_gps_pass_us = 0; + _last_gps_fail_us = 0; + resetGpsDriftCheckFilters(); + } + + /* + * 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 + */ + bool runGnssChecks(const gnssSample &gps, uint64_t time_us); + bool passed() const { return _checks_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; } + + const gps_check_fail_status_u &getFailStatus() const { return _gps_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; } + +private: + enum class GnssChecksMask : int32_t { + kNsats = (1 << 0), + kPdop = (1 << 1), + kHacc = (1 << 2), + kVacc = (1 << 3), + kSacc = (1 << 4), + kHdrift = (1 << 5), + kVdrift = (1 << 6), + kHspd = (1 << 7), + kVspd = (1 << 8), + kSpoofed = (1 << 9) + }; + + bool isGnssCheckEnabled(GnssChecksMask check) { return (_params.check_mask & static_cast(check)); } + + bool runInitialFixChecks(const gnssSample &gnss); + void runOnGroundGnssChecks(const gnssSample &gnss); + + void resetGpsDriftCheckFilters(); + + 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{}; + + 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) + + 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) + + // variables used for the GPS quality checks + Vector3f _gps_pos_deriv_filt{}; + Vector2f _gps_velNE_filt{}; ///< filtered GPS North and East velocity (m/sec) + + 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}; + bool _initial_checks_passed{false}; + bool _checks_passed{false}; + + struct Params { + const int32_t &check_mask; + const int32_t &req_nsats; + const float &req_pdop; + const float &req_hacc; + const float &req_vacc; + const float &req_sacc; + const float &req_hdrift; + const float &req_vdrift; + const float &velocity_limit; + }; + + 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 + +#endif // !EKF_GNSS_CHECKS_H diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index 9426fa2664..e8285f7267 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -87,7 +87,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) // determine if we should use height aiding const bool common_conditions_passing = measurement_valid && _local_origin_lat_lon.isInitialized() - && _gps_checks_passed; + && _gnss_checks.passed(); const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast(GnssCtrl::VPOS)) && common_conditions_passing; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp index bd37c0fb37..06b8fffccc 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp @@ -66,7 +66,7 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample) 2 * GNSS_YAW_MAX_INTERVAL); const bool starting_conditions_passing = continuing_conditions_passing - && _gps_checks_passed + && _gnss_checks.passed() && !is_gnss_yaw_data_intermittent && !_gps_intermittent; 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 9eabfe156d..2373c76481 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -63,23 +63,22 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) if (_gps_data_ready) { const gnssSample &gnss_sample = _gps_sample_delayed; - if (runGnssChecks(gnss_sample) - && isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) { - if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) { - // First time checks are passing, latching. - if (!_gps_checks_passed) { - _information_events.flags.gps_checks_passed = true; - } + const bool initial_checks_passed_prev = _gnss_checks.initialChecksPassed(); - _gps_checks_passed = true; + if (_gnss_checks.runGnssChecks(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; } } else { // Skip this sample _gps_data_ready = false; - if ((_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) - && isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) { + const bool using_gnss = _control_status.flags.gnss_vel || _control_status.flags.gnss_pos; + const bool gnss_checks_pass_timeout = isTimedOut(_gnss_checks.getLastPassUs(), _params.reset_timeout_max); + + if (using_gnss && gnss_checks_pass_timeout) { stopGnssFusion(); ECL_WARN("GNSS quality poor - stopping use"); } @@ -128,7 +127,7 @@ void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool for const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast(GnssCtrl::VEL)) && _control_status.flags.tilt_align && _control_status.flags.yaw_align; - const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed; + const bool starting_conditions_passing = continuing_conditions_passing && _gnss_checks.passed(); if (_control_status.flags.gnss_vel) { if (continuing_conditions_passing) { @@ -175,8 +174,8 @@ void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool for const bool continuing_conditions_passing = gnss_pos_enabled && _control_status.flags.tilt_align && _control_status.flags.yaw_align; - const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed; - const bool gpos_init_conditions_passing = gnss_pos_enabled && _gps_checks_passed; + const bool starting_conditions_passing = continuing_conditions_passing && _gnss_checks.passed(); + const bool gpos_init_conditions_passing = gnss_pos_enabled && _gnss_checks.passed(); if (_control_status.flags.gnss_pos) { if (continuing_conditions_passing) { @@ -394,8 +393,7 @@ bool Ekf::shouldResetGpsFusion() const void Ekf::stopGnssFusion() { if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) { - _last_gps_fail_us = 0; - _last_gps_pass_us = 0; + _gnss_checks.reset(); } stopGnssVelFusion(); @@ -416,8 +414,7 @@ void Ekf::stopGnssVelFusion() //TODO: what if gnss yaw or height is used? if (!_control_status.flags.gnss_pos) { - _last_gps_fail_us = 0; - _last_gps_pass_us = 0; + _gnss_checks.reset(); } } } @@ -430,8 +427,7 @@ void Ekf::stopGnssPosFusion() //TODO: what if gnss yaw or height is used? if (!_control_status.flags.gnss_vel) { - _last_gps_fail_us = 0; - _last_gps_pass_us = 0; + _gnss_checks.reset(); } } } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 5ec5e42925..8ee95dea82 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -543,24 +543,6 @@ union innovation_fault_status_u { uint16_t value; }; -// publish the status of various GPS quality checks -union gps_check_fail_status_u { - struct { - uint16_t fix : 1; ///< 0 - true if the fix type is insufficient (no 3D solution) - uint16_t nsats : 1; ///< 1 - true if number of satellites used is insufficient - uint16_t pdop : 1; ///< 2 - true if position dilution of precision is insufficient - uint16_t hacc : 1; ///< 3 - true if reported horizontal accuracy is insufficient - uint16_t vacc : 1; ///< 4 - true if reported vertical accuracy is insufficient - uint16_t sacc : 1; ///< 5 - true if reported speed accuracy is insufficient - uint16_t hdrift : 1; ///< 6 - true if horizontal drift is excessive (can only be used when stationary on ground) - uint16_t vdrift : 1; ///< 7 - true if vertical drift is excessive (can only be used when stationary on ground) - uint16_t hspeed : 1; ///< 8 - true if horizontal speed is excessive (can only be used when stationary on ground) - uint16_t vspeed : 1; ///< 9 - true if vertical speed error is excessive - uint16_t spoofed: 1; ///< 10 - true if the GNSS data is spoofed - } flags; - uint16_t value; -}; - // bitmask containing filter control status union filter_control_status_u { struct { diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index b6efa80e5d..5444886870 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -94,8 +94,7 @@ void Ekf::reset() _innov_check_fail_status.value = 0; #if defined(CONFIG_EKF2_GNSS) - resetGpsDriftCheckFilters(); - _gps_checks_passed = false; + _gnss_checks.resetHard(); #endif // CONFIG_EKF2_GNSS _local_origin_alt = NAN; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index dc5a40d707..02632f0f5c 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -46,6 +46,7 @@ #include "estimator_interface.h" #if defined(CONFIG_EKF2_GNSS) +# include "aid_sources/gnss/gnss_checks.hpp" # include "yaw_estimator/EKFGSF_yaw.h" #endif // CONFIG_EKF2_GNSS @@ -376,10 +377,10 @@ 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; } + const GnssChecks::gps_check_fail_status_u &gps_check_fail_status() const { return _gnss_checks.getFailStatus(); } + const decltype(GnssChecks::gps_check_fail_status_u::flags) &gps_check_fail_status_flags() const { return _gnss_checks.getFailStatus().flags; } - bool gps_checks_passed() const { return _gps_checks_passed; }; + bool gps_checks_passed() const { return _gnss_checks.passed(); }; const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); } @@ -554,17 +555,6 @@ private: #if defined(CONFIG_EKF2_GNSS) bool _gps_data_ready {false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused - // variables used for the GPS quality checks - Vector3f _gps_pos_deriv_filt{}; ///< GPS NED position derivative (m/sec) - Vector2f _gps_velNE_filt{}; ///< filtered GPS North and East velocity (m/sec) - - float _gps_vel_d_filt{0.0f}; ///< GNSS filtered Down velocity (m/sec) - uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks - uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks - uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time - bool _gps_checks_passed{false}; ///> true when all active GPS checks have passed - - gps_check_fail_status_u _gps_check_fail_status{}; // height sensor status bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent @@ -877,34 +867,9 @@ private: void resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src); bool shouldResetGpsFusion() const; - /* - * 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 - */ - bool runGnssChecks(const gnssSample &gps); - - enum class GnssChecksMask : int32_t { - kNsats = (1 << 0), - kPdop = (1 << 1), - kHacc = (1 << 2), - kVacc = (1 << 3), - kSacc = (1 << 4), - kHdrift = (1 << 5), - kVdrift = (1 << 6), - kHspd = (1 << 7), - kVspd = (1 << 8), - kSpoofed = (1 << 9) - }; - - bool isGnssCheckEnabled(GnssChecksMask check); - void runOnGroundGnssChecks(const gnssSample &gnss); - void controlGnssHeightFusion(const gnssSample &gps_sample); void stopGpsHgtFusion(); - void resetGpsDriftCheckFilters(); - # if defined(CONFIG_EKF2_GNSS_YAW) void controlGnssYawFusion(const gnssSample &gps_sample); void stopGnssYawFusion(); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 10c1832c67..18ffeef55f 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -71,6 +71,10 @@ # include "aid_sources/range_finder/sensor_range_finder.hpp" #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_GNSS) +# include "aid_sources/gnss/gnss_checks.hpp" +#endif // CONFIG_EKF2_GNSS + #include #include #include @@ -89,9 +93,9 @@ public: const gnssSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } - 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; } + float gps_horizontal_position_drift_rate_m_s() const { return _gnss_checks.horizontal_position_drift_rate_m_s(); } + float gps_vertical_position_drift_rate_m_s() const { return _gnss_checks.vertical_position_drift_rate_m_s(); } + float gps_filtered_horizontal_velocity_m_s() const { return _gnss_checks.filtered_horizontal_velocity_m_s(); } #endif // CONFIG_EKF2_GNSS @@ -398,12 +402,18 @@ protected: gnssSample _gps_sample_delayed{}; - 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) - - 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) + uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time + GnssChecks _gnss_checks{_params.gps_check_mask, + _params.req_nsats, + _params.req_pdop, + _params.req_hacc, + _params.req_vacc, + _params.req_sacc, + _params.req_hdrift, + _params.req_vdrift, + _params.velocity_limit, + _min_gps_health_time_us, + _control_status}; # if defined(CONFIG_EKF2_GNSS_YAW) // innovation consistency check monitoring ratios