Tools: handle message interval commands as both long and int

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',

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,