Copter: Reduce dependency on inertial nav

ArduCopter/GCS_Mavlink.cpp

@@ -993,9 +993,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
                 if (!AP::ahrs().home_is_set()) {
                     break;
                 }
-                const Location &origin = copter.inertial_nav.get_origin();
+                Location origin;
                 pos_vector.z += AP::ahrs().get_home().alt;
-                pos_vector.z -= origin.alt;
+                if (copter.ahrs.get_origin(origin)) {
+                    pos_vector.z -= origin.alt;
+                }
             }
         }
 

ArduCopter/commands.cpp

@@ -23,8 +23,8 @@ void Copter::update_home_from_EKF()
 void Copter::set_home_to_current_location_inflight() {
     // get current location from EKF
     Location temp_loc;
-    if (inertial_nav.get_location(temp_loc)) {
+    Location ekf_origin;
-        const struct Location &ekf_origin = inertial_nav.get_origin();
+    if (ahrs.get_location(temp_loc) && ahrs.get_origin(ekf_origin)) {
         temp_loc.alt = ekf_origin.alt;
         if (!set_home(temp_loc, false)) {
             return;
@@ -40,7 +40,7 @@ void Copter::set_home_to_current_location_inflight() {
 bool Copter::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)) {
         if (!set_home(temp_loc, lock)) {
             return false;
         }
@@ -106,8 +106,8 @@ bool Copter::set_home(const Location& loc, bool lock)
 bool Copter::far_from_EKF_origin(const Location& loc)
 {
     // check distance to EKF origin
-    const struct Location &ekf_origin = inertial_nav.get_origin();
+    Location ekf_origin;
-    if (ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_M) {
+    if (ahrs.get_origin(ekf_origin) && (ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_M)) {
         return true;
     }
 

ArduCopter/inertia.cpp

@@ -4,11 +4,13 @@
 void Copter::read_inertia()
 {
     // inertial altitude estimates
-    inertial_nav.update(G_Dt);
+    inertial_nav.update();
 
-    // pull position from interial nav library
+    // pull position from ahrs
-    current_loc.lng = inertial_nav.get_longitude();
+    Location loc;
-    current_loc.lat = inertial_nav.get_latitude();
+    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) {