From b589115dc326265cdf5cd6fee9a7486a9c2c7d25 Mon Sep 17 00:00:00 2001 From: gguidone Date: Mon, 23 Mar 2026 16:41:37 +0100 Subject: [PATCH] feat(gpsRedundancyCheck): add GPS redundancy failsafe with divergence check - Monitors GPS count and triggers configurable failsafe (COM_GPS_LOSS_ACT) when count drops below SYS_HAS_NUM_GPS - Tracks online (present+fresh) and fixed (3D fix) receivers separately; emits "receiver offline" vs "receiver lost fix" - Detects position divergence between two receivers against combined RMS eph uncertainty plus lever-arm separation - Pre-arm warns immediately; in-flight requires 2s sustained divergence to suppress multipath false alarms - Adds GpsRedundancyCheckTest functional test suite New parameters: SYS_HAS_NUM_GPS, COM_GPS_LOSS_ACT --- msg/FailsafeFlags.msg | 1 + src/lib/systemlib/system_params.yaml | 12 + .../HealthAndArmingChecks/CMakeLists.txt | 5 + .../GpsRedundancyCheckTest.cpp | 217 ++++++++++++++++++ .../HealthAndArmingChecks.hpp | 3 + .../checks/gpsRedundancyCheck.cpp | 181 +++++++++++++++ .../checks/gpsRedundancyCheck.hpp | 66 ++++++ src/modules/commander/commander_params.yaml | 13 ++ src/modules/commander/failsafe/failsafe.cpp | 31 +++ src/modules/commander/failsafe/failsafe.h | 11 +- 10 files changed, 539 insertions(+), 1 deletion(-) create mode 100644 src/modules/commander/HealthAndArmingChecks/GpsRedundancyCheckTest.cpp create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg index 2d974791f7..e0094a575c 100644 --- a/msg/FailsafeFlags.msg +++ b/msg/FailsafeFlags.msg @@ -59,3 +59,4 @@ bool flight_time_limit_exceeded # Maximum flight time exceeded bool position_accuracy_low # Position estimate has dropped below threshold, but is currently still declared valid bool navigator_failure # Navigator failed to execute a mode bool parachute_unhealthy # Parachute system missing or unhealthy +bool gps_redundancy_lost # Number of active GPS receivers dropped below COM_GPS_MIN diff --git a/src/lib/systemlib/system_params.yaml b/src/lib/systemlib/system_params.yaml index ec8b55d8ce..e1c7d2cb86 100644 --- a/src/lib/systemlib/system_params.yaml +++ b/src/lib/systemlib/system_params.yaml @@ -136,6 +136,18 @@ parameters: type: boolean default: 1 reboot_required: true + SYS_HAS_NUM_GPS: + description: + short: Control if and how many GPS receivers are expected + long: |- + 0: Disable GPS redundancy check (any number of GPS is acceptable). + 1-N: Require the presence of N GPS receivers for arming and during flight. + If the active count drops below this value in flight, COM_GPS_LOSS_ACT is triggered. + type: int32 + default: 0 + min: 0 + max: 2 + reboot_required: false SYS_HAS_MAG: description: short: Control if and how many magnetometers are expected diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index ad18a2ad79..044aaad4b3 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -48,6 +48,7 @@ px4_add_library(health_and_arming_checks checks/failureDetectorCheck.cpp checks/flightTimeCheck.cpp checks/geofenceCheck.cpp + checks/gpsRedundancyCheck.cpp checks/gyroCheck.cpp checks/homePositionCheck.cpp checks/imuConsistencyCheck.cpp @@ -78,3 +79,7 @@ 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 new file mode 100644 index 0000000000..67820e5f34 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/GpsRedundancyCheckTest.cpp @@ -0,0 +1,217 @@ +/**************************************************************************** + * + * 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_GPS"), &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; + } + + 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, no_gps) +{ + EXPECT_FALSE(hasDivergenceWarning(false)); +} + +// Only one active receiver → divergence check requires ≥ 2 +TEST_F(GpsRedundancyCheckTest, single_gps) +{ + 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, two_gps_agreeing_prearm) +{ + 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, two_gps_diverging_prearm) +{ + 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, two_gps_diverging_inflight_not_yet_sustained) +{ + 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, fix_type_below_3_treated_as_inactive) +{ + 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 then recovery: timer resets and warning stops +TEST_F(GpsRedundancyCheckTest, divergence_clears_after_recovery) +{ + // 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/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index 5a5e5fb119..b9d9c09647 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -75,6 +75,7 @@ #include "checks/openDroneIDCheck.hpp" #include "checks/trafficAvoidanceCheck.hpp" #include "checks/externalChecks.hpp" +#include "checks/gpsRedundancyCheck.hpp" class HealthAndArmingChecks : public ModuleParams { @@ -164,6 +165,7 @@ private: VtolChecks _vtol_checks; OffboardChecks _offboard_checks; TrafficAvoidanceChecks _traffic_avoidance_checks; + GpsRedundancyChecks _gps_redundancy_checks; #ifndef CONSTRAINED_FLASH ExternalChecks _external_checks; #endif @@ -206,5 +208,6 @@ private: &_rc_and_data_link_checks, &_vtol_checks, &_traffic_avoidance_checks, + &_gps_redundancy_checks, }; }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp new file mode 100644 index 0000000000..0403a835e3 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp @@ -0,0 +1,181 @@ +/**************************************************************************** + * + * 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" + +#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 GPS_DIVERGENCE_SIGMA = 3.0f; +static constexpr uint64_t GPS_DIVERGENCE_SUSTAIN = 2_s; // must be sustained before warning + +void GpsRedundancyChecks::checkAndReport(const Context &context, Report &reporter) +{ + // Always reset — will be set below only when the condition is active + reporter.failsafeFlags().gps_redundancy_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] {}; + + for (int i = 0; i < _sensor_gps_sub.size(); i++) { + sensor_gps_s gps{}; + + if (_sensor_gps_sub[i].copy(&gps) + && (gps.device_id != 0) + && (hrt_elapsed_time(&gps.timestamp) < 2_s)) { + online_count++; + + if (gps.fix_type >= 3) { + fixed_gps[fixed_count++] = gps; + } + } + } + + // Position divergence check: warn if two fixed receivers disagree beyond their combined + // uncertainty. The gate is: lever-arm separation + GPS_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. + 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 * GPS_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() ? GPS_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) { + divergence_active = true; + const bool act = (_param_com_gps_loss_act.get() > 0); + + /* EVENT + * @description + * Two GPS receivers report positions that are inconsistent with their reported accuracy. + * + * + * Configure the failsafe action with COM_GPS_LOSS_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); + } + + } else { + _divergence_since = 0; + } + } + + // Track the highest fixed count seen — used to detect any GPS loss regardless of SYS_HAS_NUM_GPS + if (fixed_count > _peak_fixed_count) { + _peak_fixed_count = fixed_count; + } + + const int required = _param_sys_has_num_gps.get(); + const bool below_required = (required > 0 && fixed_count < required); + const bool dropped_below_peak = (_peak_fixed_count > 1 && fixed_count < _peak_fixed_count); + + if (!below_required && !dropped_below_peak && !divergence_active) { + return; + } + + reporter.failsafeFlags().gps_redundancy_lost = below_required || divergence_active; + + if (!below_required && !dropped_below_peak) { + return; + } + + // act==0: warn only, never blocks arming; act>0: blocks arming and shows red + const bool block_arming = below_required && (_param_com_gps_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 + * + * Configure the minimum required GPS count with SYS_HAS_NUM_GPS. + * Configure the failsafe action with COM_GPS_LOSS_ACT. + * + */ + 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_GPS. + * 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); + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp new file mode 100644 index 0000000000..4add9ba7dc --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include + +class GpsRedundancyChecks : public HealthAndArmingCheckBase +{ +public: + GpsRedundancyChecks() = default; + ~GpsRedundancyChecks() = default; + + 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}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamInt) _param_sys_has_num_gps, + (ParamInt) _param_com_gps_loss_act, + (ParamFloat) _param_gps0_offx, + (ParamFloat) _param_gps0_offy, + (ParamFloat) _param_gps1_offx, + (ParamFloat) _param_gps1_offy + ) +}; diff --git a/src/modules/commander/commander_params.yaml b/src/modules/commander/commander_params.yaml index f265455db8..0c81296c87 100644 --- a/src/modules/commander/commander_params.yaml +++ b/src/modules/commander/commander_params.yaml @@ -131,6 +131,19 @@ parameters: 1: Allow arming (with warning) 2: Allow arming (no warning) default: 1 + COM_GPS_LOSS_ACT: + description: + short: GPS redundancy loss failsafe mode + long: |- + Action the system takes when the number of active GPS receivers drops + below SYS_HAS_NUM_GPS during flight. + type: enum + values: + 0: Warning + 1: Return + 2: Land + 3: Terminate + default: 0 COM_ARM_SWISBTN: description: short: Arm switch is a momentary button diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 25d9728cc6..eab1c2f888 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -412,6 +412,36 @@ FailsafeBase::ActionOptions Failsafe::fromPosLowActParam(int param_value) return options; } +FailsafeBase::ActionOptions Failsafe::fromGpsRedundancyActParam(int param_value) +{ + ActionOptions options{}; + + switch (gps_redundancy_failsafe_mode(param_value)) { + case gps_redundancy_failsafe_mode::Warning: + default: + options.action = Action::Warn; + break; + + case gps_redundancy_failsafe_mode::Return_mode: + options.action = Action::RTL; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + + case gps_redundancy_failsafe_mode::Land_mode: + options.action = Action::Land; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + + case gps_redundancy_failsafe_mode::Terminate: + options.allow_user_takeover = UserTakeoverAllowed::Never; + options.action = Action::Terminate; + options.clear_condition = ClearCondition::Never; + break; + } + + return options; +} + FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int param_value) { ActionOptions options{}; @@ -634,6 +664,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, CHECK_FAILSAFE(status_flags, fd_imbalanced_prop, fromImbalancedPropActParam(_param_com_imb_prop_act.get())); CHECK_FAILSAFE(status_flags, fd_motor_failure, fromActuatorFailureActParam(_param_com_actuator_failure_act.get())); + CHECK_FAILSAFE(status_flags, gps_redundancy_lost, fromGpsRedundancyActParam(_param_com_gps_loss_act.get())); diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index d5b3f0a79e..83534e3cc5 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -157,6 +157,13 @@ private: Return_mode = 3 }; + enum class gps_redundancy_failsafe_mode : int32_t { + Warning = 0, + Return_mode = 1, + Land_mode = 2, + Terminate = 3, + }; + static ActionOptions fromNavDllOrRclActParam(int param_value); static ActionOptions fromGfActParam(int param_value); @@ -168,6 +175,7 @@ private: static ActionOptions fromHighWindLimitActParam(int param_value); static ActionOptions fromPosLowActParam(int param_value); static ActionOptions fromRemainingFlightTimeLowActParam(int param_value); + static ActionOptions fromGpsRedundancyActParam(int param_value); static bool isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter); @@ -209,7 +217,8 @@ private: (ParamInt) _param_com_qc_act, (ParamInt) _param_com_wind_max_act, (ParamInt) _param_com_fltt_low_act, - (ParamInt) _param_com_pos_low_act + (ParamInt) _param_com_pos_low_act, + (ParamInt) _param_com_gps_loss_act ); };