Browse Source

SITL: fixed GPS heading in simulated UBlox

longitude scale does not apply to velocity->heading conversions
master
Andrew Tridgell 13 years ago
parent
commit
27761737ce
  1. 3
      libraries/Desktop/support/sitl_gps.cpp

3
libraries/Desktop/support/sitl_gps.cpp

@ -144,8 +144,7 @@ void sitl_update_gps(double latitude, double longitude, float altitude, @@ -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;
}

Loading…
Cancel
Save