From af4d9986971960da927858efd291561e0047d6be Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Wed, 26 Dec 2012 20:49:04 +0900
Subject: [PATCH] AP_InertialNav: correct lat/lon to cm

---
 libraries/AP_InertialNav/AP_InertialNav.cpp | 20 ++++++--------------
 libraries/AP_InertialNav/AP_InertialNav.h   |  2 ++
 2 files changed, 8 insertions(+), 14 deletions(-)

diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp
index e5ba95c16a..ec842a9b9c 100644
--- a/libraries/AP_InertialNav/AP_InertialNav.cpp
+++ b/libraries/AP_InertialNav/AP_InertialNav.cpp
@@ -162,10 +162,8 @@ void AP_InertialNav::correct_with_gps(int32_t lon, int32_t lat, float dt)
     }
 
     // calculate distance from base location
-    //x = (float)(lat - _base_lat) * 1.113195;
-    //y = (float)(lon - _base_lon) * _lon_to_m_scaling * 1.113195;
-    x = (float)(lat - _base_lat);
-    y = (float)(lon - _base_lon) * _lon_to_m_scaling;
+    x = (float)(lat - _base_lat) * AP_INERTIALNAV_LATLON_TO_CM;
+    y = (float)(lon - _base_lon) * _lon_to_m_scaling * AP_INERTIALNAV_LATLON_TO_CM;
 
     // convert accelerometer readings to earth frame
     Matrix3f dcm = _ahrs->get_dcm_matrix();
@@ -209,8 +207,7 @@ int32_t AP_InertialNav::get_latitude()
         return 0;
     }
 
-    //return _base_lat - (int32_t)(_position.x / 1.113195);
-    return _base_lat - (int32_t)(_position_base.x + _position_correction.x);
+    return _base_lat - (int32_t)((_position_base.x + _position_correction.x)/AP_INERTIALNAV_LATLON_TO_CM);
 }
 
 // get accel based longitude
@@ -221,8 +218,7 @@ int32_t AP_InertialNav::get_longitude()
         return 0;
     }
 
-    //return _base_lon - (int32_t)(_position.y / (_lon_to_m_scaling * 1.113195) );
-    return _base_lon - (int32_t)((_position_base.y+_position_correction.y) / _lon_to_m_scaling );
+    return _base_lon - (int32_t)((_position_base.y+_position_correction.y) / (_lon_to_m_scaling*AP_INERTIALNAV_LATLON_TO_CM));
 }
 
 // set_current_position - all internal calculations are recorded as the distances from this point
@@ -258,9 +254,7 @@ float AP_InertialNav::get_latitude_diff()
         return 0;
     }
 
-    //return _base_lat + (int32_t)_position.x;
-    //return -_position.x / 1.113195;
-    return -(_position_base.x+_position_correction.x);
+    return -((_position_base.x+_position_correction.x)/AP_INERTIALNAV_LATLON_TO_CM);
 }
 
 // get accel based longitude
@@ -271,9 +265,7 @@ float AP_InertialNav::get_longitude_diff()
         return 0;
     }
 
-    //return _base_lon - (int32_t)(_position.x / _lon_to_m_scaling);
-    //return -_position.y / (_lon_to_m_scaling * 1.113195);
-    return -(_position_base.y+_position_correction.y) / _lon_to_m_scaling;
+    return -(_position_base.y+_position_correction.y) / (_lon_to_m_scaling*AP_INERTIALNAV_LATLON_TO_CM);
 }
 
 // get velocity in latitude & longitude directions
diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h
index c00016e397..c381af3ccb 100644
--- a/libraries/AP_InertialNav/AP_InertialNav.h
+++ b/libraries/AP_InertialNav/AP_InertialNav.h
@@ -17,6 +17,8 @@
 #define AP_INTERTIALNAV_SAVE_POS_AFTER_ITERATIONS   10
 #define AP_INTERTIALNAV_GPS_LAG_IN_10HZ_INCREMENTS  4       // must not be larger than size of _hist_position_estimate_x and _hist_position_estimate_y
 
+#define AP_INERTIALNAV_LATLON_TO_CM 1.1113175
+
 /*
  * AP_InertialNav is an attempt to use accelerometers to augment other sensors to improve altitud e position hold
  */