AP_InertialNav: add get_origin function

libraries/AP_InertialNav/AP_InertialNav.h

@@ -79,6 +79,13 @@ public:
      */
     virtual nav_filter_status get_filter_status() const;
 
+    /**
+     * get_origin - returns the inertial navigation origin in lat/lon/alt
+     *
+     * @return origin Location
+     */
+    virtual struct Location get_origin() const { return _ahrs.get_home(); }
+
     //
     // XY Axis specific methods
     //

libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp

@@ -51,6 +51,15 @@ nav_filter_status AP_InertialNav_NavEKF::get_filter_status() const
     return AP_InertialNav::get_filter_status();
 }
 
+struct Location AP_InertialNav_NavEKF::get_origin() const {
+    if (_ahrs.have_inertial_nav()) {
+        struct Location ret;
+        _ahrs_ekf.get_NavEKF().getOriginLLH(ret);
+        return ret;
+    }
+    return AP_InertialNav::get_origin();
+}
+
 /**
  * get_position - returns the current position relative to the home location in cm.
  *

libraries/AP_InertialNav/AP_InertialNav_NavEKF.h

@@ -38,6 +38,13 @@ public:
      */
     nav_filter_status get_filter_status() const;
 
+    /**
+     * get_origin - returns the inertial navigation origin in lat/lon/alt
+     *
+     * @return origin Location
+     */
+    struct Location get_origin() const;
+
     /**
      * get_position - returns the current position relative to the home location in cm.
      *