From 30faf8771517b9427393627bdb182aeb21679ab6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 5 May 2013 14:32:25 +0900 Subject: [PATCH] InertialNav: use AP_Math's longitude_scale --- libraries/AP_InertialNav/AP_InertialNav.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index 074c7958f7..17ca4d693d 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -211,9 +211,11 @@ void AP_InertialNav::set_current_position(int32_t lon, int32_t lat) _base_lon = lon; _base_lat = lat; - // set longitude->meters scaling - // this is used to offset the shrinking longitude as we go towards the poles - _lon_to_m_scaling = cosf((fabsf((float)lat)*1.0e-7f) * DEG_TO_RAD) * LATLON_TO_CM; + // set longitude to meters scaling to offset the shrinking longitude as we go towards the poles + Location temp_loc; + temp_loc.lat = lat; + temp_loc.lng = lon; + _lon_to_m_scaling = longitude_scale(&temp_loc) * LATLON_TO_CM; // reset corrections to base position to zero _position_base.x = 0;