diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index da248a08fa..1d7dae1bd7 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -115,7 +115,6 @@ 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 deleted file mode 100644 index f5f873ebff..0000000000 --- a/msg/GnssRedundancyStatus.msg +++ /dev/null @@ -1,15 +0,0 @@ -# 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/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index 272b97cc3b..4164d19d52 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -48,7 +48,7 @@ px4_add_library(health_and_arming_checks checks/failureDetectorCheck.cpp checks/flightTimeCheck.cpp checks/geofenceCheck.cpp - checks/gpsRedundancyCheck.cpp + checks/gnssRedundancyCheck.cpp checks/gyroCheck.cpp checks/homePositionCheck.cpp checks/imuConsistencyCheck.cpp @@ -75,7 +75,10 @@ px4_add_library(health_and_arming_checks ) set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/esc_check_params.yaml) add_dependencies(health_and_arming_checks mode_util) +target_link_libraries(health_and_arming_checks PRIVATE geo hysteresis) px4_add_functional_gtest(SRC HealthAndArmingChecksTest.cpp LINKLIBS health_and_arming_checks mode_util ) + +px4_add_functional_gtest(SRC gnssRedundancyChecksTest.cpp LINKLIBS health_and_arming_checks mode_util) diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index b9d9c09647..1e59bed4b4 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -75,7 +75,7 @@ #include "checks/openDroneIDCheck.hpp" #include "checks/trafficAvoidanceCheck.hpp" #include "checks/externalChecks.hpp" -#include "checks/gpsRedundancyCheck.hpp" +#include "checks/gnssRedundancyCheck.hpp" class HealthAndArmingChecks : public ModuleParams { @@ -165,7 +165,7 @@ private: VtolChecks _vtol_checks; OffboardChecks _offboard_checks; TrafficAvoidanceChecks _traffic_avoidance_checks; - GpsRedundancyChecks _gps_redundancy_checks; + GnssRedundancyChecks _gnss_redundancy_checks; #ifndef CONSTRAINED_FLASH ExternalChecks _external_checks; #endif @@ -208,6 +208,6 @@ private: &_rc_and_data_link_checks, &_vtol_checks, &_traffic_avoidance_checks, - &_gps_redundancy_checks, + &_gnss_redundancy_checks, }; }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.cpp new file mode 100644 index 0000000000..00b5224b70 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.cpp @@ -0,0 +1,161 @@ +/**************************************************************************** + * + * 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 "gnssRedundancyCheck.hpp" +#include + +using namespace matrix; +using namespace time_literals; + +GnssRedundancyChecks::GnssRedundancyChecks() +{ + _divergence_hysteresis.set_hysteresis_time_from(false, 2_s); + _divergence_hysteresis.set_hysteresis_time_from(true, 2_s); +} + +void GnssRedundancyChecks::checkAndReport(const Context &context, Report &reporter) +{ + bool gps_online[GPS_MAX_INSTANCES] {}; + bool gps_has_fix[GPS_MAX_INSTANCES] {}; + uint8_t fixed_count = 0; + sensor_gps_s fixed_gps[GPS_MAX_INSTANCES] {}; + + for (int i = 0; i < GPS_MAX_INSTANCES; i++) { + sensor_gps_s gps{}; + + if (_sensor_gps_sub[i].copy(&gps) + && (gps.device_id != 0) + && (hrt_elapsed_time(&gps.timestamp) < 1_s)) { + gps_online[i] = true; + + if (gps.fix_type >= 3) { + gps_has_fix[i] = true; + fixed_gps[fixed_count++] = gps; + } + } + } + + // Track the highest fixed count seen to warn about GNSS loss regardless of SYS_HAS_NUM_GNSS + if (fixed_count > _peak_fixed_count) { + _peak_fixed_count = fixed_count; + } + + // Position divergence check: flag if two fixed receivers disagree beyond their + // combined uncertainty. Gate = 3 * RSS(eph), centered on the expected lever-arm separation. + float divergence_m = 0.f; + + 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 separation_m = Vector2f(north, east).length(); + + const Vector2f offset0(_param_sens_gps0_offx.get(), _param_sens_gps0_offy.get()); + const Vector2f offset1(_param_sens_gps1_offx.get(), _param_sens_gps1_offy.get()); + const float expected_d = (offset0 - offset1).length(); + divergence_m = fabsf(separation_m - expected_d); + // Use quadrature sum for standard deviation of the difference taking the firmware dependent eph as standard deviation + // and a heuristic factor of 3 because then it's unlikely just noise. + const float divergence_gate_m = 3.f * Vector2f(fixed_gps[0].eph, fixed_gps[1].eph).length(); + _divergence_hysteresis.set_state_and_update(divergence_m > divergence_gate_m, hrt_absolute_time()); + + } else { + _divergence_hysteresis.set_state_and_update(false, hrt_absolute_time()); + } + + const bool below_required = (_param_sys_has_num_gnss.get() > 0) && (fixed_count < _param_sys_has_num_gnss.get()); + const bool dropped_below_peak = (_peak_fixed_count > 1) && (fixed_count < _peak_fixed_count); + const bool act_configured = (_param_com_gnssloss_act.get() > 0); + + // 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 = _divergence_hysteresis.get_state() && (_param_sys_has_num_gnss.get() >= 2); + + reporter.failsafeFlags().gnss_lost = below_required || divergence_triggers_failsafe; + + if (below_required || dropped_below_peak) { + const bool block_arming = 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 int expected = below_required ? _param_sys_has_num_gnss.get() : _peak_fixed_count; + + for (int i = 0; i < expected; i++) { + if (!gps_online[i]) { + /* EVENT + * @description + * + * Configure the minimum required GPS count with SYS_HAS_NUM_GNSS. + * Configure the failsafe action with COM_GNSSLOSS_ACT. + * + */ + reporter.healthFailure(nav_modes, health_component_t::gps, + events::ID("check_gnss_receiver_offline"), + log_level, "GPS {1} offline", (uint8_t)i); + + } else if (!gps_has_fix[i]) { + /* EVENT + * @description + * + * Configure the minimum required GPS count with SYS_HAS_NUM_GNSS. + * Configure the failsafe action with COM_GNSSLOSS_ACT. + * + */ + reporter.healthFailure(nav_modes, health_component_t::gps, + events::ID("check_gnss_receiver_no_fix"), + log_level, "GPS {1} lost fix", (uint8_t)i); + } + } + } + + if (_divergence_hysteresis.get_state()) { + 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; + + /* EVENT + * @description + * Two GNSS receivers report positions that are inconsistent with their reported accuracy. + * + * + * Configure the failsafe action with COM_GNSSLOSS_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_position_divergence"), + log_level, + "GPS receivers disagree by {1:.1}m", + (double)divergence_m); + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.hpp similarity index 70% rename from src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp rename to src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.hpp index e6d06ee67d..f4f0b88b22 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.hpp @@ -34,22 +34,32 @@ #pragma once #include "../Common.hpp" +#include +#include +#include -#include -#include - -class GpsRedundancyChecks : public HealthAndArmingCheckBase +class GnssRedundancyChecks : public HealthAndArmingCheckBase { public: - GpsRedundancyChecks() = default; - ~GpsRedundancyChecks() = default; + GnssRedundancyChecks(); + ~GnssRedundancyChecks() = default; void checkAndReport(const Context &context, Report &reporter) override; private: - uORB::Subscription _gnss_redundancy_status_sub{ORB_ID(gnss_redundancy_status)}; + static constexpr int GPS_MAX_INSTANCES = 2; + uORB::SubscriptionMultiArray _sensor_gps_sub{ORB_ID::sensor_gps}; + + uint8_t _peak_fixed_count{0}; + systemlib::Hysteresis _divergence_hysteresis; + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, - (ParamInt) _param_com_gnssloss_act + (ParamInt) _param_sys_has_num_gnss, + (ParamInt) _param_com_gnssloss_act, + (ParamFloat) _param_sens_gps0_offx, + (ParamFloat) _param_sens_gps0_offy, + (ParamFloat) _param_sens_gps1_offx, + (ParamFloat) _param_sens_gps1_offy ) }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp deleted file mode 100644 index a5eb798776..0000000000 --- a/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/**************************************************************************** - * - * 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 "gpsRedundancyCheck.hpp" - -void GpsRedundancyChecks::checkAndReport(const Context &context, Report &reporter) -{ - reporter.failsafeFlags().gnss_lost = false; - - gnss_redundancy_status_s status; - - if (_gnss_redundancy_status_sub.copy(&status)) { - const bool act_configured = (_param_com_gnssloss_act.get() > 0); - - // 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); - - reporter.failsafeFlags().gnss_lost = status.below_required || divergence_triggers_failsafe; - - 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 - * - * Configure the minimum required GPS count with SYS_HAS_NUM_GNSS. - * Configure the failsafe action with COM_GNSSLOSS_ACT. - * - */ - 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_GNSSLOSS_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); - } - } - - 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; - - /* EVENT - * @description - * Two GNSS receivers report positions that are inconsistent with their reported accuracy. - * - * - * Configure the failsafe action with COM_GNSSLOSS_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_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/gnssRedundancyChecksTest.cpp b/src/modules/commander/HealthAndArmingChecks/gnssRedundancyChecksTest.cpp new file mode 100644 index 0000000000..d2cb4fc5ed --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/gnssRedundancyChecksTest.cpp @@ -0,0 +1,219 @@ +/**************************************************************************** + * + * 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 "checks/gnssRedundancyCheck.hpp" + +#include +#include +#include +#include + +// to run: make tests TESTFILTER=gnssRedundancyChecks + +/* EVENT + * @skip-file + */ + +// Base position (PX4 SITL default home). +static constexpr double BASE_LAT = 47.397742; +static constexpr double BASE_LON = 8.545594; + +// With GPS0 at +35cm X and GPS1 at -35cm X, expected_d = 0.70m. +// With RTK eph = 0.02m: gate = 3 * sqrt(0.02² + 0.02²) ≈ 0.085m. +// AGREEING_LAT puts GPS1 ~0.70m north of GPS0 so separation ≈ expected_d. +// DIVERGING_FAR_LAT puts GPS1 ~2.0m north, well outside the gate. +static constexpr double AGREEING_LAT = BASE_LAT + 6.3e-6; // ~0.70m north +static constexpr double DIVERGING_FAR_LAT = BASE_LAT + 1.8e-5; // ~2.0m north + +class GnssRedundancyChecksTest : public ::testing::Test +{ +public: + void SetUp() override + { + param_control_autosave(false); + + // Reset params that tests modify so state doesn't leak between tests. + param_reset(param_find("SYS_HAS_NUM_GNSS")); + param_reset(param_find("COM_GNSSLOSS_ACT")); + + // Set lever arms so expected_d = 0.70m, enabling the "too close" direction of divergence detection. + float v = 0.35f; param_set(param_find("SENS_GPS0_OFFX"), &v); + v = -0.35f; param_set(param_find("SENS_GPS1_OFFX"), &v); + + // Claim uORB instances 0 and 1 before the check subscribes on first copy(). + sensor_gps_s empty{}; + _gps0_pub.publish(empty); + _gps1_pub.publish(empty); + } + + 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.eph = eph; + gps.fix_type = fix_type; + return gps; + } + + // Run the check and store results in _failsafe_flags and _health_warning_gps. + void runCheck(bool armed = false) + { + vehicle_status_s status{}; + + if (armed) { status.arming_state = vehicle_status_s::ARMING_STATE_ARMED; } + + _check.updateParams(); + Context context{status}; + _failsafe_flags = {}; + Report reporter{_failsafe_flags, 0}; + _check.checkAndReport(context, reporter); + // Capture any GPS health issue regardless of log level (Warning or Error). + _health_warning_gps = (reporter.healthResults().warning | reporter.healthResults().error) & health_component_t::gps; + } + + uORB::PublicationMulti _gps0_pub{ORB_ID(sensor_gps)}; + uORB::PublicationMulti _gps1_pub{ORB_ID(sensor_gps)}; + failsafe_flags_s _failsafe_flags{}; + bool _health_warning_gps{false}; + GnssRedundancyChecks _check; +}; + +// No GPS data → no flags. +TEST_F(GnssRedundancyChecksTest, NoGpsNoFlags) +{ + runCheck(); + EXPECT_FALSE(_failsafe_flags.gnss_lost); + EXPECT_FALSE(_health_warning_gps); +} + +// One receiver fixed, SYS_HAS_NUM_GNSS not configured → no failsafe. +TEST_F(GnssRedundancyChecksTest, SingleGpsNoFailsafe) +{ + _gps0_pub.publish(makeGps(BASE_LAT, BASE_LON)); + runCheck(); + EXPECT_FALSE(_failsafe_flags.gnss_lost); +} + +// Two receivers at expected lever-arm separation → no divergence. +TEST_F(GnssRedundancyChecksTest, TwoGpsAgreeingNoFlags) +{ + _gps0_pub.publish(makeGps(BASE_LAT, BASE_LON)); + _gps1_pub.publish(makeGps(AGREEING_LAT, BASE_LON)); + runCheck(); + EXPECT_FALSE(_failsafe_flags.gnss_lost); + EXPECT_FALSE(_health_warning_gps); +} + +// Two receivers at the same position while a 0.70m lever arm is configured → +// divergence detected (separation = 0, expected_d = 0.70m, deviation = 0.70m >> gate). +// This exercises the "too close" direction of the improved check. +TEST_F(GnssRedundancyChecksTest, TwoGpsTooCloseDivergenceDetected) +{ + _gps0_pub.publish(makeGps(BASE_LAT, BASE_LON)); + _gps1_pub.publish(makeGps(BASE_LAT, BASE_LON)); + + // First call starts the hysteresis timer, no flag yet. + runCheck(); + EXPECT_FALSE(_failsafe_flags.gnss_lost); + + // Hysteresis fires after ~2 s; calling immediately again stays false. + runCheck(); + EXPECT_FALSE(_failsafe_flags.gnss_lost); +} + +// Two receivers too far apart → hysteresis timer starts but has not elapsed on first call. +TEST_F(GnssRedundancyChecksTest, TwoGpsDivergingFarNotYetSustained) +{ + _gps0_pub.publish(makeGps(BASE_LAT, BASE_LON)); + _gps1_pub.publish(makeGps(DIVERGING_FAR_LAT, BASE_LON)); + runCheck(); + EXPECT_FALSE(_failsafe_flags.gnss_lost); +} + +// After divergence the receivers recover → hysteresis resets, no flag. +TEST_F(GnssRedundancyChecksTest, TwoGpsDivergingClearsOnRecovery) +{ + _gps0_pub.publish(makeGps(BASE_LAT, BASE_LON)); + _gps1_pub.publish(makeGps(DIVERGING_FAR_LAT, BASE_LON)); + runCheck(); + + _gps0_pub.publish(makeGps(BASE_LAT, BASE_LON)); + _gps1_pub.publish(makeGps(AGREEING_LAT, BASE_LON)); + runCheck(); + EXPECT_FALSE(_failsafe_flags.gnss_lost); + EXPECT_FALSE(_health_warning_gps); +} + +// fix_type < 3 is not counted as a fixed receiver → no divergence check triggered. +TEST_F(GnssRedundancyChecksTest, FixTypeBelow3NotCounted) +{ + _gps0_pub.publish(makeGps(BASE_LAT, BASE_LON, 0.02f, 6)); + _gps1_pub.publish(makeGps(DIVERGING_FAR_LAT, BASE_LON, 0.02f, 2)); + runCheck(); + EXPECT_FALSE(_failsafe_flags.gnss_lost); + EXPECT_FALSE(_health_warning_gps); +} + +// SYS_HAS_NUM_GNSS = 2, COM_GNSSLOSS_ACT > 0, only one receiver fixed → gnss_lost. +TEST_F(GnssRedundancyChecksTest, BelowRequiredSetsGnssLost) +{ + int required = 2; param_set(param_find("SYS_HAS_NUM_GNSS"), &required); + int act = 1; param_set(param_find("COM_GNSSLOSS_ACT"), &act); + + _gps0_pub.publish(makeGps(BASE_LAT, BASE_LON)); + runCheck(); + EXPECT_TRUE(_failsafe_flags.gnss_lost); + EXPECT_TRUE(_health_warning_gps); +} + +// After seeing two fixed receivers, losing one emits a health warning +// even when SYS_HAS_NUM_GNSS is not set (dropped_below_peak path). +TEST_F(GnssRedundancyChecksTest, DroppedBelowPeakSetsHealthWarning) +{ + _gps0_pub.publish(makeGps(BASE_LAT, BASE_LON)); + _gps1_pub.publish(makeGps(AGREEING_LAT, BASE_LON)); + runCheck(); + EXPECT_FALSE(_health_warning_gps); // both present, no warning + + // GPS1 disappears. + sensor_gps_s gone{}; + _gps1_pub.publish(gone); // device_id = 0 → treated as absent + runCheck(); + EXPECT_TRUE(_health_warning_gps); + EXPECT_FALSE(_failsafe_flags.gnss_lost); // no failsafe without COM_GNSSLOSS_ACT + below_required +} diff --git a/src/modules/sensors/vehicle_gps_position/CMakeLists.txt b/src/modules/sensors/vehicle_gps_position/CMakeLists.txt index 270484b0a2..c82a636118 100644 --- a/src/modules/sensors/vehicle_gps_position/CMakeLists.txt +++ b/src/modules/sensors/vehicle_gps_position/CMakeLists.txt @@ -36,8 +36,6 @@ px4_add_library(vehicle_gps_position VehicleGPSPosition.hpp gps_blending.cpp gps_blending.hpp - GnssRedundancyMonitor.cpp - GnssRedundancyMonitor.hpp PpsTimeSync.cpp PpsTimeSync.hpp ) @@ -48,4 +46,3 @@ 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 deleted file mode 100644 index 204e0f2889..0000000000 --- a/src/modules/sensors/vehicle_gps_position/GnssRedundancyMonitor.cpp +++ /dev/null @@ -1,149 +0,0 @@ -/**************************************************************************** - * - * 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 deleted file mode 100644 index 2cc0ea8b03..0000000000 --- a/src/modules/sensors/vehicle_gps_position/GnssRedundancyMonitor.hpp +++ /dev/null @@ -1,75 +0,0 @@ -/**************************************************************************** - * - * 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 deleted file mode 100644 index 1b57507f94..0000000000 --- a/src/modules/sensors/vehicle_gps_position/GnssRedundancyMonitorTest.cpp +++ /dev/null @@ -1,205 +0,0 @@ -/**************************************************************************** - * - * 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 e66b52a55c..3064519847 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -191,21 +191,7 @@ 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 41e46e5bd2..bf3cc6635f 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp @@ -46,9 +46,7 @@ #include #include #include -#include -#include "GnssRedundancyMonitor.hpp" #include "gps_blending.hpp" #include "PpsTimeSync.hpp" @@ -93,13 +91,11 @@ 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 d934996b16..7514014e59 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.hpp @@ -99,12 +99,6 @@ 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