diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 254eacf75e..b0d9362b3e 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -25,7 +25,7 @@ #define DEG_TO_RAD 0.017453292519943295769236907684886f #define RAD_TO_DEG 57.295779513082320876798154814105f -#define RadiansToCentiDegrees(x) ((x) * 5729.578f) +#define RadiansToCentiDegrees(x) ((x) * 5729.5779513082320876798154814105f) // acceleration due to gravity in m/s/s #define GRAVITY_MSS 9.80665f diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp index 0eb0022a3c..236e4ead25 100644 --- a/libraries/AP_Math/location.cpp +++ b/libraries/AP_Math/location.cpp @@ -36,7 +36,7 @@ float longitude_scale(const struct Location *loc) // the same scale factor. return scale; } - scale = cosf((fabsf((float)loc->lat)/1.0e7f) * 0.0174532925f); + scale = cosf((fabsf((float)loc->lat)/1.0e7f) * DEG_TO_RAD); last_lat = loc->lat; return scale; }