AHRS: added common origin between backends

Tools/autotest/arducopter.py

@@ -11889,6 +11889,58 @@ def ScriptingAHRSSource(self):
         self.set_rc(10, 2000)
         self.wait_statustext('Using EKF Source Set 3', check_context=True)
 
+    def CommonOrigin(self):
+        """Test common origin between EKF2 and EKF3"""
+        self.context_push()
+
+        # start on EKF2
+        self.set_parameters({
+            'AHRS_EKF_TYPE': 2,
+            'EK2_ENABLE': 1,
+            'EK3_CHECK_SCALE': 1, # make EK3 slow to get origin
+        })
+        self.reboot_sitl()
+
+        self.context_collect('STATUSTEXT')
+
+        self.wait_statustext("EKF2 IMU0 origin set", timeout=60, check_context=True)
+        self.wait_statustext("EKF2 IMU0 is using GPS", timeout=60, check_context=True)
+        self.wait_statustext("EKF2 active", timeout=60, check_context=True)
+
+        self.context_collect('GPS_GLOBAL_ORIGIN')
+
+        # get EKF2 origin
+        self.run_cmd(mavutil.mavlink.MAV_CMD_GET_HOME_POSITION)
+        ek2_origin = self.assert_receive_message('GPS_GLOBAL_ORIGIN', check_context=True)
+
+        # switch to EKF3
+        self.set_parameters({
+            'SIM_GPS_GLITCH_X' : 0.001, # about 100m
+            'EK3_CHECK_SCALE' : 100,
+            'AHRS_EKF_TYPE'   : 3})
+
+        self.wait_statustext("EKF3 IMU0 is using GPS", timeout=60, check_context=True)
+        self.wait_statustext("EKF3 active", timeout=60, check_context=True)
+
+        self.run_cmd(mavutil.mavlink.MAV_CMD_GET_HOME_POSITION)
+        ek3_origin = self.assert_receive_message('GPS_GLOBAL_ORIGIN', check_context=True)
+
+        self.progress("Checking origins")
+        if ek2_origin.time_usec == ek3_origin.time_usec:
+            raise NotAchievedException("Did not get a new GPS_GLOBAL_ORIGIN message")
+
+        print(ek2_origin, ek3_origin)
+
+        if (ek2_origin.latitude != ek3_origin.latitude or
+                ek2_origin.longitude != ek3_origin.longitude or
+                ek2_origin.altitude != ek3_origin.altitude):
+            raise NotAchievedException("Did not get matching EK2 and EK3 origins")
+
+        self.context_pop()
+
+        # restart GPS driver
+        self.reboot_sitl()
+
     def tests2b(self):  # this block currently around 9.5mins here
         '''return list of all tests'''
         ret = ([
@@ -11992,6 +12044,7 @@ def tests2b(self):  # this block currently around 9.5mins here
             self.BatteryInternalUseOnly,
             self.MAV_CMD_MISSION_START_p1_p2,
             self.ScriptingAHRSSource,
+            self.CommonOrigin,
         ])
         return ret
 

libraries/AP_AHRS/AP_AHRS.cpp

@@ -617,6 +617,22 @@ void AP_AHRS::update_EKF2(void)
             EKF2.getFilterStatus(filt_state);
             update_notify_from_filter_status(filt_state);
         }
+
+        /*
+          if we now have an origin then set in all backends
+        */
+        if (!done_common_origin) {
+            Location new_origin;
+            if (EKF2.getOriginLLH(new_origin)) {
+                done_common_origin = true;
+#if HAL_NAVEKF3_AVAILABLE
+                EKF3.setOriginLLH(new_origin);
+#endif
+#if AP_AHRS_EXTERNAL_ENABLED
+                external.set_origin(new_origin);
+#endif
+            }
+        }
     }
 }
 #endif
@@ -686,6 +702,21 @@ void AP_AHRS::update_EKF3(void)
             EKF3.getFilterStatus(filt_state);
             update_notify_from_filter_status(filt_state);
         }
+        /*
+          if we now have an origin then set in all backends
+        */
+        if (!done_common_origin) {
+            Location new_origin;
+            if (EKF3.getOriginLLH(new_origin)) {
+                done_common_origin = true;
+#if HAL_NAVEKF2_AVAILABLE
+                EKF2.setOriginLLH(new_origin);
+#endif
+#if AP_AHRS_EXTERNAL_ENABLED
+                external.set_origin(new_origin);
+#endif
+            }
+        }
     }
 }
 #endif
