From 5e381280dcf5a83159ed3c2f336fffbc02f60523 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Fri, 17 Oct 2014 11:39:52 -0700
Subject: [PATCH] AP_InertialNav: update properly if home position moves

---
 libraries/AP_InertialNav/AP_InertialNav.cpp | 43 +++++++++++++++++++++
 libraries/AP_InertialNav/AP_InertialNav.h   |  8 ++++
 2 files changed, 51 insertions(+)

diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp
index 4f99056693..4ebc60e1d3 100644
--- a/libraries/AP_InertialNav/AP_InertialNav.cpp
+++ b/libraries/AP_InertialNav/AP_InertialNav.cpp
@@ -47,6 +47,9 @@ void AP_InertialNav::update(float dt)
     // check if new baro readings have arrived and use them to correct vertical accelerometer offsets.
     check_baro();
 
+    // check if home has moved and update
+    check_home();
+
     // check if new gps readings have arrived and use them to correct position estimates
     check_gps();
 
@@ -127,6 +130,44 @@ bool AP_InertialNav::position_ok() const
     return _xy_enabled;
 }
 
+void AP_InertialNav::check_home() {
+    if (!_xy_enabled) {
+        return;
+    }
+
+    int32_t lat_offset = _ahrs.get_home().lat - _last_home_lat;
+    int32_t lng_offset = _ahrs.get_home().lng - _last_home_lng;
+
+    if (lat_offset != 0) {
+        float x_offset_cm = lat_offset * LATLON_TO_CM;
+
+        _position_base.x -= x_offset_cm;
+        _position.x -= x_offset_cm;
+
+        for (uint8_t i = 0; i < _hist_position_estimate_x.size(); i++) {
+            float &x = _hist_position_estimate_x.peek_mutable(i);
+            x -= x_offset_cm;
+        }
+
+        _lon_to_cm_scaling = longitude_scale(_ahrs.get_home()) * LATLON_TO_CM;
+    }
+
+    if (lng_offset != 0) {
+        float y_offset_cm = lng_offset * _lon_to_cm_scaling;
+
+        _position_base.y -= y_offset_cm;
+        _position.y -= y_offset_cm;
+
+        for (uint8_t i = 0; i < _hist_position_estimate_y.size(); i++) {
+            float &y = _hist_position_estimate_y.peek_mutable(i);
+            y -= y_offset_cm;
+        }
+    }
+
+    _last_home_lat = _ahrs.get_home().lat;
+    _last_home_lng = _ahrs.get_home().lng;
+}
+
 // check_gps - check if new gps readings have arrived and use them to correct position estimates
 void AP_InertialNav::check_gps()
 {
@@ -243,6 +284,8 @@ void AP_InertialNav::setup_home_position(void)
     _position_correction.y = 0.0f;
     _position.x = 0.0f;
     _position.y = 0.0f;
+    _last_home_lat = _ahrs.get_home().lat;
+    _last_home_lng = _ahrs.get_home().lng;
 
     // clear historic estimates
     _hist_position_estimate_x.clear();
diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h
index 9977a01111..3b8fe0d4a6 100644
--- a/libraries/AP_InertialNav/AP_InertialNav.h
+++ b/libraries/AP_InertialNav/AP_InertialNav.h
@@ -213,6 +213,11 @@ protected:
      */
     void        correct_with_gps(uint32_t now, int32_t lon, int32_t lat);
 
+    /**
+     * check_home - checks if the home position has moved and offsets everything so it still lines up
+     */
+    void check_home();
+
     /**
      * check_gps - checks if new gps readings have arrived and calls correct_with_gps to
      * calculate the horizontal position error
@@ -297,6 +302,9 @@ protected:
     Baro_Glitch&            _baro_glitch;               // Baro glitch detector
     uint8_t                 _error_count;               // number of missed GPS updates
 
+    int32_t                 _last_home_lat;
+    int32_t                 _last_home_lng;
+
 };
 
 #if AP_AHRS_NAVEKF_AVAILABLE