autotest: add test for Plane dead-reckoning support

Tools/autotest/ArduPlane_Tests/Deadreckoning/flaps.txt

@@ -0,0 +1,13 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	584.390015	1
+1	0	3	22	15.000000	0.000000	0.000000	0.000000	-35.361279	149.164230	30.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361229	149.163025	80.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364563	149.163773	80.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364384	149.164795	80.000000	1
+5	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361027	149.164093	80.000000	1
+6	0	0	177	2.000000	3.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+7	0	3	189	0.000000	0.000000	0.000000	0.000000	-35.362915	149.162613	60.000000	1
+8	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.363136	149.162750	60.000000	1
+9	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.365467	149.164215	55.000000	1
+10	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.365009	149.165482	39.889999	1
+11	0	3	21	0.000000	0.000000	0.000000	1.000000	-35.363041	149.165222	0.000000	1

Tools/autotest/arduplane.py

@@ -1327,6 +1327,75 @@ class AutoTestPlane(AutoTest):
         self.wait_waypoint(7, num_wp-1, timeout=500)
         self.wait_disarmed(timeout=120)
 
+    def deadreckoning_main(self, disable_airspeed_sensor=False):
+        self.gpi = None
+        self.simstate = None
+        self.last_print = 0
+        self.max_divergence = 0
+        def validate_global_position_int_against_simstate(mav, m):
+            if m.get_type() == 'GLOBAL_POSITION_INT':
+                self.gpi = m
+            elif m.get_type() == 'SIMSTATE':
+                self.simstate = m
+            if self.gpi is None:
+                return
+            if self.simstate is None:
+                return
+            divergence = self.get_distance_int(self.gpi, self.simstate)
+            max_allowed_divergence = 200
+            if time.time() - self.last_print > 1:
+                self.progress("position-estimate-divergence=%fm" % (divergence,))
+                self.last_print = time.time()
+            if divergence > max_allowed_divergence:
+                raise NotAchievedException("global-position-int diverged from simstate by >%fm" % (max_allowed_divergence,))
+            if divergence > self.max_divergence:
+                self.max_divergence = divergence
+
+        self.install_message_hook(validate_global_position_int_against_simstate)
+
+        try:
+            # wind is from the West:
+            self.set_parameter("SIM_WIND_DIR", 270)
+            # light winds:
+            self.set_parameter("SIM_WIND_SPD", 10)
+            if disable_airspeed_sensor:
+                self.set_parameter("ARSPD_USE", 0)
+
+            self.takeoff(50)
+            loc = self.mav.location()
+            loc.lat = -35.35690712
+            loc.lng = 149.17083386
+            self.run_cmd_int(
+                mavutil.mavlink.MAV_CMD_DO_REPOSITION,
+                0,
+                mavutil.mavlink.MAV_DO_REPOSITION_FLAGS_CHANGE_MODE,
+                0,
+                0,
+                int(loc.lat*1e7),
+                int(loc.lng*1e7),
+                100,    # alt
+                frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+            )
+            self.wait_location(loc, accuracy=100)
+            self.progress("Stewing")
+            self.delay_sim_time(20)
+            self.set_parameter("SIM_GPS_DISABLE", 1)
+            self.progress("Roasting")
+            self.delay_sim_time(20)
+            self.change_mode("RTL")
+            self.wait_distance_to_home(100, 200, timeout=200)
+            self.set_parameter("SIM_GPS_DISABLE", 0)
+            self.delay_sim_time(10)
+            self.set_rc(3, 1000)
+            self.fly_home_land_and_disarm()
+            self.progress("max-divergence: %fm" % (self.max_divergence,))
+        finally:
+            self.remove_message_hook(validate_global_position_int_against_simstate)
+
+    def deadreckoning(self):
+        self.deadreckoning_main()
+        self.deadreckoning_main(disable_airspeed_sensor=True)
+
     def sample_enable_parameter(self):
         return "Q_ENABLE"
 
@@ -1878,6 +1947,10 @@ class AutoTestPlane(AutoTest):
              "Test terrain following in mission",
              self.fly_terrain_mission),
 
+            ("Deadreckoning",
+             "Test deadreckoning support",
+             self.deadreckoning),
+
             ("LogUpload",
              "Log upload",
              self.log_upload),