From b8030816b8f4ccd827ba6604fdc3ea8379d3ef59 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 14 Apr 2018 21:31:22 +1000 Subject: [PATCH] Tools: autotest: add tests for mode changes for Copter and Rover --- Tools/autotest/apmrover2.py | 88 ++++++++++++++++++++++- Tools/autotest/arducopter.py | 84 +++++++++++++++++++++- Tools/autotest/common.py | 26 ++++--- Tools/autotest/default_params/copter.parm | 2 +- Tools/scripts/runfliptest.py | 2 +- 5 files changed, 189 insertions(+), 13 deletions(-) diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py index aa43925ce5..7f8f3b6e45 100644 --- a/Tools/autotest/apmrover2.py +++ b/Tools/autotest/apmrover2.py @@ -328,7 +328,7 @@ class AutoTestRover(AutoTest): self.set_rc(3, 1500) self.wait_mode('AUTO') self.wait_waypoint(1, 4, max_dist=5) - self.wait_mode('HOLD') + self.wait_mode('HOLD', timeout=300) self.progress("Mission OK") def drive_mission_rover1(self): @@ -465,6 +465,83 @@ class AutoTestRover(AutoTest): raise NotAchievedException() self.progress("Pin mask changed after relay command") + def test_setting_modes_via_mavproxy_switch(self): + fnoo = [(1, 'MANUAL'), + (2, 'MANUAL'), + (3, 'RTL'), + # (4, 'AUTO'), // no mission, can't set auto + (5, 'RTL'), # non-existant mode, should stay in RTL + (6, 'MANUAL')] + for (num, expected) in fnoo: + self.mavproxy.send('switch %u\n' % num) + self.wait_mode(expected) + + def test_setting_modes_via_modeswitch(self): + # test setting of modes through mode switch + self.context_push(); + ex = None + try: + self.set_parameter("MODE_CH", 8) + self.set_rc(8, 1000) + # mavutil.mavlink.ROVER_MODE_HOLD: + self.set_parameter("MODE6", 4) + # mavutil.mavlink.ROVER_MODE_ACRO + self.set_parameter("MODE5", 1) + self.set_rc(8, 1800) # PWM for mode6 + self.wait_mode("HOLD") + self.set_rc(8, 1700) # PWM for mode5 + self.wait_mode("ACRO") + self.set_rc(8, 1800) # PWM for mode6 + self.wait_mode("HOLD") + self.set_rc(8, 1700) # PWM for mode5 + self.wait_mode("ACRO") + except Exception as e: + self.progress("Exception caught") + ex = e + + self.context_pop(); + + if ex is not None: + raise ex + + def test_setting_modes_via_auxswitches(self): + self.context_push(); + ex = None + try: + self.set_parameter("MODE5", 1) + self.mavproxy.send('switch 5\n') # acro mode + self.wait_mode("ACRO") + self.set_rc(9, 1000) + self.set_rc(10, 1000) + self.set_parameter("RC9_OPTION", 53) # steering + self.set_parameter("RC10_OPTION", 54) # hold + self.set_rc(9, 1900) + self.wait_mode("STEERING") + self.set_rc(10, 1900) + self.wait_mode("HOLD") + + # reset both switches - should go back to ACRO + self.set_rc(9, 1000) + self.set_rc(10, 1000) + self.wait_mode("ACRO") + + self.set_rc(9, 1900) + self.wait_mode("STEERING") + self.set_rc(10, 1900) + self.wait_mode("HOLD") + + self.set_rc(10, 1000) # this re-polls the mode switch + self.wait_mode("ACRO") + self.set_rc(9, 1000) + except Exception as e: + self.progress("Exception caught") + ex = e + + self.context_pop(); + + if ex is not None: + raise ex + def autotest(self): """Autotest APMrover2 in SITL.""" if not self.hasInit: @@ -489,6 +566,15 @@ class AutoTestRover(AutoTest): self.wait_ready_to_arm() self.arm_vehicle() + self.run_test("Set modes via mavproxy switch", + self.test_setting_modes_via_mavproxy_switch) + + self.run_test("Set modes via modeswitch", + self.test_setting_modes_via_modeswitch) + + self.run_test("Set modes via auxswitches", + self.test_setting_modes_via_auxswitches) + self.run_test("Drive an RTL Mission", self.drive_rtl_mission) self.run_test("Learn/Drive Square with Ch7 option", diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 1927ef8c35..6cb9d73798 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -633,7 +633,7 @@ class AutoTestCopter(AutoTest): self.set_rc(3, 1800) # wait for fence to trigger - self.wait_mode('RTL') + self.wait_mode('RTL', timeout=120) self.progress("Waiting for disarm") self.mav.motors_disarmed_wait() @@ -1297,6 +1297,79 @@ class AutoTestCopter(AutoTest): if ex is not None: raise ex + def test_setting_modes_via_modeswitch(self): + self.context_push(); + ex = None + try: + fltmode_ch = 5 + self.set_parameter("FLTMODE_CH", fltmode_ch) + self.set_rc(fltmode_ch, 1000) # PWM for mode1 + testmodes = [("FLTMODE1", 4, "GUIDED", 1165), + ("FLTMODE2", 13, "SPORT", 1295), + ("FLTMODE3", 6, "RTL", 1425), + ("FLTMODE4", 7, "CIRCLE", 1555), + ("FLTMODE5", 1, "ACRO", 1685), + ("FLTMODE6", 17, "BRAKE", 1815), + ] + for mode in testmodes: + (parm, parm_value, name, pwm) = mode + self.set_parameter(parm, parm_value) + + for mode in reversed(testmodes): + (parm, parm_value, name, pwm) = mode + self.set_rc(fltmode_ch, pwm) + self.wait_mode(name) + + for mode in testmodes: + (parm, parm_value, name, pwm) = mode + self.set_rc(fltmode_ch, pwm) + self.wait_mode(name) + + for mode in reversed(testmodes): + (parm, parm_value, name, pwm) = mode + self.set_rc(fltmode_ch, pwm) + self.wait_mode(name) + + self.mavproxy.send('switch 6\n') + self.wait_mode("BRAKE") + self.mavproxy.send('switch 5\n') + self.wait_mode("ACRO") + + except Exception as e: + self.progress("Exception caught") + ex = e + + self.context_pop() + + if ex is not None: + raise ex + + def test_setting_modes_via_auxswitch(self): + self.context_push(); + ex = None + try: + fltmode_ch = int(self.get_parameter("FLTMODE_CH")) + self.set_rc(fltmode_ch, 1000) + self.wait_mode("CIRCLE") + self.set_rc(9, 1000) + self.set_rc(10, 1000) + self.set_parameter("RC9_OPTION", 18) # land + self.set_parameter("RC10_OPTION", 55) # guided + self.set_rc(9, 1900) + self.wait_mode("LAND") + self.set_rc(10, 1900) + self.wait_mode("GUIDED") + self.set_rc(10, 1000) # this re-polls the mode switch + self.wait_mode("CIRCLE") + except Exception as e: + self.progress("Exception caught") + ex = e + + self.context_pop(); + + if ex is not None: + raise ex + def guided_achieve_heading(self, heading): tstart = self.get_sim_time() self.run_cmd(mavutil.mavlink.MAV_CMD_CONDITION_YAW, @@ -1415,6 +1488,15 @@ class AutoTestCopter(AutoTest): self.progress("Waiting reading for arm") self.wait_ready_to_arm() + self.run_test("Set modes via modeswitch", + self.test_setting_modes_via_modeswitch) + + self.run_test("Set modes via auxswitch", + self.test_setting_modes_via_auxswitch) + + self.mavproxy.send('switch 6\n') # stabilize mode + self.wait_mode('STABILIZE') + # Arm self.run_test("Arm motors", self.arm_vehicle) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 79db695eb7..e14d7c85f6 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -223,6 +223,11 @@ class AutoTest(ABC): self.buildlogs_path("%s-valgrind.log" % self.log_name)) + def start_test(self, description): + self.progress("#") + self.progress("########## %s ##########" % description) + self.progress("#") + ################################################# # GENERAL UTILITIES ################################################# @@ -729,7 +734,7 @@ class AutoTest(ABC): (wpnum_end, wpnum_end)) raise WaitWaypointTimeout() - def wait_mode(self, mode, timeout=None): + def wait_mode(self, mode, timeout=60): """Wait for mode to change.""" mode_map = self.mav.mode_mapping() if mode_map is None or mode not in mode_map: @@ -738,15 +743,15 @@ class AutoTest(ABC): raise ErrorException() self.progress("Waiting for mode %s" % mode) tstart = self.get_sim_time() - hastimeout = False - while self.mav.flightmode != mode and not hastimeout: - if timeout is not None: - hastimeout = self.get_sim_time() > tstart + timeout + self.mav.wait_heartbeat() + while self.mav.flightmode != mode: + if (timeout is not None and + self.get_sim_time() > tstart + timeout): + raise WaitModeTimeout() self.mav.wait_heartbeat() +# self.progress("heartbeat mode %s Want: %s" % ( +# self.mav.flightmode, mode)) self.progress("Got mode %s" % mode) - if self.mav.flightmode != mode and hastimeout: - raise WaitModeTimeout() - return True def wait_ready_to_arm(self, timeout=None): # wait for EKF checks to pass @@ -791,7 +796,7 @@ class AutoTest(ABC): self.progress("Failed to get EKF.flags=%u" % required_value) raise AutoTestTimeoutException() - def run_test(self, desc, function): + def run_test(self, desc, function, interact=False): self.progress("#") self.progress("########## %s ##########" % (desc)) self.progress("#") @@ -801,6 +806,9 @@ class AutoTest(ABC): except Exception as e: self.progress('FAILED: "%s": %s' % (desc, repr(e))) self.fail_list.append((desc, e)) + if interact: + self.progress("Starting MAVProxy interaction as directed") + self.mavproxy.interact() return self.progress('PASSED: "%s"' % desc) diff --git a/Tools/autotest/default_params/copter.parm b/Tools/autotest/default_params/copter.parm index 3e4e03d15b..6c0cd46842 100644 --- a/Tools/autotest/default_params/copter.parm +++ b/Tools/autotest/default_params/copter.parm @@ -3,7 +3,6 @@ FRAME_TYPE 0 MAG_ENABLE 1 FS_THR_ENABLE 1 BATT_MONITOR 4 -CH7_OPT 7 COMPASS_LEARN 0 COMPASS_OFS_X 5 COMPASS_OFS_Y 13 @@ -33,6 +32,7 @@ RC6_MIN 1000.000000 RC6_TRIM 1500.000000 RC7_MAX 2000.000000 RC7_MIN 1000.000000 +RC7_OPTION 7 RC7_TRIM 1500.000000 RC8_MAX 2000.000000 RC8_MIN 1000.000000 diff --git a/Tools/scripts/runfliptest.py b/Tools/scripts/runfliptest.py index 1aa638307a..02747ee002 100755 --- a/Tools/scripts/runfliptest.py +++ b/Tools/scripts/runfliptest.py @@ -47,7 +47,7 @@ mavproxy.expect('ARMED') mavproxy.send('alt_hold\n') wait_mode(mav, ['ALT_HOLD']) mavproxy.send('rc 3 1800\n') -mavproxy.send('param set CH7_OPT 2\n') +mavproxy.send('param set RC7_OPTION 2\n') mavproxy.send('repeat add 2 rc 7 2000\n') wait_time(mav, 1) mavproxy.send('repeat add 2 rc 7 1000\n')