diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp index eebaa877f4..87f270b970 100644 --- a/libraries/AP_Math/location.cpp +++ b/libraries/AP_Math/location.cpp @@ -26,24 +26,8 @@ float longitude_scale(const struct Location &loc) { -#if HAL_CPU_CLASS < HAL_CPU_CLASS_150 - static int32_t last_lat; - static float scale = 1.0; - // don't optimise on faster CPUs. It causes some minor errors on Replay - if (labs(last_lat - loc.lat) < 100000) { - // we are within 0.01 degrees (about 1km) of the - // same latitude. We can avoid the cos() and return - // the same scale factor. - return scale; - } - scale = cosf(loc.lat * 1.0e-7f * DEG_TO_RAD); - scale = constrain_float(scale, 0.01f, 1.0f); - last_lat = loc.lat; - return scale; -#else float scale = cosf(loc.lat * 1.0e-7f * DEG_TO_RAD); return constrain_float(scale, 0.01f, 1.0f); -#endif }