AP_AHRS: add set_origin

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 {

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
 {

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;