From bbcb976e73822a05a24a7f420893fee185b1be8b Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Fri, 28 Jun 2019 13:55:49 +0000
Subject: [PATCH] Sub: Reduce dependency on inertial nav

---
 ArduSub/commands.cpp        | 10 +++++-----
 ArduSub/inertia.cpp         | 10 ++++++----
 ArduSub/position_vector.cpp | 10 ++++++++--
 3 files changed, 19 insertions(+), 11 deletions(-)

diff --git a/ArduSub/commands.cpp b/ArduSub/commands.cpp
index b5299fb17e..3ac5383eaa 100644
--- a/ArduSub/commands.cpp
+++ b/ArduSub/commands.cpp
@@ -24,8 +24,8 @@ void Sub::set_home_to_current_location_inflight()
 {
     // get current location from EKF
     Location temp_loc;
-    if (inertial_nav.get_location(temp_loc)) {
-        const struct Location &ekf_origin = inertial_nav.get_origin();
+    Location ekf_origin;
+    if (ahrs.get_location(temp_loc) && ahrs.get_origin(ekf_origin)) {
         temp_loc.alt = ekf_origin.alt;
         if (!set_home(temp_loc, false)) {
             // ignore this failure
@@ -38,7 +38,7 @@ bool Sub::set_home_to_current_location(bool lock)
 {
     // get current location from EKF
     Location temp_loc;
-    if (inertial_nav.get_location(temp_loc)) {
+    if (ahrs.get_location(temp_loc)) {
 
         // Make home always at the water's surface.
         // This allows disarming and arming again at depth.
@@ -98,6 +98,6 @@ bool Sub::set_home(const Location& loc, bool lock)
 bool Sub::far_from_EKF_origin(const Location& loc)
 {
     // check distance to EKF origin
-    const struct Location &ekf_origin = inertial_nav.get_origin();
-    return (ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_M);
+    Location ekf_origin;
+    return ahrs.get_origin(ekf_origin) && (ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_M);
 }
diff --git a/ArduSub/inertia.cpp b/ArduSub/inertia.cpp
index 85ced29853..b6182c6512 100644
--- a/ArduSub/inertia.cpp
+++ b/ArduSub/inertia.cpp
@@ -4,11 +4,13 @@
 void Sub::read_inertia()
 {
     // inertial altitude estimates
-    inertial_nav.update(G_Dt);
+    inertial_nav.update();
 
-    // pull position from interial nav library
-    current_loc.lng = inertial_nav.get_longitude();
-    current_loc.lat = inertial_nav.get_latitude();
+    // pull position from ahrs
+    Location loc;
+    ahrs.get_position(loc);
+    current_loc.lat = loc.lat;
+    current_loc.lng = loc.lng;
 
     // exit immediately if we do not have an altitude estimate
     if (!inertial_nav.get_filter_status().flags.vert_pos) {
diff --git a/ArduSub/position_vector.cpp b/ArduSub/position_vector.cpp
index 6d4a3bb2c4..ddf8df3abf 100644
--- a/ArduSub/position_vector.cpp
+++ b/ArduSub/position_vector.cpp
@@ -10,7 +10,10 @@
 // pv_location_to_vector - convert lat/lon coordinates to a position vector
 Vector3f Sub::pv_location_to_vector(const Location& loc)
 {
-    const struct Location &origin = inertial_nav.get_origin();
+    Location origin;
+    if (!ahrs.get_origin(origin)) {
+        origin.zero();
+    }
     float alt_above_origin = pv_alt_above_origin(loc.alt);  // convert alt-relative-to-home to alt-relative-to-origin
     return Vector3f((loc.lat-origin.lat) * LATLON_TO_CM, (loc.lng-origin.lng) * LATLON_TO_CM * scaleLongDown, alt_above_origin);
 }
@@ -18,7 +21,10 @@ Vector3f Sub::pv_location_to_vector(const Location& loc)
 // pv_alt_above_origin - convert altitude above home to altitude above EKF origin
 float Sub::pv_alt_above_origin(float alt_above_home_cm)
 {
-    const struct Location &origin = inertial_nav.get_origin();
+    Location origin;
+    if (!ahrs.get_origin(origin)) {
+        origin.zero();
+    }
     return alt_above_home_cm + (ahrs.get_home().alt - origin.alt);
 }