diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 1d7dae1bd7..da248a08fa 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -115,6 +115,7 @@ set(msg_files GpioIn.msg GpioOut.msg GpioRequest.msg + GnssRedundancyStatus.msg GpsDump.msg GpsInjectData.msg Gripper.msg diff --git a/msg/GnssRedundancyStatus.msg b/msg/GnssRedundancyStatus.msg new file mode 100644 index 0000000000..f5f873ebff --- /dev/null +++ b/msg/GnssRedundancyStatus.msg @@ -0,0 +1,15 @@ +# GNSS redundancy status published by vehicle_gps_position and consumed by commander + +uint64 timestamp # time since system start (microseconds) + +uint8 num_receivers_online # receivers publishing fresh data +uint8 num_receivers_fixed # receivers with fix_type >= 3 +uint8 num_receivers_required # configured minimum from SYS_HAS_NUM_GNSS (0 = no minimum) +uint8 peak_receivers_fixed # highest fixed count seen since boot + +bool below_required # num_receivers_fixed < num_receivers_required +bool dropped_below_peak # num_receivers_fixed < peak_receivers_fixed +bool divergence_detected # two receivers diverge beyond the gate after sustain + +float32 divergence_m # last-measured horizontal distance between the two receivers +float32 divergence_gate_m # current gate (lever-arm separation + sigma * RMS(eph)) diff --git a/src/lib/systemlib/system_params.yaml b/src/lib/systemlib/system_params.yaml index 177b44adf8..676a5b03b3 100644 --- a/src/lib/systemlib/system_params.yaml +++ b/src/lib/systemlib/system_params.yaml @@ -142,10 +142,10 @@ parameters: long: |- 0: No minimum receiver count required. Position divergence between two receivers and loss of a GNSS receiver still produce a warning but never trigger the - COM_GPS_LOSS_ACT failsafe action. + COM_GNSS_LSS_ACT failsafe action. 1-N: Require the presence of N GNSS receivers for arming and during flight. - If the active count drops below this value in flight, COM_GPS_LOSS_ACT is triggered. - When set to 2, position divergence between the two receivers also triggers COM_GPS_LOSS_ACT. + If the active count drops below this value in flight, COM_GNSS_LSS_ACT is triggered. + When set to 2, position divergence between the two receivers also triggers COM_GNSS_LSS_ACT. type: int32 default: 0 min: 0 diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index 044aaad4b3..272b97cc3b 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -79,7 +79,3 @@ add_dependencies(health_and_arming_checks mode_util) px4_add_functional_gtest(SRC HealthAndArmingChecksTest.cpp LINKLIBS health_and_arming_checks mode_util ) - -px4_add_functional_gtest(SRC GpsRedundancyCheckTest.cpp - LINKLIBS health_and_arming_checks mode_util geo - ) diff --git a/src/modules/commander/HealthAndArmingChecks/GpsRedundancyCheckTest.cpp b/src/modules/commander/HealthAndArmingChecks/GpsRedundancyCheckTest.cpp deleted file mode 100644 index d9ecc33405..0000000000 --- a/src/modules/commander/HealthAndArmingChecks/GpsRedundancyCheckTest.cpp +++ /dev/null @@ -1,266 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2024 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. - * - ****************************************************************************/ - -#include - -#include "Common.hpp" -#include "checks/gpsRedundancyCheck.hpp" - -#include -#include -#include - -using namespace time_literals; - -// to run: make tests TESTFILTER=GpsRedundancyCheck - -/* EVENT - * @skip-file - */ - -// Base position (PX4 SITL default home) -static constexpr double BASE_LAT = 47.397742; -static constexpr double BASE_LON = 8.545594; - -// 1 deg latitude ≈ 111111m -// AGREEING: ~0.4m north → below gate of 0.785m (0.70m lever arm + 0.085m eph tolerance) -// DIVERGING: ~2.0m north → above gate -static constexpr double AGREEING_LAT = BASE_LAT + 3.6e-6; -static constexpr double DIVERGING_LAT = BASE_LAT + 1.8e-5; - -class GpsRedundancyCheckTest : public ::testing::Test -{ -public: - static void SetUpTestSuite() - { - // Advertise the event topic so queued events are not lost - orb_advertise(ORB_ID(event), nullptr); - - // Pre-advertise sensor_gps instances 0 and 1 so they are always - // available to the check's SubscriptionMultiArray regardless of test order. - sensor_gps_s gps{}; - _pub0 = orb_advertise(ORB_ID(sensor_gps), &gps); - int inst; - _pub1 = orb_advertise_multi(ORB_ID(sensor_gps), &gps, &inst); - } - - void SetUp() override - { - param_control_autosave(false); - - // Lever arm: GPS0 at +35cm forward, GPS1 at -35cm → expected_d = 0.70m - // With both RTK Fixed (eph=0.02m): gate = 0.70 + 3*√(0.02²+0.02²) = 0.785m - float f = 0.35f; - param_set(param_find("SENS_GPS0_OFFX"), &f); - f = 0.f; - param_set(param_find("SENS_GPS0_OFFY"), &f); - f = -0.35f; - param_set(param_find("SENS_GPS1_OFFX"), &f); - f = 0.f; - param_set(param_find("SENS_GPS1_OFFY"), &f); - int i = 0; - param_set(param_find("SYS_HAS_NUM_GNSS"), &i); - param_set(param_find("COM_GPS_LOSS_ACT"), &i); - - // Construct check after params are set so ParamFloat reads correct initial values - _check = new GpsRedundancyChecks(); - - // Reset both instances to "absent" — device_id=0 is filtered by the check - sensor_gps_s empty{}; - orb_publish(ORB_ID(sensor_gps), _pub0, &empty); - orb_publish(ORB_ID(sensor_gps), _pub1, &empty); - } - - void TearDown() override - { - delete _check; - _check = nullptr; - } - - sensor_gps_s makeGps(double lat, double lon, float eph = 0.02f, uint8_t fix_type = 6) - { - sensor_gps_s gps{}; - gps.timestamp = hrt_absolute_time(); - gps.device_id = 1; - gps.latitude_deg = lat; - gps.longitude_deg = lon; - gps.altitude_ellipsoid_m = 100.0; - gps.eph = eph; - gps.epv = 0.05f; - gps.fix_type = fix_type; - return gps; - } - - // Runs one check cycle and returns true if the GPS divergence health warning fired - bool hasDivergenceWarning(bool armed) - { - vehicle_status_s status{}; - status.arming_state = armed - ? vehicle_status_s::ARMING_STATE_ARMED - : vehicle_status_s::ARMING_STATE_DISARMED; - Context context{status}; - failsafe_flags_s failsafe_flags{}; - Report reporter{failsafe_flags, 0_s}; - _check->checkAndReport(context, reporter); - return ((uint64_t)reporter.healthResults().warning - & (uint64_t)events::px4::enums::health_component_t::gps) != 0; - } - - // Runs one check cycle and returns true if gnss_lost was set (failsafe would trigger) - bool hasGnssLost(bool armed) - { - vehicle_status_s status{}; - status.arming_state = armed - ? vehicle_status_s::ARMING_STATE_ARMED - : vehicle_status_s::ARMING_STATE_DISARMED; - Context context{status}; - failsafe_flags_s failsafe_flags{}; - Report reporter{failsafe_flags, 0_s}; - _check->checkAndReport(context, reporter); - return failsafe_flags.gnss_lost; - } - - static orb_advert_t _pub0; - static orb_advert_t _pub1; - GpsRedundancyChecks *_check{nullptr}; -}; - -orb_advert_t GpsRedundancyCheckTest::_pub0{nullptr}; -orb_advert_t GpsRedundancyCheckTest::_pub1{nullptr}; - -// No GPS published → active_count=0 → no divergence check -TEST_F(GpsRedundancyCheckTest, NoGnss) -{ - EXPECT_FALSE(hasDivergenceWarning(false)); -} - -// Only one active receiver → divergence check requires ≥ 2 -TEST_F(GpsRedundancyCheckTest, SingleGnss) -{ - sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON); - orb_publish(ORB_ID(sensor_gps), _pub0, &gps0); - EXPECT_FALSE(hasDivergenceWarning(false)); -} - -// Two receivers ~0.4m apart (within gate of 0.785m) → no warning -TEST_F(GpsRedundancyCheckTest, TwoGnssAgreeingPrearm) -{ - sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON); - sensor_gps_s gps1 = makeGps(AGREEING_LAT, BASE_LON); - orb_publish(ORB_ID(sensor_gps), _pub0, &gps0); - orb_publish(ORB_ID(sensor_gps), _pub1, &gps1); - EXPECT_FALSE(hasDivergenceWarning(false)); -} - -// Two receivers ~2.0m apart (beyond gate), pre-arm → immediate warning (sustain=0 pre-arm) -TEST_F(GpsRedundancyCheckTest, TwoGnssDivergingPrearm) -{ - sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON); - sensor_gps_s gps1 = makeGps(DIVERGING_LAT, BASE_LON); - orb_publish(ORB_ID(sensor_gps), _pub0, &gps0); - orb_publish(ORB_ID(sensor_gps), _pub1, &gps1); - EXPECT_TRUE(hasDivergenceWarning(false)); -} - -// Two receivers ~2.0m apart, in-flight, first call → no warning yet (2s sustain not elapsed) -TEST_F(GpsRedundancyCheckTest, TwoGnssDivergingInflightNotYetSustained) -{ - sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON); - sensor_gps_s gps1 = makeGps(DIVERGING_LAT, BASE_LON); - orb_publish(ORB_ID(sensor_gps), _pub0, &gps0); - orb_publish(ORB_ID(sensor_gps), _pub1, &gps1); - EXPECT_FALSE(hasDivergenceWarning(true)); -} - -// GPS1 fix_type=2 (2D only) → treated as inactive → only 1 active receiver → no divergence check -TEST_F(GpsRedundancyCheckTest, FixTypeBelow3TreatedAsInactive) -{ - sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON); - sensor_gps_s gps1 = makeGps(DIVERGING_LAT, BASE_LON, 0.02f, 2); - orb_publish(ORB_ID(sensor_gps), _pub0, &gps0); - orb_publish(ORB_ID(sensor_gps), _pub1, &gps1); - EXPECT_FALSE(hasDivergenceWarning(false)); -} - -// Divergence with SYS_HAS_NUM_GNSS=2 and COM_GPS_LOSS_ACT>0 → gnss_lost set (failsafe fires) -TEST_F(GpsRedundancyCheckTest, DivergenceWithRedundancyRequiredSetsGnssLost) -{ - int i = 2; - param_set(param_find("SYS_HAS_NUM_GNSS"), &i); - i = 1; // Return - param_set(param_find("COM_GPS_LOSS_ACT"), &i); - delete _check; - _check = new GpsRedundancyChecks(); - - sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON); - sensor_gps_s gps1 = makeGps(DIVERGING_LAT, BASE_LON); - orb_publish(ORB_ID(sensor_gps), _pub0, &gps0); - orb_publish(ORB_ID(sensor_gps), _pub1, &gps1); - EXPECT_TRUE(hasGnssLost(false)); -} - -// Divergence with SYS_HAS_NUM_GNSS=0 and COM_GPS_LOSS_ACT>0 → warning only, gnss_lost not set -TEST_F(GpsRedundancyCheckTest, DivergenceWithoutRedundancyRequiredWarnsOnly) -{ - int i = 0; - param_set(param_find("SYS_HAS_NUM_GNSS"), &i); - i = 1; // Return - param_set(param_find("COM_GPS_LOSS_ACT"), &i); - delete _check; - _check = new GpsRedundancyChecks(); - - sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON); - sensor_gps_s gps1 = makeGps(DIVERGING_LAT, BASE_LON); - orb_publish(ORB_ID(sensor_gps), _pub0, &gps0); - orb_publish(ORB_ID(sensor_gps), _pub1, &gps1); - EXPECT_FALSE(hasGnssLost(false)); - EXPECT_TRUE(hasDivergenceWarning(false)); -} - -// Divergence then recovery: timer resets and warning stops -TEST_F(GpsRedundancyCheckTest, DivergenceClearsAfterRecovery) -{ - // First: diverging pre-arm → warning fires - sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON); - sensor_gps_s gps1 = makeGps(DIVERGING_LAT, BASE_LON); - orb_publish(ORB_ID(sensor_gps), _pub0, &gps0); - orb_publish(ORB_ID(sensor_gps), _pub1, &gps1); - EXPECT_TRUE(hasDivergenceWarning(false)); - - // Then: receivers agree → warning clears - gps0 = makeGps(BASE_LAT, BASE_LON); - gps1 = makeGps(AGREEING_LAT, BASE_LON); - orb_publish(ORB_ID(sensor_gps), _pub0, &gps0); - orb_publish(ORB_ID(sensor_gps), _pub1, &gps1); - EXPECT_FALSE(hasDivergenceWarning(false)); -} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp index 4375f3e15a..3df8887156 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp @@ -33,153 +33,79 @@ #include "gpsRedundancyCheck.hpp" -#include - -using namespace time_literals; - -// eph is firmware-dependent and not a rigorous 1-sigma, so 3 is a heuristic consistency gate -// rather than a precise statistical claim. It relaxes the gate automatically when receivers degrade. -static constexpr float GNSS_DIVERGENCE_SIGMA = 3.0f; - -// Matches the 1 s staleness threshold used by other sensor checks in this framework. -static constexpr uint64_t GNSS_ONLINE_TIMEOUT = 1_s; - -// Multipath spikes typically resolve within 1 s; 2 s filters these out while still -// detecting real receiver faults promptly. -static constexpr uint64_t GNSS_DIVERGENCE_SUSTAIN = 2_s; - void GpsRedundancyChecks::checkAndReport(const Context &context, Report &reporter) { - // Always reset — will be set below only when the condition is active reporter.failsafeFlags().gnss_lost = false; - // Separate "online" (present + fresh data) from "fixed" (online + 3D fix). - // online_count tracks receivers that are communicating; fixed_count tracks those - // suitable for navigation. Keeping them separate lets us emit a more informative - // warning: "receiver offline" vs "receiver lost fix". - int online_count = 0; - int fixed_count = 0; - sensor_gps_s fixed_gps[GPS_MAX_INSTANCES] {}; + gnss_redundancy_status_s status; - for (int i = 0; i < _sensor_gps_sub.size(); i++) { - sensor_gps_s gps{}; + if (_gnss_redundancy_status_sub.copy(&status)) { + const bool act_configured = (_param_com_gnss_loss_act.get() > 0); - if (_sensor_gps_sub[i].copy(&gps) - && (gps.device_id != 0) - && (hrt_elapsed_time(&gps.timestamp) < GNSS_ONLINE_TIMEOUT)) { - online_count++; + // Divergence triggers the failsafe only when the operator explicitly expects two + // receivers (SYS_HAS_NUM_GNSS >= 2); otherwise it remains a warning. + const bool divergence_triggers_failsafe = status.divergence_detected + && (status.num_receivers_required >= 2); - if (gps.fix_type >= 3) { - fixed_gps[fixed_count++] = gps; - } - } - } + reporter.failsafeFlags().gnss_lost = status.below_required || divergence_triggers_failsafe; - const int required = _param_sys_has_num_gnss.get(); - - // Position divergence check: warn if two fixed receivers disagree beyond their combined - // uncertainty. The gate is: lever-arm separation + GNSS_DIVERGENCE_SIGMA * RMS(eph0, eph1). - // Using RMS(eph) means the gate tightens when both receivers are accurate and widens - // automatically when one degrades, avoiding false alarms without a hard-coded threshold. - // Pre-arm: warn immediately. In-flight: require GPS_DIVERGENCE_SUSTAIN to suppress - // transient multipath spikes. - // The failsafe action is only triggered when SYS_HAS_NUM_GNSS >= 2; otherwise divergence - // is reported as a warning only. - bool divergence_active = false; - - if (fixed_count >= 2) { - float north, east; - get_vector_to_next_waypoint(fixed_gps[0].latitude_deg, fixed_gps[0].longitude_deg, - fixed_gps[1].latitude_deg, fixed_gps[1].longitude_deg, - &north, &east); - - const float divergence_m = sqrtf(north * north + east * east); - const float rms_eph = sqrtf(fixed_gps[0].eph * fixed_gps[0].eph + fixed_gps[1].eph * fixed_gps[1].eph); - const float dx = _param_gps0_offx.get() - _param_gps1_offx.get(); - const float dy = _param_gps0_offy.get() - _param_gps1_offy.get(); - const float expected_d = sqrtf(dx * dx + dy * dy); - const float gate_m = expected_d + rms_eph * GNSS_DIVERGENCE_SIGMA; - - // Pre-arm: trigger immediately so the operator can decide before takeoff. - // In-flight: require sustained divergence to avoid false alarms from transient multipath. - const uint64_t sustain = context.isArmed() ? GNSS_DIVERGENCE_SUSTAIN : 0_s; - - if (divergence_m > gate_m) { - if (_divergence_since == 0) { - _divergence_since = hrt_absolute_time(); - } - - if (hrt_elapsed_time(&_divergence_since) >= sustain) { - const bool act = (_param_com_gnss_loss_act.get() > 0) && (required >= 2); - divergence_active = act; + if (status.below_required || status.dropped_below_peak) { + const bool block_arming = status.below_required && act_configured; + const NavModes nav_modes = block_arming ? NavModes::All : NavModes::None; + const events::Log log_level = block_arming ? events::Log::Error : events::Log::Warning; + const uint8_t expected = status.below_required ? status.num_receivers_required + : status.peak_receivers_fixed; + if (status.num_receivers_online < status.num_receivers_fixed + || status.num_receivers_online < status.num_receivers_required) { /* EVENT * @description - * Two GNSS receivers report positions that are inconsistent with their reported accuracy. - * * - * Configure the failsafe action with COM_GPS_LOSS_ACT. - * The failsafe action is only triggered when SYS_HAS_NUM_GNSS is set to 2. + * Configure the minimum required GPS count with SYS_HAS_NUM_GNSS. + * Configure the failsafe action with COM_GNSS_LSS_ACT. * */ - reporter.healthFailure(act ? NavModes::All : NavModes::None, - health_component_t::gps, - events::ID("check_gps_position_divergence"), - act ? events::Log::Error : events::Log::Warning, - "GPS receivers disagree: {1:.1}m apart (gate {2:.1}m)", - (double)divergence_m, (double)gate_m); + reporter.healthFailure(nav_modes, health_component_t::gps, + events::ID("check_gps_redundancy_offline"), + log_level, + "GPS receiver offline: {1} of {2} online", + status.num_receivers_online, expected); + + } else { + /* EVENT + * @description + * + * Configure the minimum required GPS count with SYS_HAS_NUM_GNSS. + * Configure the failsafe action with COM_GNSS_LSS_ACT. + * + */ + reporter.healthFailure(nav_modes, health_component_t::gps, + events::ID("check_gps_redundancy_no_fix"), + log_level, + "GPS receiver lost 3D fix: {1} of {2} fixed", + status.num_receivers_fixed, expected); } - - } else { - _divergence_since = 0; } - } - // Track the highest fixed count seen — used to detect any GPS loss regardless of SYS_HAS_NUM_GNSS - if (fixed_count > _peak_fixed_count) { - _peak_fixed_count = fixed_count; - } + if (status.divergence_detected) { + const bool block_arming = divergence_triggers_failsafe && act_configured; + const NavModes nav_modes = block_arming ? NavModes::All : NavModes::None; + const events::Log log_level = block_arming ? events::Log::Error : events::Log::Warning; - const bool below_required = (required > 0 && fixed_count < required); - const bool dropped_below_peak = (_peak_fixed_count > 1 && fixed_count < _peak_fixed_count); - - reporter.failsafeFlags().gnss_lost = below_required || divergence_active; - - if (below_required || dropped_below_peak) { - // act==0: warn only, never blocks arming; act>0: blocks arming and shows red - const bool block_arming = below_required && (_param_com_gnss_loss_act.get() > 0); - const NavModes nav_modes = block_arming ? NavModes::All : NavModes::None; - const events::Log log_level = block_arming ? events::Log::Error : events::Log::Warning; - const uint8_t expected = below_required ? (uint8_t)required : (uint8_t)_peak_fixed_count; - - // Differentiate: if online_count is also low the receiver is offline; otherwise it lost fix. - if (online_count < fixed_count || online_count < required) { /* EVENT * @description + * Two GNSS receivers report positions that are inconsistent with their reported accuracy. + * * - * Configure the minimum required GPS count with SYS_HAS_NUM_GNSS. - * Configure the failsafe action with COM_GPS_LOSS_ACT. + * Configure the failsafe action with COM_GNSS_LSS_ACT. + * The failsafe action is only triggered when SYS_HAS_NUM_GNSS is set to 2. * */ - reporter.healthFailure(nav_modes, health_component_t::gps, - events::ID("check_gps_redundancy_offline"), - log_level, - "GPS receiver offline: {1} of {2} online", - (uint8_t)online_count, expected); - - } else { - /* EVENT - * @description - * - * Configure the minimum required GPS count with SYS_HAS_NUM_GNSS. - * Configure the failsafe action with COM_GPS_LOSS_ACT. - * - */ - reporter.healthFailure(nav_modes, health_component_t::gps, - events::ID("check_gps_redundancy_no_fix"), - log_level, - "GPS receiver lost 3D fix: {1} of {2} fixed", - (uint8_t)fixed_count, expected); + reporter.healthFailure(nav_modes, health_component_t::gps, + events::ID("check_gps_position_divergence"), + log_level, + "GPS receivers disagree: {1:.1}m apart (gate {2:.1}m)", + (double)status.divergence_m, (double)status.divergence_gate_m); } } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp index a3ae987ab4..74190cb5df 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp @@ -35,8 +35,8 @@ #include "../Common.hpp" -#include -#include +#include +#include class GpsRedundancyChecks : public HealthAndArmingCheckBase { @@ -47,20 +47,9 @@ public: void checkAndReport(const Context &context, Report &reporter) override; private: - static constexpr int GPS_MAX_INSTANCES = 2; - - uORB::SubscriptionMultiArray _sensor_gps_sub{ORB_ID::sensor_gps}; - - int _peak_fixed_count{0}; - - hrt_abstime _divergence_since{0}; + uORB::Subscription _gnss_redundancy_status_sub{ORB_ID(gnss_redundancy_status)}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, - (ParamInt) _param_sys_has_num_gnss, - (ParamInt) _param_com_gnss_loss_act, - (ParamFloat) _param_gps0_offx, - (ParamFloat) _param_gps0_offy, - (ParamFloat) _param_gps1_offx, - (ParamFloat) _param_gps1_offy + (ParamInt) _param_com_gnss_loss_act ) }; diff --git a/src/modules/commander/commander_params.yaml b/src/modules/commander/commander_params.yaml index 3bd7b96e35..299d17991f 100644 --- a/src/modules/commander/commander_params.yaml +++ b/src/modules/commander/commander_params.yaml @@ -131,7 +131,7 @@ parameters: 1: Allow arming (with warning) 2: Allow arming (no warning) default: 1 - COM_GPS_LOSS_ACT: + COM_GNSS_LSS_ACT: description: short: GNSS loss failsafe mode long: |- diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index 9007aebbbe..bb0793f685 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -219,7 +219,7 @@ private: (ParamInt) _param_com_fltt_low_act, (ParamInt) _param_com_pos_low_act, (ParamInt) _param_com_arm_odid, - (ParamInt) _param_com_gnss_loss_act + (ParamInt) _param_com_gnss_loss_act ); }; diff --git a/src/modules/sensors/vehicle_gps_position/CMakeLists.txt b/src/modules/sensors/vehicle_gps_position/CMakeLists.txt index c82a636118..270484b0a2 100644 --- a/src/modules/sensors/vehicle_gps_position/CMakeLists.txt +++ b/src/modules/sensors/vehicle_gps_position/CMakeLists.txt @@ -36,6 +36,8 @@ px4_add_library(vehicle_gps_position VehicleGPSPosition.hpp gps_blending.cpp gps_blending.hpp + GnssRedundancyMonitor.cpp + GnssRedundancyMonitor.hpp PpsTimeSync.cpp PpsTimeSync.hpp ) @@ -46,3 +48,4 @@ target_link_libraries(vehicle_gps_position ) px4_add_unit_gtest(SRC gps_blending_test.cpp LINKLIBS vehicle_gps_position) +px4_add_functional_gtest(SRC GnssRedundancyMonitorTest.cpp LINKLIBS vehicle_gps_position) diff --git a/src/modules/sensors/vehicle_gps_position/GnssRedundancyMonitor.cpp b/src/modules/sensors/vehicle_gps_position/GnssRedundancyMonitor.cpp new file mode 100644 index 0000000000..204e0f2889 --- /dev/null +++ b/src/modules/sensors/vehicle_gps_position/GnssRedundancyMonitor.cpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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. + * + ****************************************************************************/ + +#include "GnssRedundancyMonitor.hpp" + +#include + +using namespace time_literals; + +namespace sensors +{ + +// eph is firmware-dependent and not a rigorous 1-sigma, so 3 is a heuristic consistency gate +// rather than a precise statistical claim. It relaxes the gate automatically when receivers degrade. +static constexpr float GNSS_DIVERGENCE_SIGMA = 3.0f; + +// Matches the 1 s staleness threshold used by other sensor checks. +static constexpr uint64_t GNSS_ONLINE_TIMEOUT = 1_s; + +// Multipath spikes typically resolve within 1 s; 2 s filters them while still +// detecting real receiver faults promptly. +static constexpr uint64_t GNSS_DIVERGENCE_SUSTAIN = 2_s; + +GnssRedundancyMonitor::GnssRedundancyMonitor(ModuleParams *parent) : + ModuleParams(parent) +{ +} + +void GnssRedundancyMonitor::update(const sensor_gps_s *gps_states, + const matrix::Vector3f *antenna_offsets, + uint8_t num_receivers, + bool is_armed) +{ + // Separate "online" (present + fresh data) from "fixed" (online + 3D fix). + // Keeping them separate lets the consumer distinguish "receiver offline" + // from "receiver lost fix" in its user-facing events. + uint8_t online_count = 0; + uint8_t fixed_count = 0; + uint8_t fixed_idx[2] {}; + + for (uint8_t i = 0; i < num_receivers && i < 2; i++) { + const sensor_gps_s &gps = gps_states[i]; + + if (gps.device_id != 0 + && hrt_elapsed_time(&gps.timestamp) < GNSS_ONLINE_TIMEOUT) { + online_count++; + + if (gps.fix_type >= 3) { + fixed_idx[fixed_count++] = i; + } + } + } + + // Position divergence check: flag if two fixed receivers disagree beyond their + // combined uncertainty. Gate = lever-arm separation + sigma * RMS(eph). + // Pre-arm: trigger immediately so the operator can decide before takeoff. + // In-flight: require sustained divergence to suppress transient multipath. + bool divergence_detected = false; + float divergence_m = 0.f; + float divergence_gate_m = 0.f; + + if (fixed_count >= 2) { + const sensor_gps_s &gps0 = gps_states[fixed_idx[0]]; + const sensor_gps_s &gps1 = gps_states[fixed_idx[1]]; + + float north, east; + get_vector_to_next_waypoint(gps0.latitude_deg, gps0.longitude_deg, + gps1.latitude_deg, gps1.longitude_deg, + &north, &east); + divergence_m = sqrtf(north * north + east * east); + + const float rms_eph = sqrtf(gps0.eph * gps0.eph + gps1.eph * gps1.eph); + const matrix::Vector3f lever_arm = antenna_offsets[fixed_idx[0]] - antenna_offsets[fixed_idx[1]]; + const float expected_d = sqrtf(lever_arm(0) * lever_arm(0) + lever_arm(1) * lever_arm(1)); + divergence_gate_m = expected_d + rms_eph * GNSS_DIVERGENCE_SIGMA; + + const uint64_t sustain = is_armed ? GNSS_DIVERGENCE_SUSTAIN : 0_s; + + if (divergence_m > divergence_gate_m) { + if (_divergence_since == 0) { + _divergence_since = hrt_absolute_time(); + } + + if (hrt_elapsed_time(&_divergence_since) >= sustain) { + divergence_detected = true; + } + + } else { + _divergence_since = 0; + } + + } else { + _divergence_since = 0; + } + + // Track the highest fixed count seen — used by consumers to detect any + // GPS loss regardless of SYS_HAS_NUM_GNSS. + if (fixed_count > _peak_fixed_count) { + _peak_fixed_count = fixed_count; + } + + const uint8_t required = (uint8_t)_param_sys_has_num_gnss.get(); + + gnss_redundancy_status_s status{}; + status.num_receivers_online = online_count; + status.num_receivers_fixed = fixed_count; + status.num_receivers_required = required; + status.peak_receivers_fixed = _peak_fixed_count; + status.below_required = (required > 0) && (fixed_count < required); + status.dropped_below_peak = (_peak_fixed_count > 1) && (fixed_count < _peak_fixed_count); + status.divergence_detected = divergence_detected; + status.divergence_m = divergence_m; + status.divergence_gate_m = divergence_gate_m; + status.timestamp = hrt_absolute_time(); + + _gnss_redundancy_status_pub.publish(status); +} + +} // namespace sensors diff --git a/src/modules/sensors/vehicle_gps_position/GnssRedundancyMonitor.hpp b/src/modules/sensors/vehicle_gps_position/GnssRedundancyMonitor.hpp new file mode 100644 index 0000000000..2cc0ea8b03 --- /dev/null +++ b/src/modules/sensors/vehicle_gps_position/GnssRedundancyMonitor.hpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "gps_blending.hpp" + +namespace sensors +{ + +// Computes system-wide GNSS redundancy health from the last-known sample of +// each receiver and publishes it on the gnss_redundancy_status topic. +// Kept separate from VehicleGPSPosition so the detection logic is small and +// unit-testable; instantiated and driven by VehicleGPSPosition each cycle. +class GnssRedundancyMonitor : public ModuleParams +{ +public: + GnssRedundancyMonitor(ModuleParams *parent); + + // Evaluate the current state given the per-instance samples and resolved + // antenna offsets (as maintained by the blender), and publish the status. + void update(const sensor_gps_s *gps_states, + const matrix::Vector3f *antenna_offsets, + uint8_t num_receivers, + bool is_armed); + +private: + uORB::Publication _gnss_redundancy_status_pub{ORB_ID(gnss_redundancy_status)}; + + uint8_t _peak_fixed_count{0}; + hrt_abstime _divergence_since{0}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams, + (ParamInt) _param_sys_has_num_gnss + ) +}; + +} // namespace sensors diff --git a/src/modules/sensors/vehicle_gps_position/GnssRedundancyMonitorTest.cpp b/src/modules/sensors/vehicle_gps_position/GnssRedundancyMonitorTest.cpp new file mode 100644 index 0000000000..1b57507f94 --- /dev/null +++ b/src/modules/sensors/vehicle_gps_position/GnssRedundancyMonitorTest.cpp @@ -0,0 +1,205 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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. + * + ****************************************************************************/ + +#include + +#include "GnssRedundancyMonitor.hpp" + +#include +#include +#include +#include +#include + +// to run: make tests TESTFILTER=GnssRedundancyMonitor + +// Base position (PX4 SITL default home) +static constexpr double BASE_LAT = 47.397742; +static constexpr double BASE_LON = 8.545594; + +// 1 deg latitude ≈ 111111 m +// AGREEING: ~0.4m north → below gate of 0.785m (0.70m lever arm + 0.085m eph tolerance) +// DIVERGING: ~2.0m north → above gate +static constexpr double AGREEING_LAT = BASE_LAT + 3.6e-6; +static constexpr double DIVERGING_LAT = BASE_LAT + 1.8e-5; + +class GnssRedundancyMonitorTest : public ::testing::Test +{ +public: + void SetUp() override + { + param_control_autosave(false); + + int required = 0; + param_set(param_find("SYS_HAS_NUM_GNSS"), &required); + + _monitor = new sensors::GnssRedundancyMonitor(nullptr); + + // Lever arm: GPS0 at +35cm forward, GPS1 at -35cm → expected_d = 0.70m + // With both RTK Fixed (eph=0.02m): gate = 0.70 + 3*√(0.02²+0.02²) = 0.785m + _offsets[0] = matrix::Vector3f(0.35f, 0.f, 0.f); + _offsets[1] = matrix::Vector3f(-0.35f, 0.f, 0.f); + + _states[0] = sensor_gps_s{}; + _states[1] = sensor_gps_s{}; + } + + void TearDown() override + { + delete _monitor; + _monitor = nullptr; + } + + sensor_gps_s makeGps(double lat, double lon, float eph = 0.02f, uint8_t fix_type = 6) + { + sensor_gps_s gps{}; + gps.timestamp = hrt_absolute_time(); + gps.device_id = 1; + gps.latitude_deg = lat; + gps.longitude_deg = lon; + gps.altitude_ellipsoid_m = 100.0; + gps.eph = eph; + gps.epv = 0.05f; + gps.fix_type = fix_type; + return gps; + } + + gnss_redundancy_status_s runUpdate(bool armed) + { + _monitor->update(_states, _offsets, 2, armed); + gnss_redundancy_status_s status{}; + _status_sub.copy(&status); + return status; + } + + sensor_gps_s _states[2] {}; + matrix::Vector3f _offsets[2] {}; + sensors::GnssRedundancyMonitor *_monitor{nullptr}; + uORB::Subscription _status_sub{ORB_ID(gnss_redundancy_status)}; +}; + +// No data → no divergence +TEST_F(GnssRedundancyMonitorTest, NoGnss) +{ + auto status = runUpdate(false); + EXPECT_FALSE(status.divergence_detected); + EXPECT_EQ(status.num_receivers_fixed, 0); +} + +// Only one fixed receiver → divergence check requires ≥ 2 +TEST_F(GnssRedundancyMonitorTest, SingleGnss) +{ + _states[0] = makeGps(BASE_LAT, BASE_LON); + auto status = runUpdate(false); + EXPECT_FALSE(status.divergence_detected); + EXPECT_EQ(status.num_receivers_fixed, 1); +} + +// Two receivers within gate → no divergence +TEST_F(GnssRedundancyMonitorTest, TwoGnssAgreeingPrearm) +{ + _states[0] = makeGps(BASE_LAT, BASE_LON); + _states[1] = makeGps(AGREEING_LAT, BASE_LON); + auto status = runUpdate(false); + EXPECT_FALSE(status.divergence_detected); +} + +// Two receivers beyond gate, pre-arm → immediate divergence (sustain=0 pre-arm) +TEST_F(GnssRedundancyMonitorTest, TwoGnssDivergingPrearm) +{ + _states[0] = makeGps(BASE_LAT, BASE_LON); + _states[1] = makeGps(DIVERGING_LAT, BASE_LON); + auto status = runUpdate(false); + EXPECT_TRUE(status.divergence_detected); +} + +// Two receivers beyond gate, in-flight, first call → sustain not elapsed +TEST_F(GnssRedundancyMonitorTest, TwoGnssDivergingInflightNotYetSustained) +{ + _states[0] = makeGps(BASE_LAT, BASE_LON); + _states[1] = makeGps(DIVERGING_LAT, BASE_LON); + auto status = runUpdate(true); + EXPECT_FALSE(status.divergence_detected); +} + +// fix_type < 3 → treated as inactive → only 1 fixed receiver → no divergence check +TEST_F(GnssRedundancyMonitorTest, FixTypeBelow3TreatedAsInactive) +{ + _states[0] = makeGps(BASE_LAT, BASE_LON); + _states[1] = makeGps(DIVERGING_LAT, BASE_LON, 0.02f, 2); + auto status = runUpdate(false); + EXPECT_FALSE(status.divergence_detected); + EXPECT_EQ(status.num_receivers_fixed, 1); +} + +// Divergence then recovery: flag clears when positions agree again +TEST_F(GnssRedundancyMonitorTest, DivergenceClearsAfterRecovery) +{ + _states[0] = makeGps(BASE_LAT, BASE_LON); + _states[1] = makeGps(DIVERGING_LAT, BASE_LON); + EXPECT_TRUE(runUpdate(false).divergence_detected); + + _states[0] = makeGps(BASE_LAT, BASE_LON); + _states[1] = makeGps(AGREEING_LAT, BASE_LON); + EXPECT_FALSE(runUpdate(false).divergence_detected); +} + +// Below-required is reported when fixed count < SYS_HAS_NUM_GNSS +TEST_F(GnssRedundancyMonitorTest, BelowRequiredFlag) +{ + int required = 2; + param_set(param_find("SYS_HAS_NUM_GNSS"), &required); + delete _monitor; + _monitor = new sensors::GnssRedundancyMonitor(nullptr); + + _states[0] = makeGps(BASE_LAT, BASE_LON); + auto status = runUpdate(false); + EXPECT_TRUE(status.below_required); + EXPECT_EQ(status.num_receivers_fixed, 1); + EXPECT_EQ(status.num_receivers_required, 2); +} + +// Peak tracking: once we've seen 2 fixed, losing one is reported as dropped_below_peak +TEST_F(GnssRedundancyMonitorTest, DroppedBelowPeakFlag) +{ + _states[0] = makeGps(BASE_LAT, BASE_LON); + _states[1] = makeGps(AGREEING_LAT, BASE_LON); + auto status = runUpdate(false); + EXPECT_EQ(status.peak_receivers_fixed, 2); + + _states[1] = sensor_gps_s{}; + status = runUpdate(false); + EXPECT_TRUE(status.dropped_below_peak); + EXPECT_EQ(status.num_receivers_fixed, 1); + EXPECT_EQ(status.peak_receivers_fixed, 2); +} diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index 3064519847..e66b52a55c 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -191,7 +191,21 @@ void VehicleGPSPosition::Run() if (any_gps_updated) { _gps_blending.update(hrt_absolute_time()); + } + // Tick the redundancy monitor every cycle so silence is detected. + { + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + const bool is_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + + _gnss_redundancy_monitor.update(_gps_blending.getGpsStates(), + _gps_blending.getAntennaOffsets(), + GPS_MAX_RECEIVERS, + is_armed); + } + + if (any_gps_updated) { if (_gps_blending.isNewOutputDataAvailable()) { sensor_gps_s gps_output{_gps_blending.getOutputGpsData()}; diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp index bf3cc6635f..41e46e5bd2 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp @@ -46,7 +46,9 @@ #include #include #include +#include +#include "GnssRedundancyMonitor.hpp" #include "gps_blending.hpp" #include "PpsTimeSync.hpp" @@ -91,11 +93,13 @@ private: }; uORB::Subscription _pps_capture_sub{ORB_ID(pps_capture)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; GpsBlending _gps_blending; PpsTimeSync _pps_time_sync; + GnssRedundancyMonitor _gnss_redundancy_monitor{this}; struct GpsParamSlot { uint32_t device_id{0}; diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp b/src/modules/sensors/vehicle_gps_position/gps_blending.hpp index 7514014e59..d934996b16 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.hpp @@ -99,6 +99,12 @@ public: } int getSelectedGps() const { return _selected_gps; } + // Exposed for the redundancy monitor so it can inspect the last known + // sample (and its resolved antenna offset) from each instance without + // maintaining a second set of subscriptions. + const sensor_gps_s *getGpsStates() const { return _gps_state; } + const Vector3f *getAntennaOffsets() const { return _antenna_offset; } + private: /* * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical