diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index 242e6944fda..b045ae2ec7c 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/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; diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h index 1d2f5f2de44..22e1614af8a 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.h +++ b/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