AP_InertialNav: add get_origin function

3drobotics · Feb 3, 2015 · 4d5dfcf · 4d5dfcf
1 parent dd465c5
commit 4d5dfcf
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 0 deletions.
diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h
@@ -79,6 +79,13 @@ class AP_InertialNav
      */
     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
     //

diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp b/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.
  *

diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h
@@ -38,6 +38,13 @@ class AP_InertialNav_NavEKF : public AP_InertialNav
      */
     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.
      *