AP_NavEKF3: accept set origin even when using GPS

Tools/autotest/ArduCopter_Tests/FarOrigin/copter_mission.txt

@@ -0,0 +1,14 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	1015.559998	1
+1	0	3	22	0.000000	0.000000	0.000000	0.000000	-35.362881	149.165222	20.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364652	149.163501	20.000000	1
+3	0	0	115	640.000000	20.000000	1.000000	1.000000	0.000000	0.000000	0.000000	1
+4	0	3	19	5.000000	0.000000	1.000000	1.000000	0.000000	0.000000	20.000000	1
+5	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.365361	149.163501	0.000000	1
+6	0	3	16	1.000000	0.000000	0.000000	0.000000	-35.365361	149.163995	40.000000	1
+7	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.365361	149.164563	20.000000	1
+8	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364652	149.164531	20.000000	1
+9	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364652	149.163995	20.000000	1
+10	0	0	177	8.000000	1.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+11	0	3	16	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+12	0	0	20	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1

Tools/autotest/arducopter.py

@@ -2768,6 +2768,46 @@ def CopterMission(self):
 
         self.progress("Auto mission completed: passed!")
 
+    def set_origin(self, loc, timeout=60):
+        '''set the GPS global origin to loc'''
+        tstart = self.get_sim_time()
+        while True:
+            if self.get_sim_time_cached() - tstart > timeout:
+                raise AutoTestTimeoutException("Did not get non-zero lat")
+            target_system = 1
+            self.mav.mav.set_gps_global_origin_send(
+                target_system,
+                int(loc.lat * 1e7),
+                int(loc.lng * 1e7),
+                int(loc.alt * 1e3)
+            )
+            gpi = self.assert_receive_message('GLOBAL_POSITION_INT')
+            self.progress("gpi=%s" % str(gpi))
+            if gpi.lat != 0:
+                break
+
+    def FarOrigin(self):
+        '''fly a mission far from the vehicle origin'''
+        # Fly mission #1
+        self.set_parameters({
+            "SIM_GPS_DISABLE": 1,
+        })
+        self.reboot_sitl()
+        nz = mavutil.location(-43.730171, 169.983118, 1466.3, 270)
+        self.set_origin(nz)
+        self.set_parameters({
+            "SIM_GPS_DISABLE": 0,
+        })
+        self.progress("# Load copter_mission")
+        # load the waypoint count
+        num_wp = self.load_mission("copter_mission.txt", strict=False)
+        if not num_wp:
+            raise NotAchievedException("load copter_mission failed")
+
+        self.fly_loaded_mission(num_wp)
+
+        self.progress("Auto mission completed: passed!")
+
     def fly_loaded_mission(self, num_wp):
         '''fly mission loaded on vehicle.  FIXME: get num_wp from vehicle'''
         self.progress("test: Fly a mission from 1 to %u" % num_wp)
@@ -11388,6 +11428,7 @@ def tests2b(self):  # this block currently around 9.5mins here
             self.CompassMot,
             self.AutoRTL,
             self.EK3_OGN_HGT_MASK,
+            self.FarOrigin,
         ])
         return ret
 

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -1392,12 +1392,10 @@ bool NavEKF3::setOriginLLH(const Location &loc)
     if (!core) {
         return false;
     }
-    if ((sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS) || 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.
-        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3 refusing set origin");
+    if (common_origin_valid) {
+        // we don't allow setting the EKF origin if it has already been set
+        // this is to prevent causing upsets from a shifting origin.
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3: origin already set");
         return false;
     }
     bool ret = false;

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -652,14 +652,9 @@ bool NavEKF3_core::assume_zero_sideslip(void) const
 }
 
 // sets the local NED origin using a LLH location (latitude, longitude, height)
-// returns false if absolute aiding and GPS is being used or if the origin is already set
+// returns false if the origin is already set
 bool NavEKF3_core::setOriginLLH(const Location &loc)
 {
-    if ((PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS)) {
-        // reject attempts to set the origin if GPS is being used or if the origin is already set
-        return false;
-    }
-
     return setOrigin(loc);
 }
 

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -224,7 +224,7 @@ class NavEKF3_core : public NavEKF_core_common
 
     // set the latitude and longitude and height used to set the NED origin
     // All NED positions calculated by the filter will be relative to this location
-    // returns false if Absolute aiding and GPS is being used or if the origin is already set
+    // returns false if the origin has already been set
     bool setOriginLLH(const Location &loc);
 
     // Set the EKF's NE horizontal position states and their corresponding variances from a supplied WGS-84 location and uncertainty