mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-21 04:33:10 +08:00
refactor(gps): move GNSS redundancy detection into sensors module
Add GnssRedundancyStatus topic and GnssRedundancyMonitor in vehicle_gps_position. Commander's gpsRedundancyCheck becomes a thin consumer of the new topic. Detection lives with blending/fallback in one module. Also rename COM_GPS_LOSS_ACT -> COM_GNSS_LSS_ACT.
This commit is contained in:
gguidone
Matthias Grob
@@ -115,6 +115,7 @@ set(msg_files
|
||||
GpioIn.msg
|
||||
GpioOut.msg
|
||||
GpioRequest.msg
|
||||
GnssRedundancyStatus.msg
|
||||
GpsDump.msg
|
||||
GpsInjectData.msg
|
||||
Gripper.msg
|
||||
|
||||
@@ -0,0 +1,15 @@
|
||||
# GNSS redundancy status published by vehicle_gps_position and consumed by commander
|
||||
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint8 num_receivers_online # receivers publishing fresh data
|
||||
uint8 num_receivers_fixed # receivers with fix_type >= 3
|
||||
uint8 num_receivers_required # configured minimum from SYS_HAS_NUM_GNSS (0 = no minimum)
|
||||
uint8 peak_receivers_fixed # highest fixed count seen since boot
|
||||
|
||||
bool below_required # num_receivers_fixed < num_receivers_required
|
||||
bool dropped_below_peak # num_receivers_fixed < peak_receivers_fixed
|
||||
bool divergence_detected # two receivers diverge beyond the gate after sustain
|
||||
|
||||
float32 divergence_m # last-measured horizontal distance between the two receivers
|
||||
float32 divergence_gate_m # current gate (lever-arm separation + sigma * RMS(eph))
|
||||
@@ -142,10 +142,10 @@ parameters:
|
||||
long: |-
|
||||
0: No minimum receiver count required. Position divergence between two receivers
|
||||
and loss of a GNSS receiver still produce a warning but never trigger the
|
||||
COM_GPS_LOSS_ACT failsafe action.
|
||||
COM_GNSS_LSS_ACT failsafe action.
|
||||
1-N: Require the presence of N GNSS receivers for arming and during flight.
|
||||
If the active count drops below this value in flight, COM_GPS_LOSS_ACT is triggered.
|
||||
When set to 2, position divergence between the two receivers also triggers COM_GPS_LOSS_ACT.
|
||||
If the active count drops below this value in flight, COM_GNSS_LSS_ACT is triggered.
|
||||
When set to 2, position divergence between the two receivers also triggers COM_GNSS_LSS_ACT.
|
||||
type: int32
|
||||
default: 0
|
||||
min: 0
|
||||
|
||||
@@ -79,7 +79,3 @@ add_dependencies(health_and_arming_checks mode_util)
|
||||
px4_add_functional_gtest(SRC HealthAndArmingChecksTest.cpp
|
||||
LINKLIBS health_and_arming_checks mode_util
|
||||
)
|
||||
|
||||
px4_add_functional_gtest(SRC GpsRedundancyCheckTest.cpp
|
||||
LINKLIBS health_and_arming_checks mode_util geo
|
||||
)
|
||||
|
||||
@@ -1,266 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <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_GNSS"), &i);
|
||||
param_set(param_find("COM_GPS_LOSS_ACT"), &i);
|
||||
|
||||
// Construct check after params are set so ParamFloat reads correct initial values
|
||||
_check = new GpsRedundancyChecks();
|
||||
|
||||
// Reset both instances to "absent" — device_id=0 is filtered by the check
|
||||
sensor_gps_s empty{};
|
||||
orb_publish(ORB_ID(sensor_gps), _pub0, &empty);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub1, &empty);
|
||||
}
|
||||
|
||||
void TearDown() override
|
||||
{
|
||||
delete _check;
|
||||
_check = nullptr;
|
||||
}
|
||||
|
||||
sensor_gps_s makeGps(double lat, double lon, float eph = 0.02f, uint8_t fix_type = 6)
|
||||
{
|
||||
sensor_gps_s gps{};
|
||||
gps.timestamp = hrt_absolute_time();
|
||||
gps.device_id = 1;
|
||||
gps.latitude_deg = lat;
|
||||
gps.longitude_deg = lon;
|
||||
gps.altitude_ellipsoid_m = 100.0;
|
||||
gps.eph = eph;
|
||||
gps.epv = 0.05f;
|
||||
gps.fix_type = fix_type;
|
||||
return gps;
|
||||
}
|
||||
|
||||
// Runs one check cycle and returns true if the GPS divergence health warning fired
|
||||
bool hasDivergenceWarning(bool armed)
|
||||
{
|
||||
vehicle_status_s status{};
|
||||
status.arming_state = armed
|
||||
? vehicle_status_s::ARMING_STATE_ARMED
|
||||
: vehicle_status_s::ARMING_STATE_DISARMED;
|
||||
Context context{status};
|
||||
failsafe_flags_s failsafe_flags{};
|
||||
Report reporter{failsafe_flags, 0_s};
|
||||
_check->checkAndReport(context, reporter);
|
||||
return ((uint64_t)reporter.healthResults().warning
|
||||
& (uint64_t)events::px4::enums::health_component_t::gps) != 0;
|
||||
}
|
||||
|
||||
// Runs one check cycle and returns true if gnss_lost was set (failsafe would trigger)
|
||||
bool hasGnssLost(bool armed)
|
||||
{
|
||||
vehicle_status_s status{};
|
||||
status.arming_state = armed
|
||||
? vehicle_status_s::ARMING_STATE_ARMED
|
||||
: vehicle_status_s::ARMING_STATE_DISARMED;
|
||||
Context context{status};
|
||||
failsafe_flags_s failsafe_flags{};
|
||||
Report reporter{failsafe_flags, 0_s};
|
||||
_check->checkAndReport(context, reporter);
|
||||
return failsafe_flags.gnss_lost;
|
||||
}
|
||||
|
||||
static orb_advert_t _pub0;
|
||||
static orb_advert_t _pub1;
|
||||
GpsRedundancyChecks *_check{nullptr};
|
||||
};
|
||||
|
||||
orb_advert_t GpsRedundancyCheckTest::_pub0{nullptr};
|
||||
orb_advert_t GpsRedundancyCheckTest::_pub1{nullptr};
|
||||
|
||||
// No GPS published → active_count=0 → no divergence check
|
||||
TEST_F(GpsRedundancyCheckTest, NoGnss)
|
||||
{
|
||||
EXPECT_FALSE(hasDivergenceWarning(false));
|
||||
}
|
||||
|
||||
// Only one active receiver → divergence check requires ≥ 2
|
||||
TEST_F(GpsRedundancyCheckTest, SingleGnss)
|
||||
{
|
||||
sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub0, &gps0);
|
||||
EXPECT_FALSE(hasDivergenceWarning(false));
|
||||
}
|
||||
|
||||
// Two receivers ~0.4m apart (within gate of 0.785m) → no warning
|
||||
TEST_F(GpsRedundancyCheckTest, TwoGnssAgreeingPrearm)
|
||||
{
|
||||
sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON);
|
||||
sensor_gps_s gps1 = makeGps(AGREEING_LAT, BASE_LON);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub0, &gps0);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub1, &gps1);
|
||||
EXPECT_FALSE(hasDivergenceWarning(false));
|
||||
}
|
||||
|
||||
// Two receivers ~2.0m apart (beyond gate), pre-arm → immediate warning (sustain=0 pre-arm)
|
||||
TEST_F(GpsRedundancyCheckTest, TwoGnssDivergingPrearm)
|
||||
{
|
||||
sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON);
|
||||
sensor_gps_s gps1 = makeGps(DIVERGING_LAT, BASE_LON);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub0, &gps0);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub1, &gps1);
|
||||
EXPECT_TRUE(hasDivergenceWarning(false));
|
||||
}
|
||||
|
||||
// Two receivers ~2.0m apart, in-flight, first call → no warning yet (2s sustain not elapsed)
|
||||
TEST_F(GpsRedundancyCheckTest, TwoGnssDivergingInflightNotYetSustained)
|
||||
{
|
||||
sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON);
|
||||
sensor_gps_s gps1 = makeGps(DIVERGING_LAT, BASE_LON);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub0, &gps0);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub1, &gps1);
|
||||
EXPECT_FALSE(hasDivergenceWarning(true));
|
||||
}
|
||||
|
||||
// GPS1 fix_type=2 (2D only) → treated as inactive → only 1 active receiver → no divergence check
|
||||
TEST_F(GpsRedundancyCheckTest, FixTypeBelow3TreatedAsInactive)
|
||||
{
|
||||
sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON);
|
||||
sensor_gps_s gps1 = makeGps(DIVERGING_LAT, BASE_LON, 0.02f, 2);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub0, &gps0);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub1, &gps1);
|
||||
EXPECT_FALSE(hasDivergenceWarning(false));
|
||||
}
|
||||
|
||||
// Divergence with SYS_HAS_NUM_GNSS=2 and COM_GPS_LOSS_ACT>0 → gnss_lost set (failsafe fires)
|
||||
TEST_F(GpsRedundancyCheckTest, DivergenceWithRedundancyRequiredSetsGnssLost)
|
||||
{
|
||||
int i = 2;
|
||||
param_set(param_find("SYS_HAS_NUM_GNSS"), &i);
|
||||
i = 1; // Return
|
||||
param_set(param_find("COM_GPS_LOSS_ACT"), &i);
|
||||
delete _check;
|
||||
_check = new GpsRedundancyChecks();
|
||||
|
||||
sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON);
|
||||
sensor_gps_s gps1 = makeGps(DIVERGING_LAT, BASE_LON);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub0, &gps0);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub1, &gps1);
|
||||
EXPECT_TRUE(hasGnssLost(false));
|
||||
}
|
||||
|
||||
// Divergence with SYS_HAS_NUM_GNSS=0 and COM_GPS_LOSS_ACT>0 → warning only, gnss_lost not set
|
||||
TEST_F(GpsRedundancyCheckTest, DivergenceWithoutRedundancyRequiredWarnsOnly)
|
||||
{
|
||||
int i = 0;
|
||||
param_set(param_find("SYS_HAS_NUM_GNSS"), &i);
|
||||
i = 1; // Return
|
||||
param_set(param_find("COM_GPS_LOSS_ACT"), &i);
|
||||
delete _check;
|
||||
_check = new GpsRedundancyChecks();
|
||||
|
||||
sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON);
|
||||
sensor_gps_s gps1 = makeGps(DIVERGING_LAT, BASE_LON);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub0, &gps0);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub1, &gps1);
|
||||
EXPECT_FALSE(hasGnssLost(false));
|
||||
EXPECT_TRUE(hasDivergenceWarning(false));
|
||||
}
|
||||
|
||||
// Divergence then recovery: timer resets and warning stops
|
||||
TEST_F(GpsRedundancyCheckTest, DivergenceClearsAfterRecovery)
|
||||
{
|
||||
// First: diverging pre-arm → warning fires
|
||||
sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON);
|
||||
sensor_gps_s gps1 = makeGps(DIVERGING_LAT, BASE_LON);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub0, &gps0);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub1, &gps1);
|
||||
EXPECT_TRUE(hasDivergenceWarning(false));
|
||||
|
||||
// Then: receivers agree → warning clears
|
||||
gps0 = makeGps(BASE_LAT, BASE_LON);
|
||||
gps1 = makeGps(AGREEING_LAT, BASE_LON);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub0, &gps0);
|
||||
orb_publish(ORB_ID(sensor_gps), _pub1, &gps1);
|
||||
EXPECT_FALSE(hasDivergenceWarning(false));
|
||||
}
|
||||
@@ -33,153 +33,79 @@
|
||||
|
||||
#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 GNSS_DIVERGENCE_SIGMA = 3.0f;
|
||||
|
||||
// Matches the 1 s staleness threshold used by other sensor checks in this framework.
|
||||
static constexpr uint64_t GNSS_ONLINE_TIMEOUT = 1_s;
|
||||
|
||||
// Multipath spikes typically resolve within 1 s; 2 s filters these out while still
|
||||
// detecting real receiver faults promptly.
|
||||
static constexpr uint64_t GNSS_DIVERGENCE_SUSTAIN = 2_s;
|
||||
|
||||
void GpsRedundancyChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
{
|
||||
// Always reset — will be set below only when the condition is active
|
||||
reporter.failsafeFlags().gnss_lost = false;
|
||||
|
||||
// Separate "online" (present + fresh data) from "fixed" (online + 3D fix).
|
||||
// online_count tracks receivers that are communicating; fixed_count tracks those
|
||||
// suitable for navigation. Keeping them separate lets us emit a more informative
|
||||
// warning: "receiver offline" vs "receiver lost fix".
|
||||
int online_count = 0;
|
||||
int fixed_count = 0;
|
||||
sensor_gps_s fixed_gps[GPS_MAX_INSTANCES] {};
|
||||
gnss_redundancy_status_s status;
|
||||
|
||||
for (int i = 0; i < _sensor_gps_sub.size(); i++) {
|
||||
sensor_gps_s gps{};
|
||||
if (_gnss_redundancy_status_sub.copy(&status)) {
|
||||
const bool act_configured = (_param_com_gnss_loss_act.get() > 0);
|
||||
|
||||
if (_sensor_gps_sub[i].copy(&gps)
|
||||
&& (gps.device_id != 0)
|
||||
&& (hrt_elapsed_time(&gps.timestamp) < GNSS_ONLINE_TIMEOUT)) {
|
||||
online_count++;
|
||||
// Divergence triggers the failsafe only when the operator explicitly expects two
|
||||
// receivers (SYS_HAS_NUM_GNSS >= 2); otherwise it remains a warning.
|
||||
const bool divergence_triggers_failsafe = status.divergence_detected
|
||||
&& (status.num_receivers_required >= 2);
|
||||
|
||||
if (gps.fix_type >= 3) {
|
||||
fixed_gps[fixed_count++] = gps;
|
||||
}
|
||||
}
|
||||
}
|
||||
reporter.failsafeFlags().gnss_lost = status.below_required || divergence_triggers_failsafe;
|
||||
|
||||
const int required = _param_sys_has_num_gnss.get();
|
||||
|
||||
// Position divergence check: warn if two fixed receivers disagree beyond their combined
|
||||
// uncertainty. The gate is: lever-arm separation + GNSS_DIVERGENCE_SIGMA * RMS(eph0, eph1).
|
||||
// Using RMS(eph) means the gate tightens when both receivers are accurate and widens
|
||||
// automatically when one degrades, avoiding false alarms without a hard-coded threshold.
|
||||
// Pre-arm: warn immediately. In-flight: require GPS_DIVERGENCE_SUSTAIN to suppress
|
||||
// transient multipath spikes.
|
||||
// The failsafe action is only triggered when SYS_HAS_NUM_GNSS >= 2; otherwise divergence
|
||||
// is reported as a warning only.
|
||||
bool divergence_active = false;
|
||||
|
||||
if (fixed_count >= 2) {
|
||||
float north, east;
|
||||
get_vector_to_next_waypoint(fixed_gps[0].latitude_deg, fixed_gps[0].longitude_deg,
|
||||
fixed_gps[1].latitude_deg, fixed_gps[1].longitude_deg,
|
||||
&north, &east);
|
||||
|
||||
const float divergence_m = sqrtf(north * north + east * east);
|
||||
const float rms_eph = sqrtf(fixed_gps[0].eph * fixed_gps[0].eph + fixed_gps[1].eph * fixed_gps[1].eph);
|
||||
const float dx = _param_gps0_offx.get() - _param_gps1_offx.get();
|
||||
const float dy = _param_gps0_offy.get() - _param_gps1_offy.get();
|
||||
const float expected_d = sqrtf(dx * dx + dy * dy);
|
||||
const float gate_m = expected_d + rms_eph * GNSS_DIVERGENCE_SIGMA;
|
||||
|
||||
// Pre-arm: trigger immediately so the operator can decide before takeoff.
|
||||
// In-flight: require sustained divergence to avoid false alarms from transient multipath.
|
||||
const uint64_t sustain = context.isArmed() ? GNSS_DIVERGENCE_SUSTAIN : 0_s;
|
||||
|
||||
if (divergence_m > gate_m) {
|
||||
if (_divergence_since == 0) {
|
||||
_divergence_since = hrt_absolute_time();
|
||||
}
|
||||
|
||||
if (hrt_elapsed_time(&_divergence_since) >= sustain) {
|
||||
const bool act = (_param_com_gnss_loss_act.get() > 0) && (required >= 2);
|
||||
divergence_active = act;
|
||||
if (status.below_required || status.dropped_below_peak) {
|
||||
const bool block_arming = status.below_required && act_configured;
|
||||
const NavModes nav_modes = block_arming ? NavModes::All : NavModes::None;
|
||||
const events::Log log_level = block_arming ? events::Log::Error : events::Log::Warning;
|
||||
const uint8_t expected = status.below_required ? status.num_receivers_required
|
||||
: status.peak_receivers_fixed;
|
||||
|
||||
if (status.num_receivers_online < status.num_receivers_fixed
|
||||
|| status.num_receivers_online < status.num_receivers_required) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* Two GNSS receivers report positions that are inconsistent with their reported accuracy.
|
||||
*
|
||||
* <profile name="dev">
|
||||
* Configure the failsafe action with <param>COM_GPS_LOSS_ACT</param>.
|
||||
* The failsafe action is only triggered when <param>SYS_HAS_NUM_GNSS</param> is set to 2.
|
||||
* Configure the minimum required GPS count with <param>SYS_HAS_NUM_GNSS</param>.
|
||||
* Configure the failsafe action with <param>COM_GNSS_LSS_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);
|
||||
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",
|
||||
status.num_receivers_online, expected);
|
||||
|
||||
} else {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* Configure the minimum required GPS count with <param>SYS_HAS_NUM_GNSS</param>.
|
||||
* Configure the failsafe action with <param>COM_GNSS_LSS_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",
|
||||
status.num_receivers_fixed, expected);
|
||||
}
|
||||
|
||||
} else {
|
||||
_divergence_since = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Track the highest fixed count seen — used to detect any GPS loss regardless of SYS_HAS_NUM_GNSS
|
||||
if (fixed_count > _peak_fixed_count) {
|
||||
_peak_fixed_count = fixed_count;
|
||||
}
|
||||
if (status.divergence_detected) {
|
||||
const bool block_arming = divergence_triggers_failsafe && act_configured;
|
||||
const NavModes nav_modes = block_arming ? NavModes::All : NavModes::None;
|
||||
const events::Log log_level = block_arming ? events::Log::Error : events::Log::Warning;
|
||||
|
||||
const bool below_required = (required > 0 && fixed_count < required);
|
||||
const bool dropped_below_peak = (_peak_fixed_count > 1 && fixed_count < _peak_fixed_count);
|
||||
|
||||
reporter.failsafeFlags().gnss_lost = below_required || divergence_active;
|
||||
|
||||
if (below_required || dropped_below_peak) {
|
||||
// act==0: warn only, never blocks arming; act>0: blocks arming and shows red
|
||||
const bool block_arming = below_required && (_param_com_gnss_loss_act.get() > 0);
|
||||
const NavModes nav_modes = block_arming ? NavModes::All : NavModes::None;
|
||||
const events::Log log_level = block_arming ? events::Log::Error : events::Log::Warning;
|
||||
const uint8_t expected = below_required ? (uint8_t)required : (uint8_t)_peak_fixed_count;
|
||||
|
||||
// Differentiate: if online_count is also low the receiver is offline; otherwise it lost fix.
|
||||
if (online_count < fixed_count || online_count < required) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* Two GNSS receivers report positions that are inconsistent with their reported accuracy.
|
||||
*
|
||||
* <profile name="dev">
|
||||
* Configure the minimum required GPS count with <param>SYS_HAS_NUM_GNSS</param>.
|
||||
* Configure the failsafe action with <param>COM_GPS_LOSS_ACT</param>.
|
||||
* Configure the failsafe action with <param>COM_GNSS_LSS_ACT</param>.
|
||||
* The failsafe action is only triggered when <param>SYS_HAS_NUM_GNSS</param> is set to 2.
|
||||
* </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_GNSS</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);
|
||||
reporter.healthFailure<float, float>(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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -35,8 +35,8 @@
|
||||
|
||||
#include "../Common.hpp"
|
||||
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/gnss_redundancy_status.h>
|
||||
|
||||
class GpsRedundancyChecks : public HealthAndArmingCheckBase
|
||||
{
|
||||
@@ -47,20 +47,9 @@ public:
|
||||
void checkAndReport(const Context &context, Report &reporter) override;
|
||||
|
||||
private:
|
||||
static constexpr int GPS_MAX_INSTANCES = 2;
|
||||
|
||||
uORB::SubscriptionMultiArray<sensor_gps_s, GPS_MAX_INSTANCES> _sensor_gps_sub{ORB_ID::sensor_gps};
|
||||
|
||||
int _peak_fixed_count{0};
|
||||
|
||||
hrt_abstime _divergence_since{0};
|
||||
uORB::Subscription _gnss_redundancy_status_sub{ORB_ID(gnss_redundancy_status)};
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
|
||||
(ParamInt<px4::params::SYS_HAS_NUM_GNSS>) _param_sys_has_num_gnss,
|
||||
(ParamInt<px4::params::COM_GPS_LOSS_ACT>) _param_com_gnss_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
|
||||
(ParamInt<px4::params::COM_GNSS_LSS_ACT>) _param_com_gnss_loss_act
|
||||
)
|
||||
};
|
||||
|
||||
@@ -131,7 +131,7 @@ parameters:
|
||||
1: Allow arming (with warning)
|
||||
2: Allow arming (no warning)
|
||||
default: 1
|
||||
COM_GPS_LOSS_ACT:
|
||||
COM_GNSS_LSS_ACT:
|
||||
description:
|
||||
short: GNSS loss failsafe mode
|
||||
long: |-
|
||||
|
||||
@@ -219,7 +219,7 @@ private:
|
||||
(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_ARM_ODID>) _param_com_arm_odid,
|
||||
(ParamInt<px4::params::COM_GPS_LOSS_ACT>) _param_com_gnss_loss_act
|
||||
(ParamInt<px4::params::COM_GNSS_LSS_ACT>) _param_com_gnss_loss_act
|
||||
);
|
||||
|
||||
};
|
||||
|
||||
@@ -36,6 +36,8 @@ px4_add_library(vehicle_gps_position
|
||||
VehicleGPSPosition.hpp
|
||||
gps_blending.cpp
|
||||
gps_blending.hpp
|
||||
GnssRedundancyMonitor.cpp
|
||||
GnssRedundancyMonitor.hpp
|
||||
PpsTimeSync.cpp
|
||||
PpsTimeSync.hpp
|
||||
)
|
||||
@@ -46,3 +48,4 @@ target_link_libraries(vehicle_gps_position
|
||||
)
|
||||
|
||||
px4_add_unit_gtest(SRC gps_blending_test.cpp LINKLIBS vehicle_gps_position)
|
||||
px4_add_functional_gtest(SRC GnssRedundancyMonitorTest.cpp LINKLIBS vehicle_gps_position)
|
||||
|
||||
@@ -0,0 +1,149 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "GnssRedundancyMonitor.hpp"
|
||||
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
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
|
||||
@@ -0,0 +1,75 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/gnss_redundancy_status.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
|
||||
#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_s> _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<px4::params::SYS_HAS_NUM_GNSS>) _param_sys_has_num_gnss
|
||||
)
|
||||
};
|
||||
|
||||
} // namespace sensors
|
||||
@@ -0,0 +1,205 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "GnssRedundancyMonitor.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4_platform_common/param.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/gnss_redundancy_status.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
|
||||
// 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);
|
||||
}
|
||||
@@ -191,7 +191,21 @@ void VehicleGPSPosition::Run()
|
||||
|
||||
if (any_gps_updated) {
|
||||
_gps_blending.update(hrt_absolute_time());
|
||||
}
|
||||
|
||||
// Tick the redundancy monitor every cycle so silence is detected.
|
||||
{
|
||||
vehicle_status_s vehicle_status{};
|
||||
_vehicle_status_sub.copy(&vehicle_status);
|
||||
const bool is_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
|
||||
|
||||
_gnss_redundancy_monitor.update(_gps_blending.getGpsStates(),
|
||||
_gps_blending.getAntennaOffsets(),
|
||||
GPS_MAX_RECEIVERS,
|
||||
is_armed);
|
||||
}
|
||||
|
||||
if (any_gps_updated) {
|
||||
if (_gps_blending.isNewOutputDataAvailable()) {
|
||||
sensor_gps_s gps_output{_gps_blending.getOutputGpsData()};
|
||||
|
||||
|
||||
@@ -46,7 +46,9 @@
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/pps_capture.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
#include "GnssRedundancyMonitor.hpp"
|
||||
#include "gps_blending.hpp"
|
||||
#include "PpsTimeSync.hpp"
|
||||
|
||||
@@ -91,11 +93,13 @@ private:
|
||||
};
|
||||
|
||||
uORB::Subscription _pps_capture_sub{ORB_ID(pps_capture)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
|
||||
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
|
||||
GpsBlending _gps_blending;
|
||||
PpsTimeSync _pps_time_sync;
|
||||
GnssRedundancyMonitor _gnss_redundancy_monitor{this};
|
||||
|
||||
struct GpsParamSlot {
|
||||
uint32_t device_id{0};
|
||||
|
||||
@@ -99,6 +99,12 @@ public:
|
||||
}
|
||||
int getSelectedGps() const { return _selected_gps; }
|
||||
|
||||
// Exposed for the redundancy monitor so it can inspect the last known
|
||||
// sample (and its resolved antenna offset) from each instance without
|
||||
// maintaining a second set of subscriptions.
|
||||
const sensor_gps_s *getGpsStates() const { return _gps_state; }
|
||||
const Vector3f *getAntennaOffsets() const { return _antenna_offset; }
|
||||
|
||||
private:
|
||||
/*
|
||||
* Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical
|
||||
|
||||
Reference in New Issue
Block a user