Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

fix(gnssRedundancyCheck): move logic back into the commander checks and various improvement suggestions

Browse Source 
- Rename to GNSS instead of gps
- Use hysteresis
- Small logic refactorings
- Adapt unit tests to different interface
- User reporting on which GPS is offline or doesn't have a fix
This commit is contained in:
Matthias Grob
2026-04-30 18:14:08 +02:00
parent feb074fe18
commit c8b08b114f
15 changed files with 405 additions and 595 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/CMakeLists.txt
-1
View File
@@ -115,7 +115,6 @@ set(msg_files
GpioIn.msg
GpioOut.msg
GpioRequest.msg
GnssRedundancyStatus.msg
GpsDump.msg
GpsInjectData.msg
Gripper.msg
msg/GnssRedundancyStatus.msg
-15
View File
@@ -1,15 +0,0 @@
# GNSS redundancy status published by vehicle_gps_position and consumed by commander
uint64 timestamp # time since system start (microseconds)
uint8 num_receivers_online # receivers publishing fresh data
uint8 num_receivers_fixed # receivers with fix_type >= 3
uint8 num_receivers_required # configured minimum from SYS_HAS_NUM_GNSS (0 = no minimum)
uint8 peak_receivers_fixed # highest fixed count seen since boot
bool below_required # num_receivers_fixed < num_receivers_required
bool dropped_below_peak # num_receivers_fixed < peak_receivers_fixed
bool divergence_detected # two receivers diverge beyond the gate after sustain
float32 divergence_m # last-measured horizontal distance between the two receivers
float32 divergence_gate_m # current gate (lever-arm separation + sigma * RMS(eph))
src/modules/commander/HealthAndArmingChecks/CMakeLists.txt
+4 -1
View File
@@ -48,7 +48,7 @@ px4_add_library(health_and_arming_checks
checks/failureDetectorCheck.cpp
checks/flightTimeCheck.cpp
checks/geofenceCheck.cpp
checks/gpsRedundancyCheck.cpp
checks/gnssRedundancyCheck.cpp
checks/gyroCheck.cpp
checks/homePositionCheck.cpp
checks/imuConsistencyCheck.cpp
@@ -75,7 +75,10 @@ px4_add_library(health_and_arming_checks
)
set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/esc_check_params.yaml)
add_dependencies(health_and_arming_checks mode_util)
target_link_libraries(health_and_arming_checks PRIVATE geo hysteresis)
px4_add_functional_gtest(SRC HealthAndArmingChecksTest.cpp
LINKLIBS health_and_arming_checks mode_util
)
px4_add_functional_gtest(SRC gnssRedundancyChecksTest.cpp LINKLIBS health_and_arming_checks mode_util)
src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp
+3 -3
View File
@@ -75,7 +75,7 @@
#include "checks/openDroneIDCheck.hpp"
#include "checks/trafficAvoidanceCheck.hpp"
#include "checks/externalChecks.hpp"
#include "checks/gpsRedundancyCheck.hpp"
#include "checks/gnssRedundancyCheck.hpp"
class HealthAndArmingChecks : public ModuleParams
{
@@ -165,7 +165,7 @@ private:
VtolChecks _vtol_checks;
OffboardChecks _offboard_checks;
TrafficAvoidanceChecks _traffic_avoidance_checks;
GpsRedundancyChecks _gps_redundancy_checks;
GnssRedundancyChecks _gnss_redundancy_checks;
#ifndef CONSTRAINED_FLASH
ExternalChecks _external_checks;
#endif
@@ -208,6 +208,6 @@ private:
&_rc_and_data_link_checks,
&_vtol_checks,
&_traffic_avoidance_checks,
&_gps_redundancy_checks,
&_gnss_redundancy_checks,
};
};
src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.cpp
+161
View File
@@ -0,0 +1,161 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "gnssRedundancyCheck.hpp"
#include <lib/geo/geo.h>
using namespace matrix;
using namespace time_literals;
GnssRedundancyChecks::GnssRedundancyChecks()
{
_divergence_hysteresis.set_hysteresis_time_from(false, 2_s);
_divergence_hysteresis.set_hysteresis_time_from(true, 2_s);
}
void GnssRedundancyChecks::checkAndReport(const Context &context, Report &reporter)
{
bool gps_online[GPS_MAX_INSTANCES] {};
bool gps_has_fix[GPS_MAX_INSTANCES] {};
uint8_t fixed_count = 0;
sensor_gps_s fixed_gps[GPS_MAX_INSTANCES] {};
for (int i = 0; i < GPS_MAX_INSTANCES; i++) {
sensor_gps_s gps{};
if (_sensor_gps_sub[i].copy(&gps)
&& (gps.device_id != 0)
&& (hrt_elapsed_time(&gps.timestamp) < 1_s)) {
gps_online[i] = true;
if (gps.fix_type >= 3) {
gps_has_fix[i] = true;
fixed_gps[fixed_count++] = gps;
}
}
}
// Track the highest fixed count seen to warn about GNSS loss regardless of SYS_HAS_NUM_GNSS
if (fixed_count > _peak_fixed_count) {
_peak_fixed_count = fixed_count;
}
// Position divergence check: flag if two fixed receivers disagree beyond their
// combined uncertainty. Gate = 3 * RSS(eph), centered on the expected lever-arm separation.
float divergence_m = 0.f;
if (fixed_count >= 2) {
float north, east;
get_vector_to_next_waypoint(fixed_gps[0].latitude_deg, fixed_gps[0].longitude_deg,
fixed_gps[1].latitude_deg, fixed_gps[1].longitude_deg,
&north, &east);
const float separation_m = Vector2f(north, east).length();
const Vector2f offset0(_param_sens_gps0_offx.get(), _param_sens_gps0_offy.get());
const Vector2f offset1(_param_sens_gps1_offx.get(), _param_sens_gps1_offy.get());
const float expected_d = (offset0 - offset1).length();
divergence_m = fabsf(separation_m - expected_d);
// Use quadrature sum for standard deviation of the difference taking the firmware dependent eph as standard deviation
// and a heuristic factor of 3 because then it's unlikely just noise.
const float divergence_gate_m = 3.f * Vector2f(fixed_gps[0].eph, fixed_gps[1].eph).length();
_divergence_hysteresis.set_state_and_update(divergence_m > divergence_gate_m, hrt_absolute_time());
} else {
_divergence_hysteresis.set_state_and_update(false, hrt_absolute_time());
}
const bool below_required = (_param_sys_has_num_gnss.get() > 0) && (fixed_count < _param_sys_has_num_gnss.get());
const bool dropped_below_peak = (_peak_fixed_count > 1) && (fixed_count < _peak_fixed_count);
const bool act_configured = (_param_com_gnssloss_act.get() > 0);
// Divergence triggers the failsafe only when the operator explicitly expects two
// receivers (SYS_HAS_NUM_GNSS >= 2); otherwise it remains a warning.
const bool divergence_triggers_failsafe = _divergence_hysteresis.get_state() && (_param_sys_has_num_gnss.get() >= 2);
reporter.failsafeFlags().gnss_lost = below_required || divergence_triggers_failsafe;
if (below_required || dropped_below_peak) {
const bool block_arming = below_required && act_configured;
const NavModes nav_modes = block_arming ? NavModes::All : NavModes::None;
const events::Log log_level = block_arming ? events::Log::Error : events::Log::Warning;
const int expected = below_required ? _param_sys_has_num_gnss.get() : _peak_fixed_count;
for (int i = 0; i < expected; i++) {
if (!gps_online[i]) {
/* EVENT
* @description
* <profile name="dev">
* Configure the minimum required GPS count with <param>SYS_HAS_NUM_GNSS</param>.
* Configure the failsafe action with <param>COM_GNSSLOSS_ACT</param>.
* </profile>
*/
reporter.healthFailure<uint8_t>(nav_modes, health_component_t::gps,
events::ID("check_gnss_receiver_offline"),
log_level, "GPS {1} offline", (uint8_t)i);
} else if (!gps_has_fix[i]) {
/* EVENT
* @description
* <profile name="dev">
* Configure the minimum required GPS count with <param>SYS_HAS_NUM_GNSS</param>.
* Configure the failsafe action with <param>COM_GNSSLOSS_ACT</param>.
* </profile>
*/
reporter.healthFailure<uint8_t>(nav_modes, health_component_t::gps,
events::ID("check_gnss_receiver_no_fix"),
log_level, "GPS {1} lost fix", (uint8_t)i);
}
}
}
if (_divergence_hysteresis.get_state()) {
const bool block_arming = divergence_triggers_failsafe && act_configured;
const NavModes nav_modes = block_arming ? NavModes::All : NavModes::None;
const events::Log log_level = block_arming ? events::Log::Error : events::Log::Warning;
/* EVENT
* @description
* Two GNSS receivers report positions that are inconsistent with their reported accuracy.
*
* <profile name="dev">
* Configure the failsafe action with <param>COM_GNSSLOSS_ACT</param>.
* The failsafe action is only triggered when <param>SYS_HAS_NUM_GNSS</param> is set to 2.
* </profile>
*/
reporter.healthFailure<float>(nav_modes, health_component_t::gps,
events::ID("check_gps_position_divergence"),
log_level,
"GPS receivers disagree by {1:.1}m",
(double)divergence_m);
}
}
src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp → src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.hpp
+18 -8
View File
@@ -34,22 +34,32 @@
#pragma once
#include "../Common.hpp"
#include <lib/hysteresis/hysteresis.h>
#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
class GnssRedundancyChecks : public HealthAndArmingCheckBase
{
public:
GpsRedundancyChecks() = default;
~GpsRedundancyChecks() = default;
GnssRedundancyChecks();
~GnssRedundancyChecks() = default;
void checkAndReport(const Context &context, Report &reporter) override;
private:
uORB::Subscription _gnss_redundancy_status_sub{ORB_ID(gnss_redundancy_status)};
static constexpr int GPS_MAX_INSTANCES = 2;
uORB::SubscriptionMultiArray<sensor_gps_s, GPS_MAX_INSTANCES> _sensor_gps_sub{ORB_ID::sensor_gps};
uint8_t _peak_fixed_count{0};
systemlib::Hysteresis _divergence_hysteresis;
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamInt<px4::params::COM_GNSSLOSS_ACT>) _param_com_gnssloss_act
(ParamInt<px4::params::SYS_HAS_NUM_GNSS>) _param_sys_has_num_gnss,
(ParamInt<px4::params::COM_GNSSLOSS_ACT>) _param_com_gnssloss_act,
(ParamFloat<px4::params::SENS_GPS0_OFFX>) _param_sens_gps0_offx,
(ParamFloat<px4::params::SENS_GPS0_OFFY>) _param_sens_gps0_offy,
(ParamFloat<px4::params::SENS_GPS1_OFFX>) _param_sens_gps1_offx,
(ParamFloat<px4::params::SENS_GPS1_OFFY>) _param_sens_gps1_offy
)
};
src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp
-111
View File
@@ -1,111 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "gpsRedundancyCheck.hpp"
void GpsRedundancyChecks::checkAndReport(const Context &context, Report &reporter)
{
reporter.failsafeFlags().gnss_lost = false;
gnss_redundancy_status_s status;
if (_gnss_redundancy_status_sub.copy(&status)) {
const bool act_configured = (_param_com_gnssloss_act.get() > 0);
// Divergence triggers the failsafe only when the operator explicitly expects two
// receivers (SYS_HAS_NUM_GNSS >= 2); otherwise it remains a warning.
const bool divergence_triggers_failsafe = status.divergence_detected
&& (status.num_receivers_required >= 2);
reporter.failsafeFlags().gnss_lost = status.below_required || divergence_triggers_failsafe;
if (status.below_required || status.dropped_below_peak) {
const bool block_arming = status.below_required && act_configured;
const NavModes nav_modes = block_arming ? NavModes::All : NavModes::None;
const events::Log log_level = block_arming ? events::Log::Error : events::Log::Warning;
const uint8_t expected = status.below_required ? status.num_receivers_required
: status.peak_receivers_fixed;
if (status.num_receivers_online < status.num_receivers_fixed
|| status.num_receivers_online < status.num_receivers_required) {
/* EVENT
* @description
* <profile name="dev">
* Configure the minimum required GPS count with <param>SYS_HAS_NUM_GNSS</param>.
* Configure the failsafe action with <param>COM_GNSSLOSS_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",
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_GNSSLOSS_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);
}
}
if (status.divergence_detected) {
const bool block_arming = divergence_triggers_failsafe && act_configured;
const NavModes nav_modes = block_arming ? NavModes::All : NavModes::None;
const events::Log log_level = block_arming ? events::Log::Error : events::Log::Warning;
/* EVENT
* @description
* Two GNSS receivers report positions that are inconsistent with their reported accuracy.
*
* <profile name="dev">
* Configure the failsafe action with <param>COM_GNSSLOSS_ACT</param>.
* The failsafe action is only triggered when <param>SYS_HAS_NUM_GNSS</param> is set to 2.
* </profile>
*/
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);
}
}
}
src/modules/commander/HealthAndArmingChecks/gnssRedundancyChecksTest.cpp
+219
View File
@@ -0,0 +1,219 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <gtest/gtest.h>
#include "checks/gnssRedundancyCheck.hpp"
#include <drivers/drv_hrt.h>
#include <px4_platform_common/param.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_gps.h>
// to run: make tests TESTFILTER=gnssRedundancyChecks
/* EVENT
* @skip-file
*/
// Base position (PX4 SITL default home).
static constexpr double BASE_LAT = 47.397742;
static constexpr double BASE_LON = 8.545594;
// With GPS0 at +35cm X and GPS1 at -35cm X, expected_d = 0.70m.
// With RTK eph = 0.02m: gate = 3 * sqrt(0.02² + 0.02²) ≈ 0.085m.
// AGREEING_LAT puts GPS1 ~0.70m north of GPS0 so separation ≈ expected_d.
// DIVERGING_FAR_LAT puts GPS1 ~2.0m north, well outside the gate.
static constexpr double AGREEING_LAT = BASE_LAT + 6.3e-6; // ~0.70m north
static constexpr double DIVERGING_FAR_LAT = BASE_LAT + 1.8e-5; // ~2.0m north
class GnssRedundancyChecksTest : public ::testing::Test
{
public:
void SetUp() override
{
param_control_autosave(false);
// Reset params that tests modify so state doesn't leak between tests.
param_reset(param_find("SYS_HAS_NUM_GNSS"));
param_reset(param_find("COM_GNSSLOSS_ACT"));
// Set lever arms so expected_d = 0.70m, enabling the "too close" direction of divergence detection.
float v = 0.35f; param_set(param_find("SENS_GPS0_OFFX"), &v);
v = -0.35f; param_set(param_find("SENS_GPS1_OFFX"), &v);
// Claim uORB instances 0 and 1 before the check subscribes on first copy().
sensor_gps_s empty{};
_gps0_pub.publish(empty);
_gps1_pub.publish(empty);
}
sensor_gps_s makeGps(double lat, double lon, float eph = 0.02f, uint8_t fix_type = 6)
{
sensor_gps_s gps{};
gps.timestamp = hrt_absolute_time();
gps.device_id = 1;
gps.latitude_deg = lat;
gps.longitude_deg = lon;
gps.eph = eph;
gps.fix_type = fix_type;
return gps;
}
// Run the check and store results in _failsafe_flags and _health_warning_gps.
void runCheck(bool armed = false)
{
vehicle_status_s status{};
if (armed) { status.arming_state = vehicle_status_s::ARMING_STATE_ARMED; }
_check.updateParams();
Context context{status};
_failsafe_flags = {};
Report reporter{_failsafe_flags, 0};
_check.checkAndReport(context, reporter);
// Capture any GPS health issue regardless of log level (Warning or Error).
_health_warning_gps = (reporter.healthResults().warning | reporter.healthResults().error) & health_component_t::gps;
}
uORB::PublicationMulti<sensor_gps_s> _gps0_pub{ORB_ID(sensor_gps)};
uORB::PublicationMulti<sensor_gps_s> _gps1_pub{ORB_ID(sensor_gps)};
failsafe_flags_s _failsafe_flags{};
bool _health_warning_gps{false};
GnssRedundancyChecks _check;
};
// No GPS data → no flags.
TEST_F(GnssRedundancyChecksTest, NoGpsNoFlags)
{
runCheck();
EXPECT_FALSE(_failsafe_flags.gnss_lost);
EXPECT_FALSE(_health_warning_gps);
}
// One receiver fixed, SYS_HAS_NUM_GNSS not configured → no failsafe.
TEST_F(GnssRedundancyChecksTest, SingleGpsNoFailsafe)
{
_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON));
runCheck();
EXPECT_FALSE(_failsafe_flags.gnss_lost);
}
// Two receivers at expected lever-arm separation → no divergence.
TEST_F(GnssRedundancyChecksTest, TwoGpsAgreeingNoFlags)
{
_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON));
_gps1_pub.publish(makeGps(AGREEING_LAT, BASE_LON));
runCheck();
EXPECT_FALSE(_failsafe_flags.gnss_lost);
EXPECT_FALSE(_health_warning_gps);
}
// Two receivers at the same position while a 0.70m lever arm is configured →
// divergence detected (separation = 0, expected_d = 0.70m, deviation = 0.70m >> gate).
// This exercises the "too close" direction of the improved check.
TEST_F(GnssRedundancyChecksTest, TwoGpsTooCloseDivergenceDetected)
{
_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON));
_gps1_pub.publish(makeGps(BASE_LAT, BASE_LON));
// First call starts the hysteresis timer, no flag yet.
runCheck();
EXPECT_FALSE(_failsafe_flags.gnss_lost);
// Hysteresis fires after ~2 s; calling immediately again stays false.
runCheck();
EXPECT_FALSE(_failsafe_flags.gnss_lost);
}
// Two receivers too far apart → hysteresis timer starts but has not elapsed on first call.
TEST_F(GnssRedundancyChecksTest, TwoGpsDivergingFarNotYetSustained)
{
_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON));
_gps1_pub.publish(makeGps(DIVERGING_FAR_LAT, BASE_LON));
runCheck();
EXPECT_FALSE(_failsafe_flags.gnss_lost);
}
// After divergence the receivers recover → hysteresis resets, no flag.
TEST_F(GnssRedundancyChecksTest, TwoGpsDivergingClearsOnRecovery)
{
_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON));
_gps1_pub.publish(makeGps(DIVERGING_FAR_LAT, BASE_LON));
runCheck();
_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON));
_gps1_pub.publish(makeGps(AGREEING_LAT, BASE_LON));
runCheck();
EXPECT_FALSE(_failsafe_flags.gnss_lost);
EXPECT_FALSE(_health_warning_gps);
}
// fix_type < 3 is not counted as a fixed receiver → no divergence check triggered.
TEST_F(GnssRedundancyChecksTest, FixTypeBelow3NotCounted)
{
_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON, 0.02f, 6));
_gps1_pub.publish(makeGps(DIVERGING_FAR_LAT, BASE_LON, 0.02f, 2));
runCheck();
EXPECT_FALSE(_failsafe_flags.gnss_lost);
EXPECT_FALSE(_health_warning_gps);
}
// SYS_HAS_NUM_GNSS = 2, COM_GNSSLOSS_ACT > 0, only one receiver fixed → gnss_lost.
TEST_F(GnssRedundancyChecksTest, BelowRequiredSetsGnssLost)
{
int required = 2; param_set(param_find("SYS_HAS_NUM_GNSS"), &required);
int act = 1; param_set(param_find("COM_GNSSLOSS_ACT"), &act);
_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON));
runCheck();
EXPECT_TRUE(_failsafe_flags.gnss_lost);
EXPECT_TRUE(_health_warning_gps);
}
// After seeing two fixed receivers, losing one emits a health warning
// even when SYS_HAS_NUM_GNSS is not set (dropped_below_peak path).
TEST_F(GnssRedundancyChecksTest, DroppedBelowPeakSetsHealthWarning)
{
_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON));
_gps1_pub.publish(makeGps(AGREEING_LAT, BASE_LON));
runCheck();
EXPECT_FALSE(_health_warning_gps); // both present, no warning
// GPS1 disappears.
sensor_gps_s gone{};
_gps1_pub.publish(gone); // device_id = 0 → treated as absent
runCheck();
EXPECT_TRUE(_health_warning_gps);
EXPECT_FALSE(_failsafe_flags.gnss_lost); // no failsafe without COM_GNSSLOSS_ACT + below_required
}
src/modules/sensors/vehicle_gps_position/CMakeLists.txt
-3
View File
@@ -36,8 +36,6 @@ px4_add_library(vehicle_gps_position
VehicleGPSPosition.hpp
gps_blending.cpp
gps_blending.hpp
GnssRedundancyMonitor.cpp
GnssRedundancyMonitor.hpp
PpsTimeSync.cpp
PpsTimeSync.hpp
)
@@ -48,4 +46,3 @@ target_link_libraries(vehicle_gps_position
)
px4_add_unit_gtest(SRC gps_blending_test.cpp LINKLIBS vehicle_gps_position)
px4_add_functional_gtest(SRC GnssRedundancyMonitorTest.cpp LINKLIBS vehicle_gps_position)
src/modules/sensors/vehicle_gps_position/GnssRedundancyMonitor.cpp
-149
View File
@@ -1,149 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "GnssRedundancyMonitor.hpp"
#include <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
src/modules/sensors/vehicle_gps_position/GnssRedundancyMonitor.hpp
-75
View File
@@ -1,75 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <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
src/modules/sensors/vehicle_gps_position/GnssRedundancyMonitorTest.cpp
-205
View File
@@ -1,205 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <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);
}
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp
-14
View File
@@ -191,21 +191,7 @@ void VehicleGPSPosition::Run()
if (any_gps_updated) {
_gps_blending.update(hrt_absolute_time());
}
// Tick the redundancy monitor every cycle so silence is detected.
{
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
const bool is_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
_gnss_redundancy_monitor.update(_gps_blending.getGpsStates(),
_gps_blending.getAntennaOffsets(),
GPS_MAX_RECEIVERS,
is_armed);
}
if (any_gps_updated) {
if (_gps_blending.isNewOutputDataAvailable()) {
sensor_gps_s gps_output{_gps_blending.getOutputGpsData()};
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp
-4
View File
@@ -46,9 +46,7 @@
#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"
@@ -93,13 +91,11 @@ private:
};
uORB::Subscription _pps_capture_sub{ORB_ID(pps_capture)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
GpsBlending _gps_blending;
PpsTimeSync _pps_time_sync;
GnssRedundancyMonitor _gnss_redundancy_monitor{this};
struct GpsParamSlot {
uint32_t device_id{0};
src/modules/sensors/vehicle_gps_position/gps_blending.hpp
-6
View File
@@ -99,12 +99,6 @@ public:
}
int getSelectedGps() const { return _selected_gps; }
// Exposed for the redundancy monitor so it can inspect the last known
// sample (and its resolved antenna offset) from each instance without
// maintaining a second set of subscriptions.
const sensor_gps_s *getGpsStates() const { return _gps_state; }
const Vector3f *getAntennaOffsets() const { return _antenna_offset; }
private:
/*
* Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical