aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Tools: Rework Plane.EKFlaneswitch autotest

Browse Source
This commit is contained in:
Paul Riseborough 2021-07-19 15:38:51 +10:00 committed by Andrew Tridgell
parent 7f3980c1f4
commit 9207744152
1 changed files with 16 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
Tools/autotest/arduplane.py
View File

@ -2588,40 +2588,44 @@ class AutoTestPlane(AutoTest):
self.start_subtest("AIRSPEED: Fail to constant value")
self.context_push()
self.context_collect("STATUSTEXT")
# create an airspeed sensor error by freezing to the
# current airspeed then changing the groundspeed
old_parameter = self.get_parameter("SIM_ARSPD_FAIL")
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
self.set_parameter("SIM_ARSPD_FAIL", m.airspeed)
def change_speed():
old_parameter = self.get_parameter("SIM_ARSPD_FAIL")
def fail_speed():
self.change_mode("GUIDED")
loc = self.mav.location()
self.run_cmd_int(
mavutil.mavlink.MAV_CMD_DO_REPOSITION,
0,
0,
0,
0,
12345, # lat* 1e7
12345, # lon* 1e7
int(loc.lat * 1e7),
int(loc.lng * 1e7),
50 # alt
)
self.delay_sim_time(5)
new_target_groundspeed = m.groundspeed + 10
# create an airspeed sensor error by freezing to the
# current airspeed then changing the airspeed demand
# to a higher value and waiting for the TECS speed
# loop to diverge
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
self.set_parameter("SIM_ARSPD_FAIL", m.airspeed)
self.run_cmd(
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
1, # groundspeed
new_target_groundspeed,
0, # airspeed
30,
-1, # throttle / no change
0, # absolute values
0,
0,
0
)
self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=change_speed, check_context=True)
self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=fail_speed, check_context=True)
if self.lane_switches != [1, 0, 1, 0, 1]:
raise NotAchievedException("Expected lane switch 1, got %s" % str(self.lane_switches[-1]))
# Cleanup
self.set_parameter("SIM_ARSPD_FAIL", old_parameter)
self.change_mode('CIRCLE')
self.context_pop()
self.context_clear_collection("STATUSTEXT")