From 1ca0998f32f43c0b9a26a0dc653024d7d5eff815 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 4 Jul 2020 09:03:18 +1000 Subject: [PATCH] Tools: take MAV_CMD_DO_SPRAYER as a mavlink command --- Tools/autotest/rover.py | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 5f9aa7d1b9..cdc44d046c 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -310,6 +310,34 @@ class AutoTestRover(AutoTest): self.progress("Waiting for sprayer to stop") self.wait_servo_channel_value(pump_ch, pump_ch_min, timeout=120) + self.start_subtest("Checking mavlink commands") + self.change_mode("MANUAL") + self.progress("Starting Sprayer") + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, + 1, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + 0, # p5 + 0, # p6 + 0, # p7 + ); + self.progress("Testing speed-ramping") + self.set_rc(3, 1700) # start driving forward + self.wait_servo_channel_value(pump_ch, 1690, timeout=60, comparator=operator.gt) + self.start_subtest("Stopping Sprayer") + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + 0, # p5 + 0, # p6 + 0, # p7 + ); + self.wait_servo_channel_value(pump_ch, pump_ch_min) + self.set_rc(3, 1000) # start driving forward + self.progress("Sprayer OK") except Exception as e: self.progress("Caught exception: %s" %