autotest: stop using MAVProxy to change modes

This commit is contained in:
Peter Barker 2021-02-18 15:12:25 +11:00 committed by Peter Barker
parent 2c1df16442
commit dcc04ccd09
4 changed files with 21 additions and 21 deletions

View File

@ -4768,8 +4768,11 @@ class AutoTestCopter(AutoTest):
# arm in Stabilize and attempt to switch to Loiter # arm in Stabilize and attempt to switch to Loiter
self.change_mode('STABILIZE') self.change_mode('STABILIZE')
self.arm_vehicle() self.arm_vehicle()
self.mavproxy.send("mode LOITER\n") self.context_collect('STATUSTEXT')
self.mavproxy.expect("requires position") self.run_cmd_do_set_mode(
"LOITER",
want_result=mavutil.mavlink.MAV_RESULT_FAILED)
self.wait_statustext("requires position", check_context=True)
self.disarm_vehicle() self.disarm_vehicle()
self.context_pop() self.context_pop()
self.reboot_sitl() self.reboot_sitl()

View File

@ -408,8 +408,7 @@ class AutoTestPlane(AutoTest):
self.change_altitude(self.homeloc.alt+300) self.change_altitude(self.homeloc.alt+300)
self.set_rc(2, 1500) self.set_rc(2, 1500)
self.mavproxy.send("mode STABILIZE\n") self.change_mode('STABILIZE')
self.wait_mode('STABILIZE')
while count > 0: while count > 0:
self.progress("Starting roll") self.progress("Starting roll")
@ -436,8 +435,7 @@ class AutoTestPlane(AutoTest):
self.change_altitude(self.homeloc.alt+300) self.change_altitude(self.homeloc.alt+300)
self.set_rc(2, 1500) self.set_rc(2, 1500)
self.mavproxy.send("mode ACRO\n") self.change_mode('ACRO')
self.wait_mode('ACRO')
while count > 0: while count > 0:
self.progress("Starting roll") self.progress("Starting roll")
@ -454,8 +452,7 @@ class AutoTestPlane(AutoTest):
self.wait_level_flight() self.wait_level_flight()
self.mavproxy.send("mode ACRO\n") self.change_mode('ACRO')
self.wait_mode('ACRO')
count = 2 count = 2
while count > 0: while count > 0:
@ -475,8 +472,7 @@ class AutoTestPlane(AutoTest):
def test_FBWB(self, mode='FBWB'): def test_FBWB(self, mode='FBWB'):
"""Fly FBWB or CRUISE mode.""" """Fly FBWB or CRUISE mode."""
self.mavproxy.send("mode %s\n" % mode) self.change_mode(mode)
self.wait_mode(mode)
self.set_rc(3, 1700) self.set_rc(3, 1700)
self.set_rc(2, 1500) self.set_rc(2, 1500)

View File

@ -106,8 +106,7 @@ class AutoTestSub(AutoTest):
""" """
self.wait_ready_to_arm() self.wait_ready_to_arm()
self.arm_vehicle() self.arm_vehicle()
self.mavproxy.send('mode ALT_HOLD\n') self.change_mode('ALT_HOLD')
self.wait_mode('ALT_HOLD')
msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=5) msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=5)
if msg is None: if msg is None:
@ -160,8 +159,7 @@ class AutoTestSub(AutoTest):
self.arm_vehicle() self.arm_vehicle()
# point North # point North
self.reach_heading_manual(0) self.reach_heading_manual(0)
self.mavproxy.send('mode POSHOLD\n') self.change_mode('POSHOLD')
self.wait_mode('POSHOLD')
# dive a little # dive a little
self.set_rc(Joystick.Throttle, 1300) self.set_rc(Joystick.Throttle, 1300)
@ -278,7 +276,7 @@ class AutoTestSub(AutoTest):
return return
self.load_mission("sub-gripper-mission.txt") self.load_mission("sub-gripper-mission.txt")
self.mavproxy.send('mode loiter\n') self.change_mode('LOITER')
self.wait_ready_to_arm() self.wait_ready_to_arm()
self.arm_vehicle() self.arm_vehicle()
self.change_mode('AUTO') self.change_mode('AUTO')

View File

@ -4181,10 +4181,7 @@ class AutoTest(ABC):
bearing += 360.00 bearing += 360.00
return bearing return bearing
def change_mode(self, mode, timeout=60): def send_cmd_do_set_mode(self, mode):
'''change vehicle flightmode'''
self.wait_heartbeat()
self.progress("Changing mode to %s" % mode)
self.send_cmd( self.send_cmd(
mavutil.mavlink.MAV_CMD_DO_SET_MODE, mavutil.mavlink.MAV_CMD_DO_SET_MODE,
mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
@ -4193,8 +4190,14 @@ class AutoTest(ABC):
0, 0,
0, 0,
0, 0,
0, 0
) )
def change_mode(self, mode, timeout=60):
'''change vehicle flightmode'''
self.wait_heartbeat()
self.progress("Changing mode to %s" % mode)
self.send_cmd_do_set_mode(mode)
tstart = self.get_sim_time() tstart = self.get_sim_time()
while not self.mode_is(mode): while not self.mode_is(mode):
custom_num = self.mav.messages['HEARTBEAT'].custom_mode custom_num = self.mav.messages['HEARTBEAT'].custom_mode
@ -4203,7 +4206,7 @@ class AutoTest(ABC):
if (timeout is not None and if (timeout is not None and
self.get_sim_time_cached() > tstart + timeout): self.get_sim_time_cached() > tstart + timeout):
raise WaitModeTimeout("Did not change mode") raise WaitModeTimeout("Did not change mode")
self.mavproxy.send('mode %s\n' % mode) self.send_cmd_do_set_mode(mode)
self.progress("Got mode %s" % mode) self.progress("Got mode %s" % mode)
def capable(self, capability): def capable(self, capability):