fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(sensors/gps): add per-receiver GPS delay parameters (#26660)

Browse Source 
* sensors: add per-receiver GPS delay parameters

Add SENS_GPS{0,1}_DELAY params to vehicle_gps_position, following the
same device-ID matching pattern used for antenna offsets. Each receiver
can now have its own measurement delay relative to the IMU.

The delay is applied to timestamp_sample before blending. When PPS time
correction is active it takes priority over the parameter-based delay.
When a GPS driver already provides its own timestamp_sample the
per-receiver delay is not applied on top of it.

* fix(ekf2): remove EKF2_GPS_DELAY and perform param transation

* fix(param_translation): fix GPS param migration return values

Add missing return for EKF2_GPS_POS_Z and remove incorrect return for
EKF2_GPS_DELAY (1-to-many migration should not return PARAM_MODIFIED).

* fix(sensors,ekf2): rename pps_compensation and clarify delay default

* fix(ekf2): account for SENS_GPS*_DELAY in observation buffer sizing

* fix(docs): migrate EKF2_GPS_DELAY param
This commit is contained in:
Jacob Dahl 2026-03-17 18:19:08 -08:00 committed by GitHub
parent 533b2d677c
commit 4b2e0a6f59
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
22 changed files with 98 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx
View File

@ -34,6 +34,7 @@ param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104
param set-default EKF2_GPS_DELAY 0
param set-default SENS_GPS0_DELAY 0
param set-default SENS_GPS1_DELAY 0
param set SIH_VEHICLE_TYPE 0

3
ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane
View File

@ -44,7 +44,8 @@ param set-default PWM_MAIN_FUNC2 202
param set-default PWM_MAIN_FUNC3 203
param set-default PWM_MAIN_FUNC4 101
param set-default EKF2_GPS_DELAY 0
param set-default SENS_GPS0_DELAY 0
param set-default SENS_GPS1_DELAY 0
# Rate controllers
param set-default FW_RR_P 0.0500

3
ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert
View File

@ -11,7 +11,8 @@
PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert}
param set-default EKF2_GPS_DELAY 0
param set-default SENS_GPS0_DELAY 0
param set-default SENS_GPS1_DELAY 0
param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
param set-default SENS_EN_GPSSIM 1

3
ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol
View File

@ -27,7 +27,8 @@ param set-default SENS_EN_BAROSIM 1
param set-default SENS_EN_MAGSIM 1
param set-default SENS_EN_ARSPDSIM 1
param set-default EKF2_GPS_DELAY 0
param set-default SENS_GPS0_DELAY 0
param set-default SENS_GPS1_DELAY 0
param set-default VT_TYPE 2
param set-default MPC_MAN_Y_MAX 60

3
ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_hex
View File

@ -44,4 +44,5 @@ param set-default PWM_MAIN_FUNC4 104
param set-default PWM_MAIN_FUNC5 105
param set-default PWM_MAIN_FUNC6 106
param set-default EKF2_GPS_DELAY 0
param set-default SENS_GPS0_DELAY 0
param set-default SENS_GPS1_DELAY 0

3
ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim
View File

@ -2,7 +2,8 @@
# shellcheck disable=SC2154
# EKF2 specifics
param set-default EKF2_GPS_DELAY 10
param set-default SENS_GPS0_DELAY 10
param set-default SENS_GPS1_DELAY 10
param set-default EKF2_MULTI_IMU 3
param set-default SENS_IMU_MODE 0

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

@ -47,7 +47,8 @@ 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 SENS_GPS0_DELAY 100
param set-default SENS_GPS1_DELAY 100
param set-default SENS_GPS0_OFFX 0.06
param set-default EKF2_GPS_V_NOISE 0.5

2
docs/en/advanced/pps_time_sync.md
View File

@ -129,5 +129,5 @@ See also:
The PPS signal provides much higher temporal precision than the transmitted time data, which has latency and jitter from serial communication.
::: warning
If the PPS driver does not sending any data for 5 seconds (despite having `PPS_CAP_ENABLE` set to 1), the `EKF2_GPS_DELAY` will be used instead for estimating the latency.
If the PPS driver does not sending any data for 5 seconds (despite having `PPS_CAP_ENABLE` set to 1), the `SENS_GPS0_DELAY` will be used instead for estimating the latency.
:::

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

@ -296,7 +296,7 @@ For all variants of SIH:
For SIH on SITL you will need to explicitly enable these sensors as shown below.
:::
- `param set-default EKF2_GPS_DELAY 0` to improve state estimator performance (the assumption of instant GPS measurements would normally be unrealistic, but is accurate for SIH).
- `param set-default SENS_GPS0_DELAY 0` to improve state estimator performance (the assumption of instant GPS measurements would normally be unrealistic, but is accurate for SIH).
For SIH on FC:

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

