mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-21 16:18:29 -04:00
SITL: support interpolated GPS lag
use SIM_GPS_LAG_MS and SIM_GPS2_LAG_MS for the lag in milliseconds
This commit is contained in:
Andrew Tridgell
parent
09cd7558c3
commit
df30d4e723
@ -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; i<delay; i++) {
|
||||
_gps_data[i] = d;
|
||||
}
|
||||
}
|
||||
// get delayed data
|
||||
d.timestamp_ms = now_ms;
|
||||
d = interpolate_data(d, _sitl->gps_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<N-1; i++) {
|
||||
uint32_t dt1 = now_ms - _gps_history[i].timestamp_ms;
|
||||
uint32_t dt2 = now_ms - _gps_history[i+1].timestamp_ms;
|
||||
if (delay_ms >= 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
|
||||
|
||||
@ -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);
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -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),
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user