diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx index 623ee41f1f..d043d9b82d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane index 100af65943..512818edf8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert index ee5ea9401c..04551ecf6d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol index 4abc9de264..7776d8f533 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_hex b/ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_hex index 36ae9a26d7..1c510a7900 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_hex +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_hex @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim index f2d571062c..195952eaf8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu index 66e0968557..9fcadae64d 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu +++ b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu @@ -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 diff --git a/docs/en/advanced/pps_time_sync.md b/docs/en/advanced/pps_time_sync.md index fcd4291f70..b48ee5713c 100644 --- a/docs/en/advanced/pps_time_sync.md +++ b/docs/en/advanced/pps_time_sync.md @@ -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. ::: diff --git a/docs/en/sim_sih/index.md b/docs/en/sim_sih/index.md index 8b065bfea3..f16477e654 100644 --- a/docs/en/sim_sih/index.md +++ b/docs/en/sim_sih/index.md @@ -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: diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index d01e1f3189..9e8bdbb974 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -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(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)) { diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index cd76bcb748..972da2eaf8 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -331,8 +331,6 @@ struct parameters { #if defined(CONFIG_EKF2_GNSS) int32_t ekf2_gps_ctrl {static_cast(GnssCtrl::HPOS) | static_cast(GnssCtrl::VEL)}; int32_t ekf2_gps_mode {static_cast(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) diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index b6f71444e9..c468697b8a 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -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(_params.ekf2_gps_delay * 1000); - const int64_t time_us = gnss_sample.time_us - - delay - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 if (time_us >= static_cast(_gps_buffer->get_newest().time_us + _min_obs_interval_us)) { diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 73b3ce2817..c95b5c56e6 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -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; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 023d2c7509..90c2d29487 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -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(gps_delay_ms)); + } + + if (param_get(param_find("SENS_GPS1_DELAY"), &gps_delay_ms) == PX4_OK) { + delay_max = math::max(delay_max, static_cast(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(vehicle_gps_position.altitude_msl_m); const float altitude_ellipsoid = static_cast(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; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 3c21ec6c9a..1e4e9f7b2b 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -514,8 +514,6 @@ private: #if defined(CONFIG_EKF2_GNSS) (ParamExtInt) _param_ekf2_gps_ctrl, (ParamExtInt) _param_ekf2_gps_mode, - (ParamExtFloat) _param_ekf2_gps_delay, - (ParamExtFloat) _param_ekf2_gps_v_noise, (ParamExtFloat) _param_ekf2_gps_p_noise, diff --git a/src/modules/ekf2/params_gnss.yaml b/src/modules/ekf2/params_gnss.yaml index 8639804129..9fd2a02d5f 100644 --- a/src/modules/ekf2/params_gnss.yaml +++ b/src/modules/ekf2/params_gnss.yaml @@ -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 diff --git a/src/modules/ekf2/test/sensor_simulator/gps.cpp b/src/modules/ekf2/test/sensor_simulator/gps.cpp index 095edc4844..d75da46ba2 100644 --- a/src/modules/ekf2/test/sensor_simulator/gps.cpp +++ b/src/modules/ekf2/test/sensor_simulator/gps.cpp @@ -15,9 +15,9 @@ Gps::~Gps() void Gps::send(const uint64_t time) { - const float dt = static_cast(time - _gps_data.time_us) * 1e-6f; + const float dt = static_cast(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); diff --git a/src/modules/ekf2/test/sensor_simulator/gps.h b/src/modules/ekf2/test/sensor_simulator/gps.h index e8ff206916..1c144f391b 100644 --- a/src/modules/ekf2/test/sensor_simulator/gps.h +++ b/src/modules/ekf2/test/sensor_simulator/gps.h @@ -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{}; }; diff --git a/src/modules/sensors/vehicle_gps_position/PpsTimeSync.hpp b/src/modules/sensors/vehicle_gps_position/PpsTimeSync.hpp index 433d13154b..bb133132bb 100644 --- a/src/modules/sensors/vehicle_gps_position/PpsTimeSync.hpp +++ b/src/modules/sensors/vehicle_gps_position/PpsTimeSync.hpp @@ -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}; diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index 4b14a6b980..3064519847 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -103,13 +103,15 @@ void VehicleGPSPosition::ParametersUpdate(bool force) _gps_blending.setPrimaryInstance(gps_prime); } - _gps_offset_slots[0] = { + _gps_param_slots[0] = { static_cast(_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(_param_sens_gps0_delay.get()) * 1000 }; - _gps_offset_slots[1] = { + _gps_param_slots[1] = { static_cast(_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(_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); } } diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp index e2be8bf6e5..bf3cc6635f 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp @@ -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) _param_sens_gps_mask, @@ -113,7 +114,9 @@ private: (ParamInt) _param_sens_gps1_id, (ParamFloat) _param_sens_gps1_offx, (ParamFloat) _param_sens_gps1_offy, - (ParamFloat) _param_sens_gps1_offz + (ParamFloat) _param_sens_gps1_offz, + (ParamInt) _param_sens_gps0_delay, + (ParamInt) _param_sens_gps1_delay ) }; }; // namespace sensors diff --git a/src/modules/sensors/vehicle_gps_position/module.yaml b/src/modules/sensors/vehicle_gps_position/module.yaml index ab53f16c3b..ffd9faf132 100644 --- a/src/modules/sensors/vehicle_gps_position/module.yaml +++ b/src/modules/sensors/vehicle_gps_position/module.yaml @@ -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