diff --git a/msg/versioned/AuxGlobalPosition.msg b/msg/versioned/AuxGlobalPosition.msg index ccaf5d952d..cb71ecbecb 100644 --- a/msg/versioned/AuxGlobalPosition.msg +++ b/msg/versioned/AuxGlobalPosition.msg @@ -1,23 +1,31 @@ # Auxiliary global position - +# # This message provides global position data from an external source such as -# pseudolites, viusal navigation, or other positioning system. +# pseudolites, visual navigation, or other positioning system. uint32 MESSAGE_VERSION = 0 uint64 timestamp # [us] Time since system start uint64 timestamp_sample # [us] Timestamp of the raw data -uint8 id # Unique identifier for the AGP source -uint8 source # 0: Unkown, 1: GNSS, 2: Vision, 3: Pseudolites, 4: Terrain, 5: Magnetic, 6: Estimator +uint8 id # [-] Unique identifier for the AGP source +uint8 source # [@enum SOURCE] Source type of the position data (based on mavlink::GLOBAL_POSITION_SRC) +uint8 SOURCE_UNKNOWN = 0 # Unknown source +uint8 SOURCE_GNSS = 1 # GNSS +uint8 SOURCE_VISION = 2 # Vision +uint8 SOURCE_PSEUDOLITES = 3 # Pseudolites +uint8 SOURCE_TERRAIN = 4 # Terrain +uint8 SOURCE_MAGNETIC = 5 # Magnetic +uint8 SOURCE_ESTIMATOR = 6 # Estimator +# lat, lon: required for horizontal position fusion, alt: required for vertical position fusion float64 lat # [deg] Latitude in WGS84 float64 lon # [deg] Longitude in WGS84 -float32 alt # [m] Altitude above mean sea level (AMSL) +float32 alt # [m] [@invalid NaN] Altitude above mean sea level (AMSL) -float32 eph # [m] Standard deviation of horizontal position error -float32 epv # [m] Standard deviation of vertical position error +float32 eph # [m] [@invalid NaN] Std dev of horizontal position, lower bounded by NOISE param +float32 epv # [m] [@invalid NaN] Std dev of vertical position, lower bounded by NOISE param -uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates +uint8 lat_lon_reset_counter # [-] Counter for reset events on horizontal position coordinates # TOPICS aux_global_position diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index 84c9234894..a205a8bb94 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -74,7 +74,7 @@ void AuxGlobalPosition::paramsUpdated() } } -float AuxGlobalPosition::test_ratio_filtered() const +float AuxGlobalPosition::testRatioFiltered() const { float max_ratio = 0.f; diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp index cda09c4f25..26ad5be09c 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp @@ -60,7 +60,10 @@ public: updateParams(); } - float test_ratio_filtered() const; + /** + * Returns the maximum filtered test ratio across all active AGP sources. + */ + float testRatioFiltered() const; bool anySourceFusing() const; int32_t getIdParam(int instance); void setIdParam(int instance, int32_t sensor_id); diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp index c6a57dce70..84e5aa062c 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp @@ -236,8 +236,6 @@ void AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed) if (!_manager->anySourceFusing()) { ekf.disableControlStatusAuxGpos(); } - - ECL_WARN("Aux global position data stopped for instance %d", _instance_id); } } diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp index b3667b2c8d..6e81dbf072 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * 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 diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index d568487da6..f5b8703c0d 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -541,7 +541,7 @@ float Ekf::getHorizontalPositionInnovationTestRatio() const #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) if (_control_status.flags.aux_gpos) { - test_ratio = math::max(test_ratio, fabsf(_aux_global_position.test_ratio_filtered())); + test_ratio = math::max(test_ratio, fabsf(_aux_global_position.testRatioFiltered())); } #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION diff --git a/src/modules/ekf2/params_aux_global_position.yaml b/src/modules/ekf2/params_aux_global_position.yaml index e376785010..7928cfcc29 100644 --- a/src/modules/ekf2/params_aux_global_position.yaml +++ b/src/modules/ekf2/params_aux_global_position.yaml @@ -6,7 +6,7 @@ parameters: definitions: EKF2_AGP${i}_ID: description: - short: Aux global position sensor ${i} ID + short: Auxiliary global position sensor ${i} ID long: 'Sensor ID for slot ${i}. Set to 0 to disable this slot.' type: int32 default: 0 @@ -16,7 +16,7 @@ parameters: instance_start: 0 EKF2_AGP${i}_CTRL: description: - short: Aux global position sensor ${i} aiding + short: Auxiliary global position sensor ${i} aiding long: 'Set bits in the following positions to enable: 0 : Horizontal position fusion 1 : Vertical position fusion' type: bitmask @@ -42,7 +42,7 @@ parameters: instance_start: 0 EKF2_AGP${i}_DELAY: description: - short: Aux global position sensor ${i} delay relative to IMU measurements + short: Auxiliary global position sensor ${i} delay relative to IMU measurements type: float default: 0 min: 0 @@ -54,7 +54,7 @@ parameters: instance_start: 0 EKF2_AGP${i}_NOISE: description: - short: Measurement noise for aux global position sensor ${i} + short: Measurement noise for auxiliary global position sensor ${i} long: Used to lower bound or replace the uncertainty included in the message type: float default: 1.0 @@ -65,7 +65,7 @@ parameters: instance_start: 0 EKF2_AGP${i}_GATE: description: - short: Gate size for aux global position sensor ${i} fusion + short: Gate size for auxiliary global position sensor ${i} fusion long: Sets the number of standard deviations used by the innovation consistency test. type: float