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

autotest: add simple test for LoiterAltQLand

Browse Source
This commit is contained in:
Peter Barker 2022-02-04 17:14:13 +11:00 committed by Peter Barker
parent a57ed97ba1
commit 7d38164176
2 changed files with 145 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Tools/autotest/common.py
View File

@ -5517,6 +5517,9 @@ class AutoTest(ABC):
0
)
def assert_mode(self, mode):
self.wait_mode(mode, timeout=0)
def change_mode(self, mode, timeout=60):
'''change vehicle flightmode'''
self.wait_heartbeat()
@ -5771,6 +5774,15 @@ class AutoTest(ABC):
while tstart + seconds_to_wait > tnow:
tnow = self.get_sim_time(drain_mav=False)
def send_terrain_check_message(self):
here = self.mav.location()
self.mav.mav.terrain_check_send(int(here.lat * 1e7), int(here.lng * 1e7))
def get_terrain_height(self, verbose=False):
self.send_terrain_check_message()
m = self.assert_receive_message('TERRAIN_REPORT', very_verbose=True)
return m.terrain_height
def get_altitude(self, relative=False, timeout=30, altitude_source=None):
'''returns vehicles altitude in metres, possibly relative-to-home'''
if altitude_source is None:

133
Tools/autotest/quadplane.py
View File

@ -769,6 +769,138 @@ class AutoTestQuadPlane(AutoTest):
self.change_mode("RTL")
self.wait_disarmed(timeout=300)
def LoiterAltQLand(self):
'''test loitering and qland with terrain involved'''
self.LoiterAltQLand_Terrain(
home="LakeGeorgeLookout",
ofs_n=0,
ofs_e=300,
)
# self.LoiterAltQLand_Terrain(
# home="KalaupapaCliffs",
# ofs_n=500,
# ofs_e=500,
# )
self.LoiterAltQLand_Relative()
def LoiterAltQLand_Relative(self):
'''test failsafe where vehicle loiters in fixed-wing mode to a
specific altitude then changes mode to QLAND'''
self.set_parameters({
'BATT_MONITOR': 4, # LoiterAltQLand
'BATT_FS_LOW_ACT': 6, # LoiterAltQLand
})
self.reboot_sitl()
takeoff_alt = 5
self.takeoff(takeoff_alt, mode='QLOITER')
loc = self.mav.location()
self.location_offset_ne(loc, 500, 500)
new_alt = 100
initial_altitude = self.get_altitude(relative=False, timeout=2)
self.run_cmd_int(
mavutil.mavlink.MAV_CMD_DO_REPOSITION,
0,
1, # reposition flags; 1 means "change to guided"
0,
0,
int(loc.lat * 1e7),
int(loc.lng * 1e7),
new_alt, # alt
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
)
self.wait_altitude(
new_alt-1,
new_alt+1,
timeout=60,
relative=True,
minimum_duration=10)
self.wait_location(loc, timeout=120, accuracy=100)
self.progress("Triggering failsafe")
self.set_parameter('BATT_LOW_VOLT', 50)
self.wait_mode(25) # LoiterAltQLand
self.drain_mav()
m = self.assert_receive_message('POSITION_TARGET_GLOBAL_INT', very_verbose=True)
q_rtl_alt = self.get_parameter('Q_RTL_ALT')
expected_alt = initial_altitude - takeoff_alt + q_rtl_alt
if abs(m.alt - expected_alt) > 20:
raise NotAchievedException("Unexpected altitude; expected=%f got=%f" %
(expected_alt, m.alt))
self.assert_mode('LOITERALTQLAND')
self.wait_mode('QLAND')
alt = self.get_altitude(relative=True)
if abs(alt - q_rtl_alt) > 2:
raise NotAchievedException("qland too late; want=%f got=%f" %
(alt, q_rtl_alt))
self.wait_disarmed(timeout=300)
def LoiterAltQLand_Terrain(self,
home=None,
ofs_n=None,
ofs_e=None,
reposition_alt=100):
'''test failsafe where vehicle loiters in fixed-wing mode to a
specific altitude then changes mode to QLAND'''
self.context_push()
self.install_terrain_handlers_context()
self.set_parameters({
'BATT_MONITOR': 4, # LoiterAltQLand
'BATT_FS_LOW_ACT': 6, # LoiterAltQLand
'TERRAIN_FOLLOW': 1, # enabled in all modes
})
self.customise_SITL_commandline(
["--home", home]
)
takeoff_alt = 5
self.takeoff(takeoff_alt, mode='QLOITER')
loc = self.mav.location()
self.location_offset_ne(loc, ofs_n, ofs_e)
initial_altitude = self.get_altitude(relative=False, timeout=2)
self.run_cmd_int(
mavutil.mavlink.MAV_CMD_DO_REPOSITION,
0,
1, # reposition flags; 1 means "change to guided"
0,
0,
int(loc.lat * 1e7),
int(loc.lng * 1e7),
reposition_alt, # alt
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
)
self.wait_altitude(
reposition_alt-1,
reposition_alt+1,
timeout=60,
relative=True,
minimum_duration=10)
self.wait_location(loc, timeout=500, accuracy=100)
self.progress("Triggering failsafe")
self.set_parameter('BATT_LOW_VOLT', 50)
self.wait_mode(25) # LoiterAltQLand
terrain_alt = self.get_terrain_height(verbose=True)
self.drain_mav()
m = self.assert_receive_message('POSITION_TARGET_GLOBAL_INT', very_verbose=True)
q_rtl_alt = self.get_parameter('Q_RTL_ALT')
expected_alt = terrain_alt + q_rtl_alt
if abs(m.alt - expected_alt) > 20:
raise NotAchievedException("Unexpected altitude; expected=%f got=%f" %
(expected_alt, m.alt))
self.assert_mode('LOITERALTQLAND')
self.wait_mode('QLAND')
alt = initial_altitude + self.get_altitude(relative=True)
if abs(alt - expected_alt) > 10:
raise NotAchievedException("qland too late; want=%f got=%f" %
(expected_alt, alt))
self.wait_disarmed(timeout=300)
self.zero_throttle()
self.reset_SITL_commandline()
self.context_pop()
def Tailsitter(self):
'''tailsitter test'''
self.set_parameter('Q_FRAME_CLASS', 10)
@ -979,5 +1111,6 @@ class AutoTestQuadPlane(AutoTest):
self.BootInAUTO,
self.Ship,
self.MAV_CMD_NAV_LOITER_TO_ALT,
self.LoiterAltQLand,
])
return ret