|
|
|
@ -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; |
|
|
|
|
} |
|
|
|
|