From 0af9433c8e75c2b03e9468ca19c2e0022559bce8 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 19 Apr 2017 15:28:14 +0900
Subject: [PATCH] AP_AHRS: add set_origin

---
 libraries/AP_AHRS/AP_AHRS.h          |  5 +++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 21 +++++++++++++++++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   |  5 +++++
 3 files changed, 31 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 74d4b1a574..ea4e96ff97 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -441,6 +441,11 @@ public:
     // current barometer and GPS altitudes correspond to this altitude
     virtual void set_home(const Location &loc) = 0;
 
+    // set the EKF's origin location in 10e7 degrees.  This should only
+    // be called when the EKF has no absolute position reference (i.e. GPS)
+    // from which to decide the origin on its own
+    virtual bool set_origin(const Location &loc) { return false; }
+
     // return true if the AHRS object supports inertial navigation,
     // with very accurate position and velocity
     virtual bool have_inertial_nav(void) const {
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 1270372c2d..3ba718b1a2 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -559,6 +559,27 @@ void AP_AHRS_NavEKF::set_home(const Location &loc)
     AP_AHRS_DCM::set_home(loc);
 }
 
+// set the EKF's origin location in 10e7 degrees.  This should only
+// be called when the EKF has no absolute position reference (i.e. GPS)
+// from which to decide the origin on its own
+bool AP_AHRS_NavEKF::set_origin(const Location &loc)
+{
+    bool ret2 = EKF2.setOriginLLH(loc);
+    bool ret3 = EKF3.setOriginLLH(loc);
+
+    // return success if active EKF's origin was set
+    switch (active_EKF_type()) {
+    case EKF_TYPE2:
+        return ret2;
+
+    case EKF_TYPE3:
+        return ret3;
+
+    default:
+        return false;
+    }
+}
+
 // return true if inertial navigation is active
 bool AP_AHRS_NavEKF::have_inertial_nav(void) const
 {
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index fb50c922ca..2a6dff960c 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -124,6 +124,11 @@ public:
     // set home location
     void set_home(const Location &loc) override;
 
+    // set the EKF's origin location in 10e7 degrees.  This should only
+    // be called when the EKF has no absolute position reference (i.e. GPS)
+    // from which to decide the origin on its own
+    bool set_origin(const Location &loc) override;
+
     // returns the inertial navigation origin in lat/lon/alt
     bool get_origin(Location &ret) const;