@ -199,6 +199,16 @@ param_modify_on_import_ret param_modify_on_import(bson_node_t node)
}
}
// 2026-03-11: translate EKF2_GPS_DELAY to SENS_GPS0_DELAY and SENS_GPS1_DELAY
{
if (strcmp("EKF2_GPS_DELAY", node->name) == 0) {
int32_t delay_ms = static_cast<int32_t>(node->d);
param_set(param_find("SENS_GPS0_DELAY"), &delay_ms);
param_set(param_find("SENS_GPS1_DELAY"), &delay_ms);
PX4_INFO("migrating %s -> %s, %s", "EKF2_GPS_DELAY", "SENS_GPS0_DELAY", "SENS_GPS1_DELAY");
}
}
// 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/common.h
View File

@ -331,8 +331,6 @@ struct parameters {
#if defined(CONFIG_EKF2_GNSS)
int32_t ekf2_gps_ctrl {static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL)};
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)
// 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)

5
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -159,7 +159,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_GNSS)
void EstimatorInterface::setGpsData(const gnssSample &gnss_sample, const bool pps_compensation)
void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
{
if (!_initialised) {
return;
@ -177,10 +177,7 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample, const bool pp
}
}
const int64_t delay = pps_compensation ? 0 : static_cast<int64_t>(_params.ekf2_gps_delay * 1000);
const int64_t time_us = gnss_sample.time_us
- delay
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
if (time_us >= static_cast<int64_t>(_gps_buffer->get_newest().time_us + _min_obs_interval_us)) {

2
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -89,7 +89,7 @@ public:
void setIMUData(const imuSample &imu_sample);
#if defined(CONFIG_EKF2_GNSS)
void setGpsData(const gnssSample &gnss_sample, const bool pps_compensation = false);
void setGpsData(const gnssSample &gnss_sample);
const gnssSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }

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

@ -80,7 +80,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
#if defined(CONFIG_EKF2_GNSS)
_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_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),
@ -928,11 +927,17 @@ void EKF2::VerifyParams()
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_GNSS)
{
int32_t gps_delay_ms = 0;
if (_param_ekf2_gps_delay.get() > delay_max) {
delay_max = _param_ekf2_gps_delay.get();
if (param_get(param_find("SENS_GPS0_DELAY"), &gps_delay_ms) == PX4_OK) {
delay_max = math::max(delay_max, static_cast<float>(gps_delay_ms));
}
if (param_get(param_find("SENS_GPS1_DELAY"), &gps_delay_ms) == PX4_OK) {
delay_max = math::max(delay_max, static_cast<float>(gps_delay_ms));
}
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
@ -2433,12 +2438,12 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
const float altitude_amsl = static_cast<float>(vehicle_gps_position.altitude_msl_m);
const float altitude_ellipsoid = static_cast<float>(vehicle_gps_position.altitude_ellipsoid_m);
// if pps_compensation is active but not valid, the timestamp_sample will be equal to timestamp
const bool pps_compensation = vehicle_gps_position.timestamp_sample > 0
&& vehicle_gps_position.timestamp_sample != vehicle_gps_position.timestamp;
// timestamp_sample is corrected by the sensors module (per-receiver delay or PPS)
const bool timestamp_corrected = vehicle_gps_position.timestamp_sample > 0
&& vehicle_gps_position.timestamp_sample != vehicle_gps_position.timestamp;
gnssSample gnss_sample{
.time_us = pps_compensation ? vehicle_gps_position.timestamp_sample : vehicle_gps_position.timestamp,
.time_us = timestamp_corrected ? vehicle_gps_position.timestamp_sample : vehicle_gps_position.timestamp,
.lat = vehicle_gps_position.latitude_deg,
.lon = vehicle_gps_position.longitude_deg,
.alt = altitude_amsl,
@ -2460,7 +2465,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
vehicle_gps_position.antenna_offset_z),
};
_ekf.setGpsData(gnss_sample, pps_compensation);
_ekf.setGpsData(gnss_sample);
const float geoid_height = altitude_ellipsoid - altitude_amsl;

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

@ -514,8 +514,6 @@ private:
#if defined(CONFIG_EKF2_GNSS)
(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl,
(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_V_NOISE>) _param_ekf2_gps_v_noise,
(ParamExtFloat<px4::params::EKF2_GPS_P_NOISE>) _param_ekf2_gps_p_noise,

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

@ -27,18 +27,6 @@ parameters:
0: Automatic
1: Dead-reckoning
default: 0
EKF2_GPS_DELAY:
description:
short: GPS measurement delay relative to IMU measurement
long: GPS measurement delay relative to IMU measurement if PPS time correction
is not available/enabled (PPS_CAP_ENABLE).
type: float
default: 110
min: 0
max: 300
unit: ms
reboot_required: true
decimal: 1
EKF2_GPS_P_NOISE:
description:
short: Measurement noise for GNSS position

4
src/modules/ekf2/test/sensor_simulator/gps.cpp
View File

@ -15,9 +15,9 @@ Gps::~Gps()
void Gps::send(const uint64_t time)
{
const float dt = static_cast<float>(time - _gps_data.time_us) * 1e-6f;
const float dt = static_cast<float>(time - _gps_data.time_us - kGpsDelayUs) * 1e-6f;
_gps_data.time_us = time;
_gps_data.time_us = time - kGpsDelayUs;
if (fabsf(_gps_pos_rate(0)) > FLT_EPSILON || fabsf(_gps_pos_rate(1)) > FLT_EPSILON) {
stepHorizontalPositionByMeters(Vector2f(_gps_pos_rate) * dt);

2
src/modules/ekf2/test/sensor_simulator/gps.h
View File

@ -71,6 +71,8 @@ public:
private:
void send(uint64_t time) override;
static constexpr uint64_t kGpsDelayUs{110000};
gnssSample _gps_data{};
Vector3f _gps_pos_rate{};
};

2
src/modules/sensors/vehicle_gps_position/PpsTimeSync.hpp
View File

@ -60,7 +60,7 @@ private:
int64_t _time_offset{0};
static constexpr uint64_t kPpsStaleTimeoutUs = 5'000'000;
static constexpr int64_t kPpsMaxCorrectionUs = 300'000; // max delay (max of EKF2_GPS_DELAY)
static constexpr int64_t kPpsMaxCorrectionUs = 300'000; // max delay (max of SENS_GPS*_DELAY)
bool _initialized{false};
bool _updated{false};

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

@ -103,13 +103,15 @@ void VehicleGPSPosition::ParametersUpdate(bool force)
_gps_blending.setPrimaryInstance(gps_prime);
}
_gps_offset_slots[0] = {
_gps_param_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()}
{_param_sens_gps0_offx.get(), _param_sens_gps0_offy.get(), _param_sens_gps0_offz.get()},
static_cast<hrt_abstime>(_param_sens_gps0_delay.get()) * 1000
};
_gps_offset_slots[1] = {
_gps_param_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()}
{_param_sens_gps1_offx.get(), _param_sens_gps1_offy.get(), _param_sens_gps1_offz.get()},
static_cast<hrt_abstime>(_param_sens_gps1_delay.get()) * 1000
};
}
}
@ -140,22 +142,32 @@ void VehicleGPSPosition::Run()
_sensor_gps_sub[i].copy(&gps_data);
// Match device_id to antenna offset slot
// Match device_id to receiver slot
matrix::Vector3f antenna_offset{};
hrt_abstime delay_us = 110_ms; // matches SENS_GPS*_DELAY default
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;
if (_gps_param_slots[slot].device_id != 0
&& _gps_param_slots[slot].device_id == gps_data.device_id) {
antenna_offset = _gps_param_slots[slot].offset;
delay_us = _gps_param_slots[slot].delay_us;
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;
if (!matched && _gps_param_slots[0].device_id == 0 && _gps_param_slots[1].device_id == 0) {
antenna_offset = _gps_param_slots[i].offset;
delay_us = _gps_param_slots[i].delay_us;
}
// Apply delay to timestamp_sample if the driver didn't set one
if (gps_data.timestamp_sample == 0 || gps_data.timestamp_sample == gps_data.timestamp) {
if (delay_us > 0 && gps_data.timestamp > delay_us) {
gps_data.timestamp_sample = gps_data.timestamp - delay_us;
}
}
_gps_blending.setAntennaOffset(antenna_offset, i);
@ -193,7 +205,13 @@ void VehicleGPSPosition::Run()
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);
const uint64_t pps_timestamp = _pps_time_sync.correct_gps_timestamp(gps_output.timestamp, gps_output.time_utc_usec);
if (pps_timestamp != gps_output.timestamp) {
// PPS provided a correction — use it instead of the per-receiver delay
gps_output.timestamp_sample = pps_timestamp;
}
_vehicle_gps_position_pub.publish(gps_output);
}
}

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

