gnss: move delay compensation to driver level

This allows to specify a delay per receiver and reduces the amount of
unnecessary parameter duplication in multi-ekf mode

ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator

@@ -172,7 +172,8 @@ else
 	# otherwise start simulator (mavlink) module
 
 	# EKF2 specifics
-	param set-default EKF2_GPS_DELAY 10
+	param set-default GPS_1_DELAY 10
+	param set-default GPS_2_DELAY 10
 	param set-default EKF2_MULTI_IMU 3
 	param set-default SENS_IMU_MODE 0
 

ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu

@@ -47,7 +47,6 @@ 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 EKF2_GPS_V_NOISE 0.5
 
@@ -79,6 +78,7 @@ param set-default EKF2_RNG_POS_Z 0.033
 
 param set-default EKF2_TERR_NOISE 1
 
+param set-default GPS_1_DELAY 100
 
 # Maximum allowed angle velocity in the landed state
 param set-default LNDMC_ROT_MAX 40

src/drivers/gps/gps.cpp

@@ -221,24 +221,11 @@ private:
 
 	px4::atomic<int> _scheduled_reset{(int)GPSRestartType::None};
 
-	/**
-	 * Publish the gps struct
-	 */
-	void 				publish();
+	void publish();
+	uint64_t get_delay_us() const;
 
-	/**
-	 * Publish the satellite info
-	 */
 	void 				publishSatelliteInfo();
-
-	/**
-	 * Publish RTCM corrections
-	 */
 	void 				publishRTCMCorrections(uint8_t *data, size_t len);
-
-	/**
-	 * Publish RTCM corrections
-	 */
 	void 				publishRelativePosition(sensor_gnss_relative_s &gnss_relative);
 
 	/**
@@ -1179,6 +1166,14 @@ void
 GPS::publish()
 {
 	if (_instance == Instance::Main || _is_gps_main_advertised.load()) {
+		if ((_report_gps_pos.timestamp_sample == 0) || (_report_gps_pos.timestamp_sample == _report_gps_pos.timestamp)) {
+			const uint64_t delay_us = get_delay_us();
+
+			if (_report_gps_pos.timestamp > delay_us) {
+				_report_gps_pos.timestamp_sample = _report_gps_pos.timestamp - get_delay_us();
+			}
+		}
+
 		_report_gps_pos.device_id = get_device_id();
 
 		_report_gps_pos.selected_rtcm_instance = _selected_rtcm_instance;
@@ -1211,6 +1206,33 @@ GPS::publish()
 	}
 }
 
+uint64_t
+GPS::get_delay_us() const
+{
+	float delay_ms;
+
+	switch (_instance) {
+	case Instance::Main:
+		param_get(param_find("GPS_1_DELAY"), &delay_ms);
+		break;
+
+	case Instance::Secondary:
+		param_get(param_find("GPS_2_DELAY"), &delay_ms);
+		break;
+
+	default:
+		delay_ms = 0.f;
+		break;
+	}
+
+	if (delay_ms < -FLT_EPSILON) {
+		// If not specified (< 0), use generic delay as this is more realistic than 0
+		delay_ms = 110.f;
+	}
+
+	return static_cast<uint64_t>(delay_ms * 1e3f);
+}
+
 void
 GPS::publishSatelliteInfo()
 {

src/drivers/gps/params.c

@@ -276,3 +276,33 @@ PARAM_DEFINE_INT32(GPS_1_GNSS, 0);
  * @group GPS
  */
 PARAM_DEFINE_INT32(GPS_2_GNSS, 0);
+
+/**
+ * GPS measurement delay relative to IMU measurements
+ *
+ * Set to -1 if unknown
+ *
+ * @min -1
+ * @max 300
+ * @unit ms
+ * @decimal 1
+ *
+ * @reboot_required true
+ * @group GPS
+ */
+PARAM_DEFINE_INT32(GPS_1_DELAY, -1);
+
+/**
+ * GPS measurement delay relative to IMU measurements
+ *
+ * Set to -1 if unknown
+ *
+ * @min -1
+ * @max 300
+ * @unit ms
+ * @decimal 1
+ *
+ * @reboot_required true
+ * @group GPS
+ */
+PARAM_DEFINE_INT32(GPS_2_DELAY, -1);

src/modules/ekf2/EKF/common.h

@@ -320,7 +320,6 @@ struct parameters {
 
 #if defined(CONFIG_EKF2_GNSS)
 	int32_t gnss_ctrl {static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL)};
-	float gps_delay_ms{110.0f};             ///< GPS measurement delay relative to the IMU (mSec)
 
 	Vector3f gps_pos_body{};                ///< xyz position of the GPS antenna in body frame (m)
 

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -178,7 +178,6 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
 	}
 
 	const int64_t time_us = gnss_sample.time_us
-				- static_cast<int64_t>(_params.gps_delay_ms * 1000)
 				- 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)) {

src/modules/ekf2/EKF2.cpp

@@ -79,7 +79,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_noaid_noise(_params->pos_noaid_noise),
 #if defined(CONFIG_EKF2_GNSS)
 	_param_ekf2_gps_ctrl(_params->gnss_ctrl),
-	_param_ekf2_gps_delay(_params->gps_delay_ms),
 	_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)),
@@ -903,14 +902,6 @@ void EKF2::VerifyParams()
 
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-#if defined(CONFIG_EKF2_GNSS)
-
-	if (_param_ekf2_gps_delay.get() > delay_max) {
-		delay_max = _param_ekf2_gps_delay.get();
-	}
-
-#endif // CONFIG_EKF2_GNSS
-
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 
 	if (_param_ekf2_of_delay.get() > delay_max) {
@@ -2429,7 +2420,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 		}
 
 		gnssSample gnss_sample{
-			.time_us = vehicle_gps_position.timestamp,
+			.time_us = vehicle_gps_position.timestamp_sample,
 			.lat = vehicle_gps_position.latitude_deg,
 			.lon = vehicle_gps_position.longitude_deg,
 			.alt = static_cast<float>(vehicle_gps_position.altitude_msl_m),

src/modules/ekf2/EKF2.hpp

@@ -513,7 +513,6 @@ private:
 
 #if defined(CONFIG_EKF2_GNSS)
 		(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl,
-		(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,

src/modules/ekf2/params_gnss.yaml

@@ -17,16 +17,6 @@ parameters:
       default: 7
       min: 0
       max: 15
-    EKF2_GPS_DELAY:
-      description:
-        short: GPS measurement delay relative to IMU measurements
-      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