@@ -699,6 +730,22 @@ void AP_AHRS::update_external(void)
     if (_active_EKF_type() == EKFType::EXTERNAL) {
         copy_estimates_from_backend_estimates(external_estimates);
     }
+
+    /*
+      if we now have an origin then set in all backends
+    */
+    if (!done_common_origin) {
+        Location new_origin;
+        if (external.get_origin(new_origin)) {
+            done_common_origin = true;
+#if HAL_NAVEKF2_AVAILABLE
+            EKF2.setOriginLLH(new_origin);
+#endif
+#if HAL_NAVEKF3_AVAILABLE
+            EKF3.setOriginLLH(new_origin);
+#endif
+        }
+    }
 }
 #endif // AP_AHRS_EXTERNAL_ENABLED
 
@@ -1412,6 +1459,9 @@ bool AP_AHRS::set_origin(const Location &loc)
 #if HAL_NAVEKF3_AVAILABLE
     const bool ret3 = EKF3.setOriginLLH(loc);
 #endif
+#if AP_AHRS_EXTERNAL_ENABLED
+    const bool ret_ext = external.set_origin(loc);
+#endif
 
     // return success if active EKF's origin was set
     bool success = false;
@@ -1441,7 +1491,7 @@ bool AP_AHRS::set_origin(const Location &loc)
 #endif
 #if AP_AHRS_EXTERNAL_ENABLED
     case EKFType::EXTERNAL:
-        // don't allow origin set with external AHRS
+        success = ret_ext;
         break;
 #endif
     }

libraries/AP_AHRS/AP_AHRS.h

@@ -1024,6 +1024,9 @@ class AP_AHRS {
     bool option_set(Options option) const {
         return (_options & uint16_t(option)) != 0;
     }
+
+    // true when we have completed the common origin setup
+    bool done_common_origin;
 };
 
 namespace AP {

libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp

@@ -180,6 +180,17 @@ bool AP_ExternalAHRS::get_origin(Location &loc)
     return false;
 }
 
+bool AP_ExternalAHRS::set_origin(const Location &loc)
+{
+    WITH_SEMAPHORE(state.sem);
+    if (state.have_origin) {
+        return false;
+    }
+    state.origin = loc;
+    state.have_origin = true;
+    return true;
+}
+
 bool AP_ExternalAHRS::get_location(Location &loc)
 {
     if (!state.have_location) {
@@ -312,19 +323,7 @@ void AP_ExternalAHRS::update(void)
         backend->update();
     }
 
-    /*
-      if backend has not supplied an origin and AHRS has an origin
-      then use that origin so we get a common origin for minimum
-      disturbance when switching backends
-     */
     WITH_SEMAPHORE(state.sem);
-    if (!state.have_origin) {
-        Location origin;
-        if (AP::ahrs().get_origin(origin)) {
-            state.origin = origin;
-            state.have_origin = true;
-        }
-    }
 #if HAL_LOGGING_ENABLED
     const uint32_t now_ms = AP_HAL::millis();
     if (log_rate.get() > 0 && now_ms - last_log_ms >= uint32_t(1000U/log_rate.get())) {

libraries/AP_ExternalAHRS/AP_ExternalAHRS.h

@@ -109,6 +109,7 @@ class AP_ExternalAHRS {
     bool initialised(void) const;
     bool get_quaternion(Quaternion &quat);
     bool get_origin(Location &loc);
+    bool set_origin(const Location &loc);
     bool get_location(Location &loc);
     Vector2f get_groundspeed_vector();
     bool get_velocity_NED(Vector3f &vel);

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -1099,6 +1099,10 @@ bool NavEKF2::getOriginLLH(Location &loc) const
     if (!core) {
         return false;
     }
+    if (common_origin_valid) {
+        loc = common_EKF_origin;
+        return true;
+    }
     return core[primary].getOriginLLH(loc);
 }
 
@@ -1113,11 +1117,9 @@ bool NavEKF2::setOriginLLH(const Location &loc)
     if (!core) {
         return false;
     }
-    if (_fusionModeGPS != 3 || common_origin_valid) {
-        // we don't allow setting of the EKF origin if using GPS
-        // or if the EKF origin has already been set.
-        // This is to prevent accidental setting of EKF origin with an
-        // invalid position or height or causing upsets from a shifting origin.
+    if (common_origin_valid) {
+        // we don't allow setting of the EKF origin if the EKF origin
+        // has already been set.
         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 refusing set origin");
         return false;
     }