diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index 76be887e84..aac02b968c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/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 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 e5e5e08343..7fe36ac830 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,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 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 004be4cc8b..ccb2f45b51 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -221,24 +221,11 @@ private: px4::atomic _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(delay_ms * 1e3f); +} + void GPS::publishSatelliteInfo() { diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c index 512a5131e1..7a2f8f87e8 100644 --- a/src/drivers/gps/params.c +++ b/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); diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index cc1d48cf16..de08151e0a 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -320,7 +320,6 @@ struct parameters { #if defined(CONFIG_EKF2_GNSS) int32_t gnss_ctrl {static_cast(GnssCtrl::HPOS) | static_cast(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) diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index f7a6c70dec..dae1feb543 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/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(_params.gps_delay_ms * 1000) - 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/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 38e5660e4f..18256a9815 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/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(vehicle_gps_position.altitude_msl_m), diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 489e0bbd5a..78b793afb2 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -513,7 +513,6 @@ private: #if defined(CONFIG_EKF2_GNSS) (ParamExtInt) _param_ekf2_gps_ctrl, - (ParamExtFloat) _param_ekf2_gps_delay, (ParamExtFloat) _param_ekf2_gps_pos_x, (ParamExtFloat) _param_ekf2_gps_pos_y, diff --git a/src/modules/ekf2/params_gnss.yaml b/src/modules/ekf2/params_gnss.yaml index 55c530a125..d3817dc9b2 100644 --- a/src/modules/ekf2/params_gnss.yaml +++ b/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