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

autotest: test landed state of copter during takeoff in auto mode

This commit is contained in:
m 2022-09-01 14:58:06 +03:00 committed by Peter Barker
parent 80c36ecc97
commit 3fdcf5f940
4 changed files with 78 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Tools/autotest/ArduCopter_Tests/MAVLandedStateTakeoff/copter_mission.txt Normal file
View File

@ -0,0 +1,3 @@
QGC WPL 110
0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.089966 1
1 0 3 22 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 30.000000 1

25
Tools/autotest/arducopter.py
View File

@ -8596,6 +8596,30 @@ class AutoTestCopter(AutoTest):
self.change_mode('AUTO') self.change_mode('AUTO')
self.wait_disarmed() self.wait_disarmed()
def MAVLandedStateTakeoff(self):
'''check EXTENDED_SYS_STATE message'''
ex = None
try:
self.set_message_rate_hz(id=mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz=1)
self.wait_extended_sys_state(vtol_state=mavutil.mavlink.MAV_VTOL_STATE_MC,
landed_state=mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, timeout=10)
self.load_mission(filename="copter_mission.txt")
self.set_parameter(name="AUTO_OPTIONS", value=3)
self.change_mode(mode="AUTO")
self.wait_ready_to_arm()
self.arm_vehicle()
self.wait_extended_sys_state(vtol_state=mavutil.mavlink.MAV_VTOL_STATE_MC,
landed_state=mavutil.mavlink.MAV_LANDED_STATE_TAKEOFF, timeout=30)
self.wait_extended_sys_state(vtol_state=mavutil.mavlink.MAV_VTOL_STATE_MC,
landed_state=mavutil.mavlink.MAV_LANDED_STATE_IN_AIR, timeout=60)
self.land_and_disarm()
except Exception as e:
self.print_exception_caught(e)
ex = e
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, -1)
if ex is not None:
raise ex
def ATTITUDE_FAST(self): def ATTITUDE_FAST(self):
'''ensure that when ATTITDE_FAST is set we get many messages''' '''ensure that when ATTITDE_FAST is set we get many messages'''
self.context_push() self.context_push()
@ -8953,6 +8977,7 @@ class AutoTestCopter(AutoTest):
self.RichenPower, self.RichenPower,
self.IE24, self.IE24,
self.LogUpload, self.LogUpload,
self.MAVLandedStateTakeoff,
]) ])
return ret return ret

50
Tools/autotest/common.py
View File

@ -12562,3 +12562,53 @@ switch value'''
0, # param5 0, # param5
0, # param6 0, # param6
0) # param7 0) # param7
def enum_state_name(self, enum_name, state, pretrim=None):
e = mavutil.mavlink.enums[enum_name]
e_value = e[state]
name = e_value.name
if pretrim is not None:
if not pretrim.startswith(pretrim):
raise NotAchievedException("Expected %s to pretrim" % (pretrim))
name = name.replace(pretrim, "")
return name
def vtol_state_name(self, state):
return self.enum_state_name("MAV_VTOL_STATE", state, pretrim="MAV_VTOL_STATE_")
def landed_state_name(self, state):
return self.enum_state_name("MAV_LANDED_STATE", state, pretrim="MAV_LANDED_STATE_")
def assert_extended_sys_state(self, vtol_state, landed_state):
m = self.assert_receive_message('EXTENDED_SYS_STATE', timeout=1)
if m.vtol_state != vtol_state:
raise ValueError("Bad MAV_VTOL_STATE. Want=%s got=%s" %
(self.vtol_state_name(vtol_state),
self.vtol_state_name(m.vtol_state)))
if m.landed_state != landed_state:
raise ValueError("Bad MAV_LANDED_STATE. Want=%s got=%s" %
(self.landed_state_name(landed_state),
self.landed_state_name(m.landed_state)))
def wait_extended_sys_state(self, vtol_state, landed_state, timeout=10):
tstart = self.get_sim_time()
while True:
if self.get_sim_time() - tstart > timeout:
raise NotAchievedException("Did not achieve vol/landed states")
self.progress("Waiting for MAV_VTOL_STATE=%s MAV_LANDED_STATE=%s" %
(self.vtol_state_name(vtol_state),
self.landed_state_name(landed_state)))
m = self.assert_receive_message('EXTENDED_SYS_STATE', verbose=True)
if m.landed_state != landed_state:
self.progress("Wrong MAV_LANDED_STATE (want=%s got=%s)" %
(self.landed_state_name(landed_state),
self.landed_state_name(m.landed_state)))
continue
if m.vtol_state != vtol_state:
self.progress("Wrong MAV_VTOL_STATE (want=%s got=%s)" %
(self.vtol_state_name(vtol_state),
self.vtol_state_name(m.vtol_state)))
continue
self.progress("vtol and landed states match")
return

50
Tools/autotest/quadplane.py
View File

@ -306,56 +306,6 @@ class AutoTestQuadPlane(AutoTest):
self.wait_disarmed(timeout=120) # give quadplane a long time to land self.wait_disarmed(timeout=120) # give quadplane a long time to land
self.progress("Mission OK") self.progress("Mission OK")
def enum_state_name(self, enum_name, state, pretrim=None):
e = mavutil.mavlink.enums[enum_name]
e_value = e[state]
name = e_value.name
if pretrim is not None:
if not pretrim.startswith(pretrim):
raise NotAchievedException("Expected %s to pretrim" % (pretrim))
name = name.replace(pretrim, "")
return name
def vtol_state_name(self, state):
return self.enum_state_name("MAV_VTOL_STATE", state, pretrim="MAV_VTOL_STATE_")
def landed_state_name(self, state):
return self.enum_state_name("MAV_LANDED_STATE", state, pretrim="MAV_LANDED_STATE_")
def assert_extended_sys_state(self, vtol_state, landed_state):
m = self.assert_receive_message('EXTENDED_SYS_STATE', timeout=1)
if m.vtol_state != vtol_state:
raise ValueError("Bad MAV_VTOL_STATE. Want=%s got=%s" %
(self.vtol_state_name(vtol_state),
self.vtol_state_name(m.vtol_state)))
if m.landed_state != landed_state:
raise ValueError("Bad MAV_LANDED_STATE. Want=%s got=%s" %
(self.landed_state_name(landed_state),
self.landed_state_name(m.landed_state)))
def wait_extended_sys_state(self, vtol_state, landed_state, timeout=10):
tstart = self.get_sim_time()
while True:
if self.get_sim_time() - tstart > timeout:
raise NotAchievedException("Did not achieve vol/landed states")
self.progress("Waiting for MAV_VTOL_STATE=%s MAV_LANDED_STATE=%s" %
(self.vtol_state_name(vtol_state),
self.landed_state_name(landed_state)))
m = self.assert_receive_message('EXTENDED_SYS_STATE', verbose=True)
if m.landed_state != landed_state:
self.progress("Wrong MAV_LANDED_STATE (want=%s got=%s)" %
(self.landed_state_name(landed_state),
self.landed_state_name(m.landed_state)))
continue
if m.vtol_state != vtol_state:
self.progress("Wrong MAV_VTOL_STATE (want=%s got=%s)" %
(self.vtol_state_name(vtol_state),
self.vtol_state_name(m.vtol_state)))
continue
self.progress("vtol and landed states match")
return
def EXTENDED_SYS_STATE_SLT(self): def EXTENDED_SYS_STATE_SLT(self):
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, 10) self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, 10)
self.change_mode("QHOVER") self.change_mode("QHOVER")