From 426006d98fb762a895e67f14f2779386db9a19c9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 22 Jun 2012 08:37:48 +1000 Subject: [PATCH] SITL: fixed GPS heading in simulated UBlox longitude scale does not apply to velocity->heading conversions --- libraries/Desktop/support/sitl_gps.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/Desktop/support/sitl_gps.cpp b/libraries/Desktop/support/sitl_gps.cpp index e59d6ec768..8bb72892c8 100644 --- a/libraries/Desktop/support/sitl_gps.cpp +++ b/libraries/Desktop/support/sitl_gps.cpp @@ -144,8 +144,7 @@ void sitl_update_gps(double latitude, double longitude, float altitude, #define sqr(x) ((x)*(x)) velned.speed_2d = sqrt(sqr(speedN)+sqr(speedE)) * 100; velned.speed_3d = velned.speed_2d; - lon_scale = cos(ToRad(latitude)); - velned.heading_2d = ToDeg(atan2(lon_scale*speedE, speedN)) * 100000.0; + velned.heading_2d = ToDeg(atan2(speedE, speedN)) * 100000.0; if (velned.heading_2d < 0.0) { velned.heading_2d += 360.0 * 100000.0; }