Copter: bug fix for loiter target

ArduCopter/navigation.pde

@@ -389,8 +389,9 @@ get_loiter_pos_lat_lon(int32_t target_lat_from_home, int32_t target_lon_from_hom
     int16_t linear_distance;      // the distace we swap between linear and sqrt.
 
     // calculate distance error
-    dist_error_lat = target_lat_from_home - inertial_nav.get_latitude_diff();
-    dist_error_lon = target_lon_from_home - inertial_nav.get_longitude_diff();
+    Vector3f curr = inertial_nav.get_position();
+    dist_error_lat = target_lat_from_home - curr.x;
+    dist_error_lon = target_lon_from_home - curr.y;
 
     linear_distance = MAX_LOITER_POS_ACCEL/(2*g.pi_loiter_lat.kP()*g.pi_loiter_lat.kP());
 

libraries/AP_InertialNav/AP_InertialNav.h

@@ -61,6 +61,9 @@ public:
     // correct_with_gps - modifies accelerometer offsets using gps.  dt is time since last gps update
     void        correct_with_gps(int32_t lon, int32_t lat, float dt);
 
+    // get_position - returns current position from home in cm
+    Vector3f    get_position() { return _position_base + _position_correction; }
+
     // get latitude & longitude positions
     int32_t     get_latitude();
     int32_t     get_longitude();