diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 0bb9d067c2..2c624496ba 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -3359,13 +3359,16 @@ class AutoTest(ABC): raise NotAchievedException("Air Temperature not received from HIGH_LATENCY2") self.HIGH_LATENCY2_links() - def context_set_message_rate_hz(self, id, rate_hz): + def context_set_message_rate_hz(self, id, rate_hz, run_cmd=None): + if run_cmd is None: + run_cmd = self.run_cmd + overridden_message_rates = self.context_get().overridden_message_rates if id not in overridden_message_rates: overridden_message_rates[id] = self.measure_message_rate(id) - self.set_message_rate_hz(id, rate_hz) + self.set_message_rate_hz(id, rate_hz, run_cmd=run_cmd) def HIGH_LATENCY2_links(self): @@ -5693,8 +5696,12 @@ class AutoTest(ABC): p6=None, p7=None, quiet=False, + mav=None, ): + if mav is None: + mav = self.mav + if p5 is not None: x = p5 if p6 is not None: @@ -5728,20 +5735,20 @@ class AutoTest(ABC): x, y, z)) - self.mav.mav.command_int_send(target_sysid, - target_compid, - frame, - command, - 0, # current - 0, # autocontinue - p1, - p2, - p3, - p4, - x, - y, - z) - self.run_cmd_get_ack(command, want_result, timeout) + mav.mav.command_int_send(target_sysid, + target_compid, + frame, + command, + 0, # current + 0, # autocontinue + p1, + p2, + p3, + p4, + x, + y, + z) + self.run_cmd_get_ack(command, want_result, timeout, mav=mav) def send_cmd(self, command, @@ -9753,21 +9760,36 @@ Also, ignores heartbeats not from our target system''' def rate_to_interval_us(self, rate): return 1/float(rate)*1000000.0 - def set_message_rate_hz(self, id, rate_hz, mav=None): + def interval_us_to_rate(self, interval): + if interval == 0: + raise ValueError("Zero interval is infinite rate") + return 1000000.0/float(interval) + + def set_message_rate_hz(self, id, rate_hz, mav=None, run_cmd=None): '''set a message rate in Hz; 0 for original, -1 to disable''' + if run_cmd is None: + run_cmd = self.run_cmd if isinstance(id, str): id = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % id) if rate_hz == 0 or rate_hz == -1: set_interval = rate_hz else: set_interval = self.rate_to_interval_us(rate_hz) - self.run_cmd( + run_cmd( mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, p1=id, p2=set_interval, mav=mav, ) + def get_message_rate_hz(self, id, mav=None, run_cmd=None): + '''return rate message is being sent, in Hz''' + if run_cmd is None: + run_cmd = self.run_cmd + + interval = self.get_message_interval(id, mav=mav, run_cmd=run_cmd) + return self.interval_us_to_rate(interval) + def send_get_message_interval(self, victim_message, mav=None): if mav is None: mav = self.mav @@ -9786,12 +9808,19 @@ Also, ignores heartbeats not from our target system''' 0, 0) - def get_message_interval(self, victim_message, mav=None): + def get_message_interval(self, victim_message, mav=None, run_cmd=None): '''returns message interval in microseconds''' + if run_cmd is None: + run_cmd = self.run_cmd + self.send_get_message_interval(victim_message, mav=mav) m = self.assert_receive_message('MESSAGE_INTERVAL', timeout=1, mav=mav) + + if isinstance(victim_message, str): + victim_message = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % victim_message) if m.message_id != victim_message: - raise NotAchievedException("Unexpected ID in MESSAGE_INTERVAL") + raise NotAchievedException(f"Unexpected ID in MESSAGE_INTERVAL (want={victim_message}, got={m.message_id}") + return m.interval_us def set_message_interval(self, victim_message, interval_us, mav=None): @@ -9865,6 +9894,22 @@ Also, ignores heartbeats not from our target system''' self.start_subtest('Many-message tests') self.test_set_message_interval_many() + def MESSAGE_INTERVAL_COMMAND_INT(self): + '''Test MAV_CMD_SET_MESSAGE_INTERVAL works as COMMAND_INT''' + original_rate = round(self.measure_message_rate("VFR_HUD", 20)) + self.context_set_message_rate_hz('VFR_HUD', original_rate*2, run_cmd=self.run_cmd_int) + if abs(original_rate*2 - round(self.get_message_rate_hz("VFR_HUD", run_cmd=self.run_cmd_int))) > 1: + raise NotAchievedException("Did not set rate") + + self.start_subtest("Use REQUEST_MESSAGE via COMMAND_INT") + # 148 is AUTOPILOT_VERSION: + self.context_collect('AUTOPILOT_VERSION') + self.run_cmd_int(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, 148) + self.delay_sim_time(2) + count = len(self.context_collection('AUTOPILOT_VERSION')) + if count != 1: + raise NotAchievedException(f"Did not get single AUTOPILOT_VERSION message (count={count}") + def test_set_message_interval_many(self): messages = [ 'CAMERA_FEEDBACK', diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 080b749c05..12c36f5dce 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -6542,6 +6542,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) self.Gripper, self.GripperMission, self.SET_MESSAGE_INTERVAL, + self.MESSAGE_INTERVAL_COMMAND_INT, self.REQUEST_MESSAGE, self.SYSID_ENFORCE, self.SET_ATTITUDE_TARGET,