mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
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
This commit is contained in:
gguidone
parent
b48f3ef6f7
commit
b589115dc3
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
)
|
||||
|
||||
@ -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 <gtest/gtest.h>
|
||||
|
||||
#include "Common.hpp"
|
||||
#include "checks/gpsRedundancyCheck.hpp"
|
||||
|
||||
#include <uORB/topics/event.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <px4_platform_common/param.h>
|
||||
|
||||
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));
|
||||
}
|
||||
@ -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,
|
||||
};
|
||||
};
|
||||
|
||||
@ -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 <lib/geo/geo.h>
|
||||
|
||||
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.
|
||||
*
|
||||
* <profile name="dev">
|
||||
* Configure the failsafe action with <param>COM_GPS_LOSS_ACT</param>.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.healthFailure<float, float>(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
|
||||
* <profile name="dev">
|
||||
* Configure the minimum required GPS count with <param>SYS_HAS_NUM_GPS</param>.
|
||||
* Configure the failsafe action with <param>COM_GPS_LOSS_ACT</param>.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.healthFailure<uint8_t, uint8_t>(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
|
||||
* <profile name="dev">
|
||||
* Configure the minimum required GPS count with <param>SYS_HAS_NUM_GPS</param>.
|
||||
* Configure the failsafe action with <param>COM_GPS_LOSS_ACT</param>.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.healthFailure<uint8_t, uint8_t>(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);
|
||||
}
|
||||
}
|
||||
@ -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 <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
|
||||
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_s, GPS_MAX_INSTANCES> _sensor_gps_sub{ORB_ID::sensor_gps};
|
||||
|
||||
int _peak_fixed_count{0};
|
||||
|
||||
hrt_abstime _divergence_since{0};
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
|
||||
(ParamInt<px4::params::SYS_HAS_NUM_GPS>) _param_sys_has_num_gps,
|
||||
(ParamInt<px4::params::COM_GPS_LOSS_ACT>) _param_com_gps_loss_act,
|
||||
(ParamFloat<px4::params::SENS_GPS0_OFFX>) _param_gps0_offx,
|
||||
(ParamFloat<px4::params::SENS_GPS0_OFFY>) _param_gps0_offy,
|
||||
(ParamFloat<px4::params::SENS_GPS1_OFFX>) _param_gps1_offx,
|
||||
(ParamFloat<px4::params::SENS_GPS1_OFFY>) _param_gps1_offy
|
||||
)
|
||||
};
|
||||
@ -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
|
||||
|
||||
@ -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()));
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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<px4::params::COM_QC_ACT>) _param_com_qc_act,
|
||||
(ParamInt<px4::params::COM_WIND_MAX_ACT>) _param_com_wind_max_act,
|
||||
(ParamInt<px4::params::COM_FLTT_LOW_ACT>) _param_com_fltt_low_act,
|
||||
(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act
|
||||
(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act,
|
||||
(ParamInt<px4::params::COM_GPS_LOSS_ACT>) _param_com_gps_loss_act
|
||||
);
|
||||
|
||||
};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user