AP_InertialNav: use AHRS home location

libraries/AP_InertialNav/AP_InertialNav.cpp

@@ -168,8 +168,8 @@ void AP_InertialNav::correct_with_gps(uint32_t now, int32_t lon, int32_t lat)
     }
 
     // calculate distance from base location
-    x = (float)(lat - _base_lat) * LATLON_TO_CM;
-    y = (float)(lon - _base_lon) * _lon_to_cm_scaling;
+    x = (float)(lat - _ahrs.get_home().lat) * LATLON_TO_CM;
+    y = (float)(lon - _ahrs.get_home().lng) * _lon_to_cm_scaling;
 
     // sanity check the gps position.  Relies on the main code calling GPS_Glitch::check_position() immediatley after a GPS update
     if (_glitch_detector.glitching()) {
@@ -213,7 +213,7 @@ int32_t AP_InertialNav::get_latitude() const
         return 0;
     }
 
-    return _base_lat + (int32_t)(_position.x/LATLON_TO_CM);
+    return _ahrs.get_home().lat + (int32_t)(_position.x/LATLON_TO_CM);
 }
 
 // get accel based longitude
@@ -224,21 +224,14 @@ int32_t AP_InertialNav::get_longitude() const
         return 0;
     }
 
-    return _base_lon + (int32_t)(_position.y / _lon_to_cm_scaling);
+    return _ahrs.get_home().lng + (int32_t)(_position.y / _lon_to_cm_scaling);
 }
 
-// set_home_position - all internal calculations are recorded as the distances from this point
-void AP_InertialNav::set_home_position(int32_t lon, int32_t lat)
+// setup_home_position - reset state for home position change
+void AP_InertialNav::setup_home_position(void)
 {
-    // set base location
-    _base_lon = lon;
-    _base_lat = lat;
-
     // 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_cm_scaling = longitude_scale(temp_loc) * LATLON_TO_CM;
+    _lon_to_cm_scaling = longitude_scale(_ahrs.get_home()) * LATLON_TO_CM;
 
     // reset corrections to base position to zero
     _position_base.x = 0;

libraries/AP_InertialNav/AP_InertialNav.h

@@ -51,9 +51,6 @@ public:
 
     /**
      * initializes the object.
-     *
-     * AP_InertialNav::set_home_position(int32_t, int32_t) should be called later,
-     * to enable all "horizontal related" getter-methods.
      */
     void        init();
 
@@ -104,8 +101,6 @@ public:
     /**
      * get_position - returns the current position relative to the home location in cm.
      *
-     * the home location was set with AP_InertialNav::set_home_position(int32_t, int32_t)
-     *
      * @return
      */
     const Vector3f&    get_position() const { return _position; }
@@ -122,16 +117,6 @@ public:
      */
     int32_t     get_longitude() const;
 
-    /**
-     * set_home_position - sets home position
-     *
-     * all internal calculations are recorded as the distances from this point.
-     *
-     * @param lon : longitude in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
-     * @param lat : latitude  in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
-     */
-    void        set_home_position(int32_t lon, int32_t lat);
-
     /**
      * get_latitude_diff - returns the current latitude difference from the home location.
      *
@@ -171,6 +156,11 @@ public:
      */
     void        set_velocity_xy(float x, float y);
 
+    /**
+       setup_home_position - reset on home position change
+    */
+    void setup_home_position(void);
+
     //
     // Z Axis methods
     //
@@ -295,8 +285,6 @@ protected:
     uint8_t                 _historic_xy_counter;       // counter used to slow saving of position estimates for later comparison to gps
     AP_BufferFloat_Size5    _hist_position_estimate_x;  // buffer of historic accel based position to account for gpslag
     AP_BufferFloat_Size5    _hist_position_estimate_y;  // buffer of historic accel based position to account for gps lag
-    int32_t                 _base_lat;                  // base latitude  (home location) in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
-    int32_t                 _base_lon;                  // base longitude (home location) in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
     float                   _lon_to_cm_scaling;         // conversion of longitude to centimeters
 
     // Z Axis specific variables