diff --git a/Tools/autotest/ArduPlane_Tests/DO_CHANGE_SPEED/mission.txt b/Tools/autotest/ArduPlane_Tests/DO_CHANGE_SPEED/mission.txt new file mode 100644 index 0000000000..2b59e24b9c --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/DO_CHANGE_SPEED/mission.txt @@ -0,0 +1,10 @@ +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 16 0.000000 0.000000 0.000000 0.000000 -35.366249 149.171337 50.000000 1 +2 0 0 178 0.000000 10.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361489 149.166778 50.000000 1 +4 0 0 178 0.000000 20.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.362949 149.162221 50.000000 1 +6 0 0 178 0.000000 15.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364969 149.165044 50.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361791 149.165045 50.000000 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index dc87335d1d..125f7f6df7 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -646,14 +646,49 @@ class AutoTestPlane(AutoTest): if not self.current_onboard_log_contains_message("BCL2"): raise NotAchievedException("Expected BCL2 message") - def fly_do_change_speed(self): + def DO_CHANGE_SPEED(self): + '''test DO_CHANGE_SPEED both as a mavlink command and as a mission item''' # the following lines ensure we revert these parameter values # - DO_CHANGE_AIRSPEED is a permanent vehicle change! self.set_parameters({ "TRIM_ARSPD_CM": self.get_parameter("TRIM_ARSPD_CM"), "MIN_GNDSPD_CM": self.get_parameter("MIN_GNDSPD_CM"), + "RTL_AUTOLAND": 1, }) + self.DO_CHANGE_SPEED_mavlink() + self.DO_CHANGE_SPEED_mission() + + def DO_CHANGE_SPEED_mission(self): + '''test DO_CHANGE_SPEED as a mission item''' + self.start_subtest("DO_CHANGE_SPEED_mission") + self.load_mission("mission.txt") + self.set_current_waypoint(1) + + self.progress("Takeoff") + self.set_rc(3, 1000) + self.takeoff(alt=10) + self.set_rc(3, 1500) + + self.start_subtest("Check initial speed") + + self.change_mode('AUTO') + + checks = [ + (1, self.get_parameter("TRIM_ARSPD_CM") * 0.01), + (3, 10), + (5, 20), + (7, 15), + ] + + for (current_waypoint, want_airspeed) in checks: + self.wait_current_waypoint(current_waypoint) + self.wait_airspeed(want_airspeed-1, want_airspeed+1, minimum_duration=5, timeout=120) + + self.fly_home_land_and_disarm() + + def DO_CHANGE_SPEED_mavlink(self): + '''test DO_CHANGE_SPEED as a mavlink command''' self.progress("Takeoff") self.takeoff(alt=100) self.set_rc(3, 1500) @@ -3592,7 +3627,9 @@ function''' ("TestFlaps", "Flaps", self.fly_flaps), - ("DO_CHANGE_SPEED", "Test mavlink DO_CHANGE_SPEED command", self.fly_do_change_speed), + ("DO_CHANGE_SPEED", + "Test DO_CHANGE_SPEED command/item", + self.DO_CHANGE_SPEED), ("DO_REPOSITION", "Test mavlink DO_REPOSITION command", diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index b33851a965..e0219f5822 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -5632,14 +5632,20 @@ class AutoTest(ABC): ) def wait_groundspeed(self, speed_min, speed_max, timeout=30, **kwargs): + self.wait_vfr_hud_speed("groundspeed", speed_min, speed_max, timeout=timeout, **kwargs) + + def wait_airspeed(self, speed_min, speed_max, timeout=30, **kwargs): + self.wait_vfr_hud_speed("airspeed", speed_min, speed_max, timeout=timeout, **kwargs) + + def wait_vfr_hud_speed(self, field, speed_min, speed_max, timeout=30, **kwargs): """Wait for a given ground speed range.""" assert speed_min <= speed_max, "Minimum speed should be less than maximum speed." - def get_groundspeed(timeout2): + def get_speed(timeout2): msg = self.mav.recv_match(type='VFR_HUD', blocking=True, timeout=timeout2) if msg: - return msg.groundspeed - raise MsgRcvTimeoutException("Failed to get Groundspeed") + return getattr(msg, field) + raise MsgRcvTimeoutException("Failed to get %s" % field) def validator(value2, target2=None): if speed_min <= value2 <= speed_max: @@ -5648,9 +5654,9 @@ class AutoTest(ABC): return False self.wait_and_maintain( - value_name="Groundspeed", + value_name=field, target=(speed_min+speed_max)/2, - current_value_getter=lambda: get_groundspeed(timeout), + current_value_getter=lambda: get_speed(timeout), accuracy=(speed_max - speed_min)/2, validator=lambda value2, target2: validator(value2, target2), timeout=timeout,