Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

autotest: add test for Plane dead-reckoning support

This commit is contained in:
Peter Barker 2020-08-15 18:07:02 +10:00 committed by Peter Barker
parent 6bf2057712
commit 67b71e2b94
2 changed files with 86 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
Tools/autotest/ArduPlane_Tests/Deadreckoning/flaps.txt Normal file
View File

@ -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

73
Tools/autotest/arduplane.py
View File

@ -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),