mirror of https://github.com/ArduPilot/ardupilot
autotest: test landed state of copter during takeoff in auto mode
This commit is contained in:
m
Peter Barker
parent
80c36ecc97
commit
3fdcf5f940
|
|
@ -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
|
||||
|
|
@ -8596,6 +8596,30 @@ class AutoTestCopter(AutoTest):
|
|||
self.change_mode('AUTO')
|
||||
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):
|
||||
'''ensure that when ATTITDE_FAST is set we get many messages'''
|
||||
self.context_push()
|
||||
|
|
@ -8953,6 +8977,7 @@ class AutoTestCopter(AutoTest):
|
|||
self.RichenPower,
|
||||
self.IE24,
|
||||
self.LogUpload,
|
||||
self.MAVLandedStateTakeoff,
|
||||
])
|
||||
return ret
|
||||
|
||||
|
|
|
|||
|
|
@ -12562,3 +12562,53 @@ switch value'''
|
|||
0, # param5
|
||||
0, # param6
|
||||
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
|
||||
|
|
|
|||
|
|
@ -306,56 +306,6 @@ class AutoTestQuadPlane(AutoTest):
|
|||
self.wait_disarmed(timeout=120) # give quadplane a long time to land
|
||||
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):
|
||||
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, 10)
|
||||
self.change_mode("QHOVER")
|
||||
|
|
|
|||
Loading…
Reference in New Issue