Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-03-23 18:13:49 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

feat(sensors/gps): move GPS antenna offsets to per-receiver parameters (#26634)

Browse Source 
* sensors: move GPS antenna offsets to per-receiver parameters

Move antenna position configuration from the single EKF2_GPS_POS_X/Y/Z
parameter set into per-receiver SENS_GPS{0,1}_OFF{X,Y,Z} parameters in
the sensors module. Each offset slot is matched to a physical receiver
by device ID (SENS_GPS{0,1}_ID), falling back to uORB instance index
when no IDs are configured.

The antenna offset is now carried through the SensorGps uORB message
and blended alongside other GPS states when multi-receiver blending is
active, so EKF2 receives the correct lever arm for whichever receiver
(or weighted combination) is selected.

- Add antenna_offset_{x,y,z} fields to SensorGps.msg
- Remove EKF2_GPS_POS_X/Y/Z params; EKF2 reads offset from gnssSample
- Add SENS_GPS{0,1}_ID and SENS_GPS{0,1}_OFF{X,Y,Z} params (module.yaml)
- Blend antenna offsets in GpsBlending (weighted average)
- Add unit tests for single, blended, and failover antenna offset cases
- Migrate params.c to module.yaml for the vehicle_gps_position module

* sensors: gps_blending: add asymmetric weight and fallthrough offset tests

Add two additional antenna offset test cases:

- dualReceiverAsymmetricWeightAntennaOffset: verify that unequal eph
  values produce correctly skewed blend weights (0.8/0.2) and that the
  output antenna offset reflects the weighted average
- blendingFallthroughAntennaOffset: verify that when blending is enabled
  but can_do_blending evaluates false (eph=0), the non-blending path
  correctly assigns the selected receiver's antenna offset

* feat(param_translation): translate EKF2_GPS_POS_ to SENS_GPS0_OFF_

* fix(msgs): proper formatting

* chore(msg): 0 if invalid/unknown

* fix(ROMFS): migrate EKF2_GPS_POS_ params

* fix(docs): migrate EKF2_GPS_POS_ params

* fix(blending): unsigned param

* Update msg/SensorGps.msg

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* fix(sensors/gps): remove 'values:' tag in  module.yaml

* fix(sensors/gps): unsigned instance index

* fix(blending): restore const on gps_blend_states()

Move antenna offset blending into blend_gps_data() where the
weights are computed, keeping gps_blend_states() const.

* fix(sensors/gps): fix msg annotation and restore SENS_GPS_PRIME values

Remove incorrect @invalid NaN annotation from antenna offset fields
(0.0 default is correct, not a sentinel). Restore values tag for
SENS_GPS_PRIME so QGC shows a dropdown.

* fix(gps_blending): fix pre-existing bug to clear _gps_updated flags

The loop iterates over i but always clears gps_select_index. The intent is to clear
all updated flags, but only the selected one gets cleared (N times)

* test(gps_blending): add stale update flag regression test
This commit is contained in:
Jacob Dahl
2026-03-17 17:33:41 -08:00
committed by GitHub
parent b597227da6
commit 89855926ef
29 changed files with 424 additions and 144 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision
View File

@@ -20,8 +20,8 @@ param set-default COM_DISARM_LAND 0.5
# EKF2 parameters
param set-default EKF2_DRAG_CTRL 1
param set-default EKF2_IMU_POS_X 0.02
param set-default EKF2_GPS_POS_X 0.055
param set-default EKF2_GPS_POS_Z -0.15
param set-default SENS_GPS0_OFFX 0.055
param set-default SENS_GPS0_OFFZ -0.15
param set-default EKF2_MIN_RNG 0.03
param set-default EKF2_OF_CTRL 1
param set-default EKF2_OF_POS_X 0.055

4
ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision
View File

@@ -19,8 +19,8 @@ param set-default COM_DISARM_LAND 0.5
# EKF2 parameters
param set-default EKF2_DRAG_CTRL 1
param set-default EKF2_IMU_POS_X 0.02
param set-default EKF2_GPS_POS_X 0.055
param set-default EKF2_GPS_POS_Z -0.15
param set-default SENS_GPS0_OFFX 0.055
param set-default SENS_GPS0_OFFZ -0.15
param set-default EKF2_MIN_RNG 0.03
param set-default EKF2_OF_CTRL 1
param set-default EKF2_OF_POS_X 0.055

4
ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5
View File

@@ -19,8 +19,8 @@ param set-default COM_DISARM_LAND 0.5
# EKF2 parameters
param set-default EKF2_DRAG_CTRL 1
param set-default EKF2_IMU_POS_X 0.02
param set-default EKF2_GPS_POS_X 0.055
param set-default EKF2_GPS_POS_Z -0.15
param set-default SENS_GPS0_OFFX 0.055
param set-default SENS_GPS0_OFFZ -0.15
param set-default EKF2_MIN_RNG 0.03
param set-default EKF2_OF_CTRL 1
param set-default EKF2_OF_POS_X 0.055

2
ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu
View File

@@ -48,7 +48,7 @@ param set-default EKF2_BCOEF_Y 25.5
param set-default EKF2_DRAG_CTRL 1
param set-default EKF2_GPS_DELAY 100
param set-default EKF2_GPS_POS_X 0.06
param set-default SENS_GPS0_OFFX 0.06
param set-default EKF2_GPS_V_NOISE 0.5
param set-default EKF2_IMU_POS_X 0.06

2
docs/en/assembly/mount_gps_compass.md
View File

@@ -20,7 +20,7 @@ For more information see [Setting the Compass Orientation](../config/flight_cont
## Position
In order to compensate for the relative motion between the receiver and the CoG, you should [configure](../advanced_config/parameters.md) the following parameters to set the offsets: [EKF2_GPS_POS_X](../advanced_config/parameter_reference.md#EKF2_GPS_POS_X), [EKF2_GPS_POS_Y](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Y) and [EKF2_GPS_POS_Z](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Z).
In order to compensate for the relative motion between the receiver and the CoG, you should [configure](../advanced_config/parameters.md) the following parameters to set the offsets: [SENS_GPS0_OFFX](../advanced_config/parameter_reference.md#SENS_GPS0_OFFX), [SENS_GPS0_OFFY](../advanced_config/parameter_reference.md#SENS_GPS0_OFFY) and [SENS_GPS0_OFFZ](../advanced_config/parameter_reference.md#SENS_GPS0_OFFZ).
This is important because the body frame estimated by the EKF will converge on the location of the GNSS module and assume it to be at the CoG. If the GNSS module is significantly offset from the CoG, then rotation around the COG will be interpreted as an altitude change, which in some flight modes (such as position mode) will result in unnecessary corrections.

2
docs/en/dronecan/ark_g5_rtk_gps.md
View File

@@ -97,7 +97,7 @@ There is also CAN built-in bus termination via [CANNODE_TERM](../advanced_config
You need to set necessary [DroneCAN](index.md) parameters and define offsets if the sensor is not centred within the vehicle:
- Enable [UAVCAN_SUB_GPS](../advanced_config/parameter_reference.md#UAVCAN_SUB_GPS), [UAVCAN_SUB_MAG](../advanced_config/parameter_reference.md#UAVCAN_SUB_MAG), and [UAVCAN_SUB_BARO](../advanced_config/parameter_reference.md#UAVCAN_SUB_BARO).
- The parameters [EKF2_GPS_POS_X](../advanced_config/parameter_reference.md#EKF2_GPS_POS_X), [EKF2_GPS_POS_Y](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Y) and [EKF2_GPS_POS_Z](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Z) can be set to account for the offset of the ARK G5 RTK GPS from the vehicle's centre of gravity.
- The parameters [SENS_GPS0_OFFX](../advanced_config/parameter_reference.md#SENS_GPS0_OFFX), [SENS_GPS0_OFFY](../advanced_config/parameter_reference.md#SENS_GPS0_OFFY) and [SENS_GPS0_OFFZ](../advanced_config/parameter_reference.md#SENS_GPS0_OFFZ) can be set to account for the offset of the ARK G5 RTK GPS from the vehicle's centre of gravity.
## LED Meanings

2
docs/en/dronecan/ark_g5_rtk_heading_gps.md
View File

@@ -99,7 +99,7 @@ You need to set necessary [DroneCAN](index.md) parameters and define offsets if
- Enable GPS yaw fusion by setting bit 3 of [EKF2_GPS_CTRL](../advanced_config/parameter_reference.md#EKF2_GPS_CTRL) to true.
- Enable GPS blending to ensure the heading is always published by setting [SENS_GPS_MASK](../advanced_config/parameter_reference.md#SENS_GPS_MASK) to 7 (all three bits checked).
- Enable [UAVCAN_SUB_GPS](../advanced_config/parameter_reference.md#UAVCAN_SUB_GPS), [UAVCAN_SUB_MAG](../advanced_config/parameter_reference.md#UAVCAN_SUB_MAG), and [UAVCAN_SUB_BARO](../advanced_config/parameter_reference.md#UAVCAN_SUB_BARO).
- The parameters [EKF2_GPS_POS_X](../advanced_config/parameter_reference.md#EKF2_GPS_POS_X), [EKF2_GPS_POS_Y](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Y) and [EKF2_GPS_POS_Z](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Z) can be set to account for the offset of the ARK G5 RTK HEADING GPS from the vehicle's centre of gravity.
- The parameters [SENS_GPS0_OFFX](../advanced_config/parameter_reference.md#SENS_GPS0_OFFX), [SENS_GPS0_OFFY](../advanced_config/parameter_reference.md#SENS_GPS0_OFFY) and [SENS_GPS0_OFFZ](../advanced_config/parameter_reference.md#SENS_GPS0_OFFZ) can be set to account for the offset of the ARK G5 RTK HEADING GPS from the vehicle's centre of gravity.
### Parameter references

2
docs/en/dronecan/ark_gps.md
View File

@@ -91,7 +91,7 @@ If the sensor is not centred within the vehicle you will also need to define sen
- Enable GPS yaw fusion by setting bit 3 of [EKF2_GPS_CTRL](../advanced_config/parameter_reference.md#EKF2_GPS_CTRL) to true.
- Enable [UAVCAN_SUB_GPS](../advanced_config/parameter_reference.md#UAVCAN_SUB_GPS), [UAVCAN_SUB_MAG](../advanced_config/parameter_reference.md#UAVCAN_SUB_MAG), and [UAVCAN_SUB_BARO](../advanced_config/parameter_reference.md#UAVCAN_SUB_BARO).
- The parameters [EKF2_GPS_POS_X](../advanced_config/parameter_reference.md#EKF2_GPS_POS_X), [EKF2_GPS_POS_Y](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Y) and [EKF2_GPS_POS_Z](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Z) can be set to account for the offset of the ARK GPS from the vehicles centre of gravity.
- The parameters [SENS_GPS0_OFFX](../advanced_config/parameter_reference.md#SENS_GPS0_OFFX), [SENS_GPS0_OFFY](../advanced_config/parameter_reference.md#SENS_GPS0_OFFY) and [SENS_GPS0_OFFZ](../advanced_config/parameter_reference.md#SENS_GPS0_OFFZ) can be set to account for the offset of the ARK GPS from the vehicles centre of gravity.
### ARK GPS Configuration

2
docs/en/dronecan/ark_rtk_gps.md
View File

@@ -86,7 +86,7 @@ You need to set necessary [DroneCAN](index.md) parameters and define offsets if
- Enable GPS blending to ensure the heading is always published by setting [SENS_GPS_MASK](../advanced_config/parameter_reference.md#SENS_GPS_MASK) to 7 (all three bits checked).
- If using [Moving Baseline & GPS Heading](#setting-up-moving-baseline--gps-heading), set [SENS_GPS_PRIME](../advanced_config/parameter_reference.md#SENS_GPS_PRIME) to the CAN node ID of the _Moving Base_ module. The moving base is preferred because the rover receiver in a moving baseline configuration can experience degraded navigation rate and increased data latency when corrections are intermittent.
- Enable [UAVCAN_SUB_GPS](../advanced_config/parameter_reference.md#UAVCAN_SUB_GPS), [UAVCAN_SUB_MAG](../advanced_config/parameter_reference.md#UAVCAN_SUB_MAG), and [UAVCAN_SUB_BARO](../advanced_config/parameter_reference.md#UAVCAN_SUB_BARO).
- The parameters [EKF2_GPS_POS_X](../advanced_config/parameter_reference.md#EKF2_GPS_POS_X), [EKF2_GPS_POS_Y](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Y) and [EKF2_GPS_POS_Z](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Z) can be set to account for the offset of the ARK RTK GPS from the vehicles centre of gravity.
- The parameters [SENS_GPS0_OFFX](../advanced_config/parameter_reference.md#SENS_GPS0_OFFX), [SENS_GPS0_OFFY](../advanced_config/parameter_reference.md#SENS_GPS0_OFFY) and [SENS_GPS0_OFFZ](../advanced_config/parameter_reference.md#SENS_GPS0_OFFZ) can be set to account for the offset of the ARK RTK GPS from the vehicles centre of gravity.
### ARK RTK GPS Configuration

2
docs/en/dronecan/ark_rtk_gps_l1_l2.md
View File

@@ -85,7 +85,7 @@ You need to set necessary [DroneCAN](index.md) parameters and define offsets if
- Enable GPS yaw fusion by setting bit 3 of [EKF2_GPS_CTRL](../advanced_config/parameter_reference.md#EKF2_GPS_CTRL) to true.
- Enable GPS blending to ensure the heading is always published by setting [SENS_GPS_MASK](../advanced_config/parameter_reference.md#SENS_GPS_MASK) to 7 (all three bits checked).
- Enable [UAVCAN_SUB_GPS](../advanced_config/parameter_reference.md#UAVCAN_SUB_GPS), [UAVCAN_SUB_MAG](../advanced_config/parameter_reference.md#UAVCAN_SUB_MAG), and [UAVCAN_SUB_BARO](../advanced_config/parameter_reference.md#UAVCAN_SUB_BARO).
- The parameters [EKF2_GPS_POS_X](../advanced_config/parameter_reference.md#EKF2_GPS_POS_X), [EKF2_GPS_POS_Y](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Y) and [EKF2_GPS_POS_Z](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Z) can be set to account for the offset of the ARK RTK GPS L1 L5 from the vehicles centre of gravity.
- The parameters [SENS_GPS0_OFFX](../advanced_config/parameter_reference.md#SENS_GPS0_OFFX), [SENS_GPS0_OFFY](../advanced_config/parameter_reference.md#SENS_GPS0_OFFY) and [SENS_GPS0_OFFZ](../advanced_config/parameter_reference.md#SENS_GPS0_OFFZ) can be set to account for the offset of the ARK RTK GPS L1 L5 from the vehicles centre of gravity.
### ARK RTK GPS L1 L5 Configuration

2
docs/en/dronecan/ark_x20_rtk_gps.md
View File

@@ -88,7 +88,7 @@ You need to set necessary [DroneCAN](index.md) parameters and define offsets if
- Enable GPS yaw fusion by setting bit 3 of [EKF2_GPS_CTRL](../advanced_config/parameter_reference.md#EKF2_GPS_CTRL) to true.
- Enable GPS blending to ensure the heading is always published by setting [SENS_GPS_MASK](../advanced_config/parameter_reference.md#SENS_GPS_MASK) to 7 (all three bits checked).
- Enable [UAVCAN_SUB_GPS](../advanced_config/parameter_reference.md#UAVCAN_SUB_GPS), [UAVCAN_SUB_MAG](../advanced_config/parameter_reference.md#UAVCAN_SUB_MAG), and [UAVCAN_SUB_BARO](../advanced_config/parameter_reference.md#UAVCAN_SUB_BARO).
- The parameters [EKF2_GPS_POS_X](../advanced_config/parameter_reference.md#EKF2_GPS_POS_X), [EKF2_GPS_POS_Y](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Y) and [EKF2_GPS_POS_Z](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Z) can be set to account for the offset of the ARK X20 RTK GPS from the vehicles centre of gravity.
- The parameters [SENS_GPS0_OFFX](../advanced_config/parameter_reference.md#SENS_GPS0_OFFX), [SENS_GPS0_OFFY](../advanced_config/parameter_reference.md#SENS_GPS0_OFFY) and [SENS_GPS0_OFFZ](../advanced_config/parameter_reference.md#SENS_GPS0_OFFZ) can be set to account for the offset of the ARK X20 RTK GPS from the vehicles centre of gravity.
### ARK X20 RTK GPS Configuration

2
docs/en/dronecan/holybro_m8n_gps.md
View File

@@ -94,4 +94,4 @@ If the sensor is not centred within the vehicle you will also need to define sen
- Enable GPS yaw fusion by setting bit 3 of [EKF2_GPS_CTRL](../advanced_config/parameter_reference.md#EKF2_GPS_CTRL) to true.
- Enable [UAVCAN_SUB_GPS](../advanced_config/parameter_reference.md#UAVCAN_SUB_GPS), [UAVCAN_SUB_MAG](../advanced_config/parameter_reference.md#UAVCAN_SUB_MAG), and [UAVCAN_SUB_BARO](../advanced_config/parameter_reference.md#UAVCAN_SUB_BARO).
- Set [CANNODE_TERM](../advanced_config/parameter_reference.md#CANNODE_TERM) to `1` if this is that last node on the CAN bus.
- The parameters [EKF2_GPS_POS_X](../advanced_config/parameter_reference.md#EKF2_GPS_POS_X), [EKF2_GPS_POS_Y](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Y) and [EKF2_GPS_POS_Z](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Z) can be set to account for the offset of the ARK GPS from the vehicles centre of gravity.
- The parameters [SENS_GPS0_OFFX](../advanced_config/parameter_reference.md#SENS_GPS0_OFFX), [SENS_GPS0_OFFY](../advanced_config/parameter_reference.md#SENS_GPS0_OFFY) and [SENS_GPS0_OFFZ](../advanced_config/parameter_reference.md#SENS_GPS0_OFFZ) can be set to account for the offset of the ARK GPS from the vehicles centre of gravity.

2
docs/en/dronecan/index.md
View File

@@ -188,7 +188,7 @@ GPS CANNODE parameter ([set using QGC](#qgc-cannode-parameter-configuration)):
Other PX4 Parameters:
- If the GPS is not positioned at the vehicle centre of gravity you can account for the offset using [EKF2_GPS_POS_X](../advanced_config/parameter_reference.md#EKF2_GPS_POS_X), [EKF2_GPS_POS_Y](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Y) and [EKF2_GPS_POS_Z](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Z).
- If the GPS is not positioned at the vehicle centre of gravity you can account for the offset using [SENS_GPS0_OFFX](../advanced_config/parameter_reference.md#SENS_GPS0_OFFX), [SENS_GPS0_OFFY](../advanced_config/parameter_reference.md#SENS_GPS0_OFFY) and [SENS_GPS0_OFFZ](../advanced_config/parameter_reference.md#SENS_GPS0_OFFZ).
- If the GPS module provides yaw information, you can enable GPS yaw fusion by setting bit 3 of [EKF2_GPS_CTRL](../advanced_config/parameter_reference.md#EKF2_GPS_CTRL) to true.
#### RTK GPS

4
msg/SensorGps.msg
View File

@@ -87,4 +87,8 @@ uint8 RTCM_MSG_USED_NOT_USED = 1
uint8 RTCM_MSG_USED_USED = 2
uint8 rtcm_msg_used # Indicates if the RTCM message was used successfully by the receiver
float32 antenna_offset_x # [m] [@frame body frame FRD] X Position of GNSS antenna
float32 antenna_offset_y # [m] [@frame body frame FRD] Y Position of GNSS antenna
float32 antenna_offset_z # [m] [@frame body frame FRD] Z Position of GNSS antenna
# TOPICS sensor_gps vehicle_gps_position

21
src/lib/parameters/param_translation.cpp
View File

@@ -178,6 +178,27 @@ param_modify_on_import_ret param_modify_on_import(bson_node_t node)
}
}
// 2026-03-11: translate EKF2_GPS_POS_X/Y/Z -> SENS_GPS0_OFFX/OFFY/OFFZ
{
if (strcmp("EKF2_GPS_POS_X", node->name) == 0) {
strcpy(node->name, "SENS_GPS0_OFFX");
PX4_INFO("migrating %s -> %s", "EKF2_GPS_POS_X", "SENS_GPS0_OFFX");
return param_modify_on_import_ret::PARAM_MODIFIED;
}
if (strcmp("EKF2_GPS_POS_Y", node->name) == 0) {
strcpy(node->name, "SENS_GPS0_OFFY");
PX4_INFO("migrating %s -> %s", "EKF2_GPS_POS_Y", "SENS_GPS0_OFFY");
return param_modify_on_import_ret::PARAM_MODIFIED;
}
if (strcmp("EKF2_GPS_POS_Z", node->name) == 0) {
strcpy(node->name, "SENS_GPS0_OFFZ");
PX4_INFO("migrating %s -> %s", "EKF2_GPS_POS_Z", "SENS_GPS0_OFFZ");
return param_modify_on_import_ret::PARAM_MODIFIED;
}
}
// 2026-03-11: translate MOT_POLE_COUNT to per-motor DSHOT_MOT_POL1-12
{
if ((node->type == bson_type_t::BSON_INT32) && (strcmp("MOT_POLE_COUNT", node->name) == 0)) {

2
src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp
View File

@@ -60,7 +60,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
}
}
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
const Vector3f pos_offset_body = gps_sample.pos_body - _params.imu_pos_body;
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
const float gnss_alt = gps_sample.alt + pos_offset_earth(2);

4
src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp
View File

@@ -304,7 +304,7 @@ bool Ekf::isGnssPosResetAllowed() const
void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src)
{
// correct velocity for offset relative to IMU
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
const Vector3f pos_offset_body = gnss_sample.pos_body - _params.imu_pos_body;
const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias;
const Vector3f vel_offset_body = angular_velocity % pos_offset_body;
@@ -342,7 +342,7 @@ void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_samp
void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src)
{
// correct position and height for offset relative to IMU
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
const Vector3f pos_offset_body = gnss_sample.pos_body - _params.imu_pos_body;
const Vector3f pos_offset_earth = Vector3f(_R_to_earth * pos_offset_body);
const LatLonAlt measurement(gnss_sample.lat, gnss_sample.lon, gnss_sample.alt);
const LatLonAlt measurement_corrected = measurement + (-pos_offset_earth);

3
src/modules/ekf2/EKF/common.h
View File

@@ -203,6 +203,7 @@ struct gnssSample {
float yaw_offset{}; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
bool spoofed{}; ///< true if GNSS data is spoofed
bool jammed{}; ///< true if GNSS data is jammed
Vector3f pos_body{}; ///< position of GPS antenna in body frame (m)
};
struct magSample {
@@ -332,8 +333,6 @@ struct parameters {
int32_t ekf2_gps_mode {static_cast<int32_t>(GnssMode::kAuto)};
float ekf2_gps_delay{110.0f}; ///< GPS measurement delay relative to the IMU (mSec)
Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m)
// position and velocity fusion
float ekf2_gps_v_noise{0.5f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec)
float ekf2_gps_p_noise{0.5f}; ///< minimum allowed observation noise for gps position fusion (m)

6
src/modules/ekf2/EKF2.cpp
View File

@@ -81,9 +81,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_gps_ctrl(_params->ekf2_gps_ctrl),
_param_ekf2_gps_mode(_params->ekf2_gps_mode),
_param_ekf2_gps_delay(_params->ekf2_gps_delay),
_param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
_param_ekf2_gps_pos_y(_params->gps_pos_body(1)),
_param_ekf2_gps_pos_z(_params->gps_pos_body(2)),
_param_ekf2_gps_v_noise(_params->ekf2_gps_v_noise),
_param_ekf2_gps_p_noise(_params->ekf2_gps_p_noise),
_param_ekf2_gps_p_gate(_params->ekf2_gps_p_gate),
@@ -2458,6 +2455,9 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
.yaw_offset = vehicle_gps_position.heading_offset,
.spoofed = vehicle_gps_position.spoofing_state == sensor_gps_s::SPOOFING_STATE_DETECTED,
.jammed = vehicle_gps_position.jamming_state == sensor_gps_s::JAMMING_STATE_DETECTED,
.pos_body = Vector3f(vehicle_gps_position.antenna_offset_x,
vehicle_gps_position.antenna_offset_y,
vehicle_gps_position.antenna_offset_z),
};
_ekf.setGpsData(gnss_sample, pps_compensation);

4
src/modules/ekf2/EKF2.hpp
View File

@@ -516,10 +516,6 @@ private:
(ParamExtInt<px4::params::EKF2_GPS_MODE>) _param_ekf2_gps_mode,
(ParamExtFloat<px4::params::EKF2_GPS_DELAY>) _param_ekf2_gps_delay,
(ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x,
(ParamExtFloat<px4::params::EKF2_GPS_POS_Y>) _param_ekf2_gps_pos_y,
(ParamExtFloat<px4::params::EKF2_GPS_POS_Z>) _param_ekf2_gps_pos_z,
(ParamExtFloat<px4::params::EKF2_GPS_V_NOISE>) _param_ekf2_gps_v_noise,
(ParamExtFloat<px4::params::EKF2_GPS_P_NOISE>) _param_ekf2_gps_p_noise,

24
src/modules/ekf2/params_gnss.yaml
View File

@@ -86,30 +86,6 @@ parameters:
max: 5.0
unit: m/s
decimal: 2
EKF2_GPS_POS_X:
description:
short: X position of GPS antenna in body frame
long: Forward (roll) axis with origin relative to vehicle centre of gravity
type: float
default: 0.0
unit: m
decimal: 3
EKF2_GPS_POS_Y:
description:
short: Y position of GPS antenna in body frame
long: Right (pitch) axis with origin relative to vehicle centre of gravity
type: float
default: 0.0
unit: m
decimal: 3
EKF2_GPS_POS_Z:
description:
short: Z position of GPS antenna in body frame
long: Down (yaw) axis with origin relative to vehicle centre of gravity
type: float
default: 0.0
unit: m
decimal: 3
EKF2_GPS_CHECK:
description:
short: Integer bitmask controlling GPS checks

1
src/modules/sensors/CMakeLists.txt
View File

@@ -73,6 +73,7 @@ px4_add_module(
Integrator.hpp
MODULE_CONFIG
module.yaml
vehicle_gps_position/module.yaml
DEPENDS
conversion
data_validator

34
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp
View File

@@ -102,6 +102,15 @@ void VehicleGPSPosition::ParametersUpdate(bool force)
if (math::isInRange(gps_prime, -1, 1)) {
_gps_blending.setPrimaryInstance(gps_prime);
}
_gps_offset_slots[0] = {
static_cast<uint32_t>(_param_sens_gps0_id.get()),
{_param_sens_gps0_offx.get(), _param_sens_gps0_offy.get(), _param_sens_gps0_offz.get()}
};
_gps_offset_slots[1] = {
static_cast<uint32_t>(_param_sens_gps1_id.get()),
{_param_sens_gps1_offx.get(), _param_sens_gps1_offy.get(), _param_sens_gps1_offz.get()}
};
}
}
@@ -130,6 +139,26 @@ void VehicleGPSPosition::Run()
any_gps_updated = true;
_sensor_gps_sub[i].copy(&gps_data);
// Match device_id to antenna offset slot
matrix::Vector3f antenna_offset{};
bool matched = false;
for (uint8_t slot = 0; slot < GPS_MAX_RECEIVERS; slot++) {
if (_gps_offset_slots[slot].device_id != 0
&& _gps_offset_slots[slot].device_id == gps_data.device_id) {
antenna_offset = _gps_offset_slots[slot].offset;
matched = true;
break;
}
}
// Fallback: if no device IDs configured, match by instance index
if (!matched && _gps_offset_slots[0].device_id == 0 && _gps_offset_slots[1].device_id == 0) {
antenna_offset = _gps_offset_slots[i].offset;
}
_gps_blending.setAntennaOffset(antenna_offset, i);
_gps_blending.setGpsData(gps_data, i);
if (math::isInRange(static_cast<int>(gps_prime), 2, 127)) {
@@ -159,6 +188,11 @@ void VehicleGPSPosition::Run()
gps_output.device_id = 0;
}
const matrix::Vector3f &out_offset = _gps_blending.getOutputAntennaOffset();
gps_output.antenna_offset_x = out_offset(0);
gps_output.antenna_offset_y = out_offset(1);
gps_output.antenna_offset_z = out_offset(2);
gps_output.timestamp_sample = _pps_time_sync.correct_gps_timestamp(gps_output.timestamp, gps_output.time_utc_usec);
_vehicle_gps_position_pub.publish(gps_output);
}

15
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp
View File

@@ -97,10 +97,23 @@ private:
GpsBlending _gps_blending;
PpsTimeSync _pps_time_sync;
struct GpsOffsetSlot {
uint32_t device_id{0};
matrix::Vector3f offset{};
} _gps_offset_slots[GPS_MAX_RECEIVERS] {};
DEFINE_PARAMETERS(
(ParamInt<px4::params::SENS_GPS_MASK>) _param_sens_gps_mask,
(ParamFloat<px4::params::SENS_GPS_TAU>) _param_sens_gps_tau,
(ParamInt<px4::params::SENS_GPS_PRIME>) _param_sens_gps_prime
(ParamInt<px4::params::SENS_GPS_PRIME>) _param_sens_gps_prime,
(ParamInt<px4::params::SENS_GPS0_ID>) _param_sens_gps0_id,
(ParamFloat<px4::params::SENS_GPS0_OFFX>) _param_sens_gps0_offx,
(ParamFloat<px4::params::SENS_GPS0_OFFY>) _param_sens_gps0_offy,
(ParamFloat<px4::params::SENS_GPS0_OFFZ>) _param_sens_gps0_offz,
(ParamInt<px4::params::SENS_GPS1_ID>) _param_sens_gps1_id,
(ParamFloat<px4::params::SENS_GPS1_OFFX>) _param_sens_gps1_offx,
(ParamFloat<px4::params::SENS_GPS1_OFFY>) _param_sens_gps1_offy,
(ParamFloat<px4::params::SENS_GPS1_OFFZ>) _param_sens_gps1_offz
)
};
}; // namespace sensors

17
src/modules/sensors/vehicle_gps_position/gps_blending.cpp
View File

@@ -82,10 +82,12 @@ void GpsBlending::update(uint64_t hrt_now_us)
}
_selected_gps = gps_select_index;
_output_antenna_offset = _antenna_offset[gps_select_index];
_is_new_output_data_available = _gps_updated[gps_select_index];
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
_gps_updated[gps_select_index] = false;
// clear updated flags
_gps_updated[i] = false;
}
}
@@ -340,6 +342,15 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us)
// offsets for each physical receiver
sensor_gps_s gps_blended_state = gps_blend_states(blend_weights);
// blend antenna offsets using the same weights
_output_antenna_offset.zero();
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
if (blend_weights[i] > 0.0f) {
_output_antenna_offset += _antenna_offset[i] * blend_weights[i];
}
}
update_gps_offsets(gps_blended_state);
// calculate a blended output from the offset corrected receiver data
@@ -438,10 +449,6 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS
}
}
// TODO read parameters for individual GPS antenna positions and blend
// Vector3f temp_antenna_offset = _antenna_offset[i];
// temp_antenna_offset *= blend_weights[i];
// _blended_antenna_offset += temp_antenna_offset;
}
/*

11
src/modules/sensors/vehicle_gps_position/gps_blending.hpp
View File

@@ -47,6 +47,7 @@
#include <lib/mathlib/mathlib.h>
using matrix::Vector2f;
using matrix::Vector3f;
using math::constrain;
using namespace time_literals;
@@ -65,7 +66,7 @@ public:
// define max number of GPS receivers supported for blending
static constexpr int GPS_MAX_RECEIVERS_BLEND = 2;
void setGpsData(const sensor_gps_s &gps_data, int instance)
void setGpsData(const sensor_gps_s &gps_data, uint8_t instance)
{
if (instance < GPS_MAX_RECEIVERS_BLEND) {
_gps_state[instance] = gps_data;
@@ -77,6 +78,11 @@ public:
void setBlendingUseVPosAccuracy(bool enabled) { _blend_use_vpos_acc = enabled; }
void setBlendingTimeConstant(float tau) { _blending_time_constant = tau; }
void setPrimaryInstance(int primary) { _primary_instance = primary; }
void setAntennaOffset(const Vector3f &offset, uint8_t instance)
{
if (instance < GPS_MAX_RECEIVERS_BLEND) { _antenna_offset[instance] = offset; }
}
const Vector3f &getOutputAntennaOffset() const { return _output_antenna_offset; }
void update(uint64_t hrt_now_us);
@@ -144,4 +150,7 @@ private:
bool _blend_use_vpos_acc{false};
float _blending_time_constant{0.f};
Vector3f _antenna_offset[GPS_MAX_RECEIVERS_BLEND] {};
Vector3f _output_antenna_offset {};
};

197
src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp
View File

@@ -292,6 +292,203 @@ TEST_F(GpsBlendingTest, dualReceiverFailover)
EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
}
TEST_F(GpsBlendingTest, singleReceiverAntennaOffset)
{
GpsBlending gps_blending;
gps_blending.setPrimaryInstance(-1);
sensor_gps_s gps_data = getDefaultGpsData();
const Vector3f offset0(0.1f, 0.0f, -0.05f);
gps_blending.setAntennaOffset(offset0, 1);
gps_blending.setGpsData(gps_data, 1);
gps_blending.update(_time_now_us);
_time_now_us += 200e3;
gps_data.timestamp = _time_now_us - 10e3;
gps_blending.setGpsData(gps_data, 1);
gps_blending.update(_time_now_us);
EXPECT_EQ(gps_blending.getSelectedGps(), 1);
EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
const Vector3f &out = gps_blending.getOutputAntennaOffset();
EXPECT_FLOAT_EQ(out(0), offset0(0));
EXPECT_FLOAT_EQ(out(1), offset0(1));
EXPECT_FLOAT_EQ(out(2), offset0(2));
}
TEST_F(GpsBlendingTest, dualReceiverBlendedAntennaOffset)
{
GpsBlending gps_blending;
sensor_gps_s gps_data0 = getDefaultGpsData();
sensor_gps_s gps_data1 = getDefaultGpsData();
gps_blending.setBlendingUseHPosAccuracy(true);
// Equal accuracy → equal weights (0.5 each)
const Vector3f offset0(0.1f, 0.0f, -0.05f);
const Vector3f offset1(-0.1f, 0.0f, -0.05f);
gps_blending.setAntennaOffset(offset0, 0);
gps_blending.setAntennaOffset(offset1, 1);
gps_blending.setGpsData(gps_data0, 0);
gps_blending.setGpsData(gps_data1, 1);
gps_blending.update(_time_now_us);
EXPECT_EQ(gps_blending.getSelectedGps(), 2); // blended
const Vector3f &out = gps_blending.getOutputAntennaOffset();
// Equal weights → average of offsets
EXPECT_NEAR(out(0), 0.0f, 1e-5f);
EXPECT_NEAR(out(1), 0.0f, 1e-5f);
EXPECT_NEAR(out(2), -0.05f, 1e-5f);
}
TEST_F(GpsBlendingTest, failoverAntennaOffset)
{
GpsBlending gps_blending;
gps_blending.setPrimaryInstance(0);
gps_blending.setBlendingUseSpeedAccuracy(false);
gps_blending.setBlendingUseHPosAccuracy(false);
gps_blending.setBlendingUseVPosAccuracy(false);
const Vector3f offset0(0.1f, 0.0f, 0.0f);
const Vector3f offset1(-0.1f, 0.0f, 0.0f);
gps_blending.setAntennaOffset(offset0, 0);
gps_blending.setAntennaOffset(offset1, 1);
// Only secondary available
sensor_gps_s gps_data1 = getDefaultGpsData();
runSeconds(10.f, gps_blending, gps_data1, 1);
EXPECT_EQ(gps_blending.getSelectedGps(), 1);
EXPECT_FLOAT_EQ(gps_blending.getOutputAntennaOffset()(0), offset1(0));
// Now primary becomes available
sensor_gps_s gps_data0 = getDefaultGpsData();
gps_data0.timestamp = gps_data1.timestamp;
runSeconds(1.f, gps_blending, gps_data0, gps_data1);
EXPECT_EQ(gps_blending.getSelectedGps(), 0);
EXPECT_FLOAT_EQ(gps_blending.getOutputAntennaOffset()(0), offset0(0));
}
TEST_F(GpsBlendingTest, dualReceiverAsymmetricWeightAntennaOffset)
{
GpsBlending gps_blending;
sensor_gps_s gps_data0 = getDefaultGpsData();
sensor_gps_s gps_data1 = getDefaultGpsData();
gps_blending.setBlendingUseHPosAccuracy(true);
// gps0 has twice the accuracy of gps1 → higher weight
gps_data0.eph = 0.5f;
gps_data1.eph = 1.0f;
const Vector3f offset0(0.2f, 0.0f, -0.1f);
const Vector3f offset1(-0.2f, 0.0f, 0.1f);
gps_blending.setAntennaOffset(offset0, 0);
gps_blending.setAntennaOffset(offset1, 1);
gps_blending.setGpsData(gps_data0, 0);
gps_blending.setGpsData(gps_data1, 1);
gps_blending.update(_time_now_us);
EXPECT_EQ(gps_blending.getSelectedGps(), 2); // blended
// hpos weights: inverse variance → w0 = 1/(0.5^2) = 4, w1 = 1/(1.0^2) = 1
// normalized: w0 = 0.8, w1 = 0.2
// blended offset = 0.8 * (0.2, 0, -0.1) + 0.2 * (-0.2, 0, 0.1)
// = (0.16, 0, -0.08) + (-0.04, 0, 0.02) = (0.12, 0, -0.06)
const Vector3f &out = gps_blending.getOutputAntennaOffset();
EXPECT_NEAR(out(0), 0.12f, 1e-5f);
EXPECT_NEAR(out(1), 0.0f, 1e-5f);
EXPECT_NEAR(out(2), -0.06f, 1e-5f);
}
TEST_F(GpsBlendingTest, blendingFallthroughAntennaOffset)
{
GpsBlending gps_blending;
// Enable blending, but give one receiver eph=0 so can_do_blending is false
gps_blending.setPrimaryInstance(-1);
gps_blending.setBlendingUseHPosAccuracy(true);
gps_blending.setBlendingUseSpeedAccuracy(false);
gps_blending.setBlendingUseVPosAccuracy(false);
const Vector3f offset0(0.15f, -0.05f, 0.0f);
const Vector3f offset1(-0.15f, 0.05f, 0.0f);
gps_blending.setAntennaOffset(offset0, 0);
gps_blending.setAntennaOffset(offset1, 1);
sensor_gps_s gps_data0 = getDefaultGpsData();
sensor_gps_s gps_data1 = getDefaultGpsData();
// eph=0 on both → horizontal_accuracy_sum_sq=0 → can_do_blending=false → fallthrough
gps_data0.eph = 0.0f;
gps_data1.eph = 0.0f;
// gps1 has more satellites → wins non-blending selection
gps_data1.satellites_used = gps_data0.satellites_used + 2;
gps_blending.setGpsData(gps_data0, 0);
gps_blending.setGpsData(gps_data1, 1);
gps_blending.update(_time_now_us);
_time_now_us += 200e3;
gps_data0.timestamp = _time_now_us - 10e3;
gps_data1.timestamp = _time_now_us - 10e3;
gps_blending.setGpsData(gps_data0, 0);
gps_blending.setGpsData(gps_data1, 1);
gps_blending.update(_time_now_us);
// Falls through to non-blending path, gps1 selected by satellite count
EXPECT_LT(gps_blending.getSelectedGps(), 2); // not blended
EXPECT_EQ(gps_blending.getSelectedGps(), 1);
const Vector3f &out = gps_blending.getOutputAntennaOffset();
EXPECT_FLOAT_EQ(out(0), offset1(0));
EXPECT_FLOAT_EQ(out(1), offset1(1));
EXPECT_FLOAT_EQ(out(2), offset1(2));
}
TEST_F(GpsBlendingTest, dualReceiverNoBlendingStaleFlag)
{
GpsBlending gps_blending;
// GIVEN: two receivers, no blending (no accuracy metrics enabled)
gps_blending.setPrimaryInstance(-1);
gps_blending.setBlendingUseSpeedAccuracy(false);
gps_blending.setBlendingUseHPosAccuracy(false);
gps_blending.setBlendingUseVPosAccuracy(false);
sensor_gps_s gps_data0 = getDefaultGpsData();
sensor_gps_s gps_data1 = getDefaultGpsData();
gps_data1.satellites_used = gps_data0.satellites_used + 2; // gps1 wins selection
// First update: both receivers provide data, gps1 is selected
gps_blending.setGpsData(gps_data0, 0);
gps_blending.setGpsData(gps_data1, 1);
gps_blending.update(_time_now_us);
EXPECT_EQ(gps_blending.getSelectedGps(), 1);
EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
// Second update: NO new data from either receiver
// With the bug (_gps_updated[gps_select_index] instead of [i]),
// the non-selected instance's flag was never cleared, so this
// would spuriously report new data available.
gps_blending.update(_time_now_us);
EXPECT_FALSE(gps_blending.isNewOutputDataAvailable());
}
TEST_F(GpsBlendingTest, dualReceiverUTCTime)
{
GpsBlending gps_blending;

108
src/modules/sensors/vehicle_gps_position/module.yaml Normal file
View File

@@ -0,0 +1,108 @@
__max_num_gps_instances: &max_num_gps_instances 2
module_name: vehicle_gps_position
parameters:
- group: Sensors
definitions:
SENS_GPS_MASK:
description:
short: Multi GPS Blending Control Mask
long: |
Set bits in the following positions to set which GPS accuracy metrics will
be used to calculate the blending weight. Set to zero to disable and always
used first GPS instance.
type: bitmask
bit:
0: use speed accuracy
1: use hpos accuracy
2: use vpos accuracy
default: 7
min: 0
max: 7
SENS_GPS_TAU:
description:
short: Multi GPS Blending Time Constant
long: |
Sets the longest time constant that will be applied to the calculation of GPS
position and height offsets used to correct data from multiple GPS data for
steady state position differences.
type: float
default: 10.0
unit: s
decimal: 1
min: 1.0
max: 100.0
SENS_GPS_PRIME:
description:
short: Multi GPS primary instance
long: |
When no blending is active, this defines the preferred GPS receiver instance.
The GPS selection logic waits until the primary receiver is available to
send data to the EKF even if a secondary instance is already available.
The secondary instance is then only used if the primary one times out.
To select a DroneCAN GPS, set this to the node ID.
This parameter has no effect if blending is active.
type: int32
default: 0
min: -1
max: 127
values:
-1: Auto (equal priority)
0: Main serial GPS
1: Secondary serial GPS
SENS_GPS${i}_ID:
description:
short: GPS ${i} device ID
long: |
Device ID of the GPS receiver for antenna offset slot ${i}.
Set to 0 to disable this slot. When all slots are 0, offsets are
matched by uORB instance index (only reliable for serial GPS).
category: System
type: int32
default: 0
num_instances: *max_num_gps_instances
instance_start: 0
SENS_GPS${i}_OFFX:
description:
short: GPS ${i} antenna X position
long: |
Forward axis relative to vehicle centre of gravity.
Matched to physical GPS receiver via SENS_GPS${i}_ID.
type: float
default: 0.0
unit: m
decimal: 3
num_instances: *max_num_gps_instances
instance_start: 0
SENS_GPS${i}_OFFY:
description:
short: GPS ${i} antenna Y position
long: |
Right axis relative to vehicle centre of gravity.
Matched to physical GPS receiver via SENS_GPS${i}_ID.
type: float
default: 0.0
unit: m
decimal: 3
num_instances: *max_num_gps_instances
instance_start: 0
SENS_GPS${i}_OFFZ:
description:
short: GPS ${i} antenna Z position
long: |
Down axis relative to vehicle centre of gravity.
Matched to physical GPS receiver via SENS_GPS${i}_ID.
type: float
default: 0.0
unit: m
decimal: 3
num_instances: *max_num_gps_instances
instance_start: 0

85
src/modules/sensors/vehicle_gps_position/params.c
View File

@@ -1,85 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020 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.
*
****************************************************************************/
/**
* Multi GPS Blending Control Mask.
*
* Set bits in the following positions to set which GPS accuracy metrics will be used to calculate the blending weight. Set to zero to disable and always used first GPS instance.
* 0 : Set to true to use speed accuracy
* 1 : Set to true to use horizontal position accuracy
* 2 : Set to true to use vertical position accuracy
*
* @group Sensors
* @min 0
* @max 7
* @bit 0 use speed accuracy
* @bit 1 use hpos accuracy
* @bit 2 use vpos accuracy
*/
PARAM_DEFINE_INT32(SENS_GPS_MASK, 7);
/**
* Multi GPS Blending Time Constant
*
* Sets the longest time constant that will be applied to the calculation of GPS position and height offsets used to correct data from multiple GPS data for steady state position differences.
*
*
* @group Sensors
* @min 1.0
* @max 100.0
* @unit s
* @decimal 1
*/
PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f);
/**
* Multi GPS primary instance
*
* When no blending is active, this defines the preferred GPS receiver instance.
* The GPS selection logic waits until the primary receiver is available to
* send data to the EKF even if a secondary instance is already available.
* The secondary instance is then only used if the primary one times out.
*
* Accepted values:
* -1 : Auto (equal priority for all instances)
* 0 : Main serial GPS instance
* 1 : Secondary serial GPS instance
* 2-127 : UAVCAN module node ID
*
* This parameter has no effect if blending is active.
*
* @group Sensors
* @min -1
* @max 127
*/
PARAM_DEFINE_INT32(SENS_GPS_PRIME, 0);