HAL_SITL: implement SIM_GPSDRIFTALT

2015-02-21 08:33:01 +11:00 · 2015-02-21 08:33:01 +11:00 · 869fb23062
parent 63c792bc1c
commit 869fb23062
1 changed files with 6 additions and 0 deletions
--- a/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp
+++ b/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp
@ -759,6 +759,12 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
 	d.latitude = latitude + glitch_offsets.x;
 	d.longitude = longitude + glitch_offsets.y;
 	d.altitude = altitude + glitch_offsets.z;
+
+    if (_sitl->gps_drift_alt > 0) {
+        // slow altitude drift
+        d.altitude += _sitl->gps_drift_alt*sinf(hal.scheduler->millis()*0.001*0.02);
+    }
+    
 	d.speedN = speedN;
 	d.speedE = speedE;
 	d.speedD = speedD;