Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_SITL: add GPS Velocity Error through a SITL parameter

This commit is contained in:
Harshit Kumar Sankhla 2020-07-27 01:15:06 +05:30 committed by Andrew Tridgell
parent 464c90a03c
commit 952c14ed33
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_HAL_SITL/sitl_gps.cpp
View File

@ -1249,10 +1249,11 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
// 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];
// Add offet to c.g. velocity to get velocity at antenna
d.speedN = speedN;
d.speedE = speedE;
d.speedD = speedD;
// Add offet to c.g. velocity to get velocity at antenna and add simulated error
Vector3f velErrorNED = _sitl->gps_vel_err[idx];
d.speedN = speedN + (velErrorNED.x * rand_float());
d.speedE = speedE + (velErrorNED.y * rand_float());
d.speedD = speedD + (velErrorNED.z * rand_float());
d.have_lock = have_lock;
if (_sitl->gps_drift_alt[idx] > 0) {