From df30d4e723a459146f7418bed1b703a7bc7735ec Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 6 Jan 2022 11:32:55 +1100 Subject: [PATCH] SITL: support interpolated GPS lag use SIM_GPS_LAG_MS and SIM_GPS2_LAG_MS for the lag in milliseconds --- libraries/SITL/SIM_GPS.cpp | 71 ++++++++++++++++++++++++++------------ libraries/SITL/SIM_GPS.h | 10 +++--- libraries/SITL/SITL.cpp | 4 +-- libraries/SITL/SITL.h | 2 +- 4 files changed, 57 insertions(+), 30 deletions(-) diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index 05a162ef98..cdad17b098 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/libraries/SITL/SIM_GPS.cpp @@ -413,7 +413,7 @@ void GPS::update_ubx(const struct gps_data *d) radians(_sitl->state.pitchRate), radians(_sitl->state.yawRate)); rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(d->yaw_deg)); - const float lag = (1.0/_sitl->gps_hertz[instance]) * delay; + const float lag = _sitl->gps_delay_ms[instance] * 0.001; rot.rotate(gyro * (-lag)); rel_antenna_pos = rot * rel_antenna_pos; relposned.version = 1; @@ -1043,8 +1043,9 @@ void GPS::update() const double speedN = _sitl->state.speedN; const double speedE = _sitl->state.speedE; const double speedD = _sitl->state.speedD; + const uint32_t now_ms = AP_HAL::millis(); - if (AP_HAL::millis() < 20000) { + if (now_ms < 20000) { // apply the init offsets for the first 20s. This allows for // having the origin a long way from the takeoff location, // which makes testing long flights easier @@ -1055,7 +1056,7 @@ void GPS::update() //Capture current position as basestation location for if (!_gps_has_basestation_position && - AP_HAL::millis() >= _sitl->gps_lock_time[0]*1000UL) { + now_ms >= _sitl->gps_lock_time[0]*1000UL) { _gps_basestation_data.latitude = latitude; _gps_basestation_data.longitude = longitude; _gps_basestation_data.altitude = altitude; @@ -1070,10 +1071,10 @@ void GPS::update() struct gps_data d; // simulate delayed lock times - bool have_lock = (!_sitl->gps_disable[idx] && AP_HAL::millis() >= _sitl->gps_lock_time[idx]*1000UL); + bool have_lock = (!_sitl->gps_disable[idx] && now_ms >= _sitl->gps_lock_time[idx]*1000UL); // run at configured GPS rate (default 5Hz) - if ((AP_HAL::millis() - last_update) < (uint32_t)(1000/_sitl->gps_hertz[idx])) { + if ((now_ms - last_update) < (uint32_t)(1000/_sitl->gps_hertz[idx])) { return; } @@ -1081,7 +1082,7 @@ void GPS::update() char c; read_from_autopilot(&c, 1); - last_update = AP_HAL::millis(); + last_update = now_ms; d.latitude = latitude; d.longitude = longitude; @@ -1090,7 +1091,7 @@ void GPS::update() d.pitch_deg = _sitl->state.pitchDeg; // add an altitude error controlled by a slow sine wave - d.altitude = altitude + _sitl->gps_noise[idx] * sinf(AP_HAL::millis() * 0.0005f) + _sitl->gps_alt_offset[idx]; + d.altitude = altitude + _sitl->gps_noise[idx] * sinf(now_ms * 0.0005f) + _sitl->gps_alt_offset[idx]; // Add offet to c.g. velocity to get velocity at antenna and add simulated error Vector3f velErrorNED = _sitl->gps_vel_err[idx]; @@ -1101,7 +1102,7 @@ void GPS::update() if (_sitl->gps_drift_alt[idx] > 0) { // slow altitude drift - d.altitude += _sitl->gps_drift_alt[idx]*sinf(AP_HAL::millis()*0.001f*0.02f); + d.altitude += _sitl->gps_drift_alt[idx]*sinf(now_ms*0.001f*0.02f); } // correct the latitude, longitude, hiehgt and NED velocity for the offset between @@ -1136,21 +1137,9 @@ void GPS::update() d.speedD += velRelOffsetEF.z; } - // add in some GPS lag - _gps_data[next_index++] = d; - if (next_index >= delay) { - next_index = 0; - } - - d = _gps_data[next_index]; - - if (_sitl->gps_delay[idx] != delay) { - // cope with updates to the delay control - delay = _sitl->gps_delay[idx]; - for (uint8_t i=0; igps_delay_ms[instance]); // Applying GPS glitch // Using first gps glitch @@ -1192,4 +1181,40 @@ void GPS::update() } } +/* + get delayed data by interpolation +*/ +GPS::gps_data GPS::interpolate_data(const gps_data &d, uint32_t delay_ms) +{ + const uint8_t N = ARRAY_SIZE(_gps_history); + const uint32_t now_ms = d.timestamp_ms; + + // add in into history array, shifting old elements + memmove(&_gps_history[1], &_gps_history[0], sizeof(_gps_history[0])*(ARRAY_SIZE(_gps_history)-1)); + _gps_history[0] = d; + + for (uint8_t i=0; i= dt1 && delay_ms <= dt2) { + // we will interpolate this pair of samples. Start with + // the older sample + const gps_data &s1 = _gps_history[i+1]; + const gps_data &s2 = _gps_history[i]; + gps_data d2 = s1; + const float p = (dt2 - delay_ms) / MAX(1,float(dt2 - dt1)); + d2.latitude += p * (s2.latitude - s1.latitude); + d2.longitude += p * (s2.longitude - s1.longitude); + d2.altitude += p * (s2.altitude - s1.altitude); + d2.speedN += p * (s2.speedN - s1.speedN); + d2.speedE += p * (s2.speedE - s1.speedE); + d2.speedD += p * (s2.speedD - s1.speedD); + d2.yaw_deg += p * (s2.yaw_deg - s1.yaw_deg); + return d2; + } + } + // delay is too long, use last sample + return _gps_history[N-1]; +} + #endif // HAL_SIM_GPS_ENABLED diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h index a55a252971..30e599feba 100644 --- a/libraries/SITL/SIM_GPS.h +++ b/libraries/SITL/SIM_GPS.h @@ -70,9 +70,8 @@ private: uint32_t last_update; // milliseconds // for delay simulation: - uint8_t next_index; - uint8_t delay; struct gps_data { + uint32_t timestamp_ms; double latitude; double longitude; float altitude; @@ -84,8 +83,8 @@ private: double pitch_deg; bool have_lock; }; -#define MAX_GPS_DELAY 100 - gps_data _gps_data[MAX_GPS_DELAY]; + // last 20 samples, allowing for up to 20 samples of delay + gps_data _gps_history[20]; #if HAL_SIM_GPS_EXTERNAL_FIFO_ENABLED @@ -114,6 +113,9 @@ private: void nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen); uint32_t CRC32Value(uint32_t icrc); uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc); + + // get delayed data + gps_data interpolate_data(const gps_data &d, uint32_t delay_ms); }; } diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index f6111d156a..a305f0cbd1 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -321,7 +321,7 @@ const AP_Param::GroupInfo SIM::BaroParm::var_info[] = { // GPS SITL parameters const AP_Param::GroupInfo SIM::var_gps[] = { AP_GROUPINFO("GPS_DISABLE", 1, SIM, gps_disable[0], 0), - AP_GROUPINFO("GPS_DELAY", 2, SIM, gps_delay[0], 1), + AP_GROUPINFO("GPS_LAG_MS", 2, SIM, gps_delay_ms[0], 100), AP_GROUPINFO("GPS_TYPE", 3, SIM, gps_type[0], GPS::Type::UBLOX), AP_GROUPINFO("GPS_BYTELOSS", 4, SIM, gps_byteloss[0], 0), AP_GROUPINFO("GPS_NUMSATS", 5, SIM, gps_numsats[0], 10), @@ -337,7 +337,7 @@ const AP_Param::GroupInfo SIM::var_gps[] = { AP_GROUPINFO("GPS_VERR", 15, SIM, gps_vel_err[0], 0), AP_GROUPINFO("GPS2_DISABLE", 30, SIM, gps_disable[1], 1), - AP_GROUPINFO("GPS2_DELAY", 31, SIM, gps_delay[1], 1), + AP_GROUPINFO("GPS2_LAG_MS", 31, SIM, gps_delay_ms[1], 100), AP_GROUPINFO("GPS2_TYPE", 32, SIM, gps_type[1], GPS::Type::UBLOX), AP_GROUPINFO("GPS2_BYTELOS", 33, SIM, gps_byteloss[1], 0), AP_GROUPINFO("GPS2_NUMSATS", 34, SIM, gps_numsats[1], 10), diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index b059756d4a..e7258f2b51 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -187,7 +187,7 @@ public: AP_Int16 gps_lock_time[2]; // delay in seconds before GPS gets lock AP_Int16 gps_alt_offset[2]; // gps alt error AP_Int8 gps_disable[2]; // disable simulated GPS - AP_Int8 gps_delay[2]; // delay in samples + AP_Int16 gps_delay_ms[2]; // delay in milliseconds AP_Int8 gps_type[2]; // see enum SITL::GPS::Type AP_Float gps_byteloss[2];// byte loss as a percent AP_Int8 gps_numsats[2]; // number of visible satellites