@ -97,10 +97,11 @@ private:
GpsBlending _gps_blending;
PpsTimeSync _pps_time_sync;
struct GpsOffsetSlot {
struct GpsParamSlot {
uint32_t device_id{0};
matrix::Vector3f offset{};
} _gps_offset_slots[GPS_MAX_RECEIVERS] {};
hrt_abstime delay_us{110_ms};
} _gps_param_slots[GPS_MAX_RECEIVERS] {};
DEFINE_PARAMETERS(
(ParamInt<px4::params::SENS_GPS_MASK>) _param_sens_gps_mask,
@ -113,7 +114,9 @@ private:
(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
(ParamFloat<px4::params::SENS_GPS1_OFFZ>) _param_sens_gps1_offz,
(ParamInt<px4::params::SENS_GPS0_DELAY>) _param_sens_gps0_delay,
(ParamInt<px4::params::SENS_GPS1_DELAY>) _param_sens_gps1_delay
)
};
}; // namespace sensors

16
src/modules/sensors/vehicle_gps_position/module.yaml
View File

@ -106,3 +106,19 @@ parameters:
decimal: 3
num_instances: *max_num_gps_instances
instance_start: 0
SENS_GPS${i}_DELAY:
description:
short: GPS ${i} measurement delay
long: |
GPS measurement delay relative to IMU measurements.
Matched to physical GPS receiver via SENS_GPS${i}_ID.
Only applied when the GPS driver does not provide its own
timestamp_sample correction.
type: int32
default: 110
unit: ms
min: 0
max: 300
num_instances: *max_num_gps_instances
instance_start: 0