From 952c14ed33db7f20c94b70bea4b7dc0b7e2d9ab5 Mon Sep 17 00:00:00 2001 From: Harshit Kumar Sankhla Date: Mon, 27 Jul 2020 01:15:06 +0530 Subject: [PATCH] AP_HAL_SITL: add GPS Velocity Error through a SITL parameter --- libraries/AP_HAL_SITL/sitl_gps.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index d3195deedf..95e8f65eae 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -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) {