autotest: add ability to watch-and-maintain from SIM_STATE.alt

Tools/autotest/arducopter.py

@@ -6894,6 +6894,33 @@ class AutoTestCopter(AutoTest):
         self.change_mode('AUTO')
         self.wait_rtl_complete()
 
+    def WatchAlts(self):
+        '''Ensure we can monitor different altitudes'''
+        self.takeoff(30, mode='GUIDED')
+        self.delay_sim_time(5, reason='let altitude settle')
+
+        self.progress("Testing absolute altitudes")
+        absolute_alt = self.get_altitude(altitude_source='SIM_STATE.alt')
+        self.progress("absolute_alt=%f" % absolute_alt)
+        epsilon = 4  # SIM_STATE and vehicle state can be off by a bit...
+        for source in ['GLOBAL_POSITION_INT.alt', 'SIM_STATE.alt', 'GPS_RAW_INT.alt']:
+            self.watch_altitude_maintained(
+                absolute_alt-epsilon,
+                absolute_alt+epsilon,
+                altitude_source=source
+            )
+
+        self.progress("Testing absolute altitudes")
+        relative_alt = self.get_altitude(relative=True)
+        for source in ['GLOBAL_POSITION_INT.relative_alt']:
+            self.watch_altitude_maintained(
+                relative_alt-epsilon,
+                relative_alt+epsilon,
+                altitude_source=source
+            )
+
+        self.do_RTL()
+
     def fly_rangefinder_drivers_fly(self, rangefinders):
         '''ensure rangefinder gives height-above-ground'''
         self.change_mode('GUIDED')
@@ -9053,6 +9080,7 @@ class AutoTestCopter(AutoTest):
             self.DefaultIntervalsFromFiles,
             self.GPSTypes,
             self.MultipleGPS,
+            self.WatchAlts,
         ])
         return ret
 

Tools/autotest/common.py

@@ -5207,6 +5207,7 @@ class AutoTest(ABC):
                  target_sysid=None,
                  target_compid=None,
                  mav=None,
+                 quiet=False,
                  ):
         """Send a MAVLink command long."""
         if mav is None:
@@ -5219,18 +5220,19 @@ class AutoTest(ABC):
             command_name = mavutil.mavlink.enums["MAV_CMD"][command].name
         except KeyError:
             command_name = "UNKNOWN=%u" % command
-        self.progress("Sending COMMAND_LONG to (%u,%u) (%s) (p1=%f p2=%f p3=%f p4=%f p5=%f p6=%f  p7=%f)" %
+        if not quiet:
-                      (
+            self.progress("Sending COMMAND_LONG to (%u,%u) (%s) (p1=%f p2=%f p3=%f p4=%f p5=%f p6=%f  p7=%f)" %
-                          target_sysid,
+                          (
-                          target_compid,
+                              target_sysid,
-                          command_name,
+                              target_compid,
-                          p1,
+                              command_name,
-                          p2,
+                              p1,
-                          p3,
+                              p2,
-                          p4,
+                              p3,
-                          p5,
+                              p4,
-                          p6,
+                              p5,
-                          p7))
+                              p6,
+                              p7))
         mav.mav.command_long_send(target_sysid,
                                   target_compid,
                                   command,
@@ -5709,12 +5711,19 @@ class AutoTest(ABC):
         while tstart + seconds_to_wait > tnow:
             tnow = self.get_sim_time(drain_mav=False)
 
-    def get_altitude(self, relative=False, timeout=30):
+    def get_altitude(self, relative=False, timeout=30, altitude_source=None):
         '''returns vehicles altitude in metres, possibly relative-to-home'''
-        msg = self.assert_receive_message("GLOBAL_POSITION_INT", timeout=timeout)
+        if altitude_source is None:
-        if relative:
+            if relative:
-            return msg.relative_alt / 1000.0  # mm -> m
+                altitude_source = "GLOBAL_POSITION_INT.relative_alt"
-        return msg.alt / 1000.0  # mm -> m
+            else:
+                altitude_source = "GLOBAL_POSITION_INT.alt"
+        (msg, field) = altitude_source.split('.')
+        msg = self.poll_message(msg, quiet=True)
+        divisor = 1000.0  # mm is pretty common in mavlink
+        if altitude_source == "SIM_STATE.alt":
+            divisor = 1.0
+        return getattr(msg, field) / divisor
 
     def assert_altitude(self, alt, accuracy=1, **kwargs):
         got_alt = self.get_altitude(**kwargs)
@@ -5827,12 +5836,15 @@ class AutoTest(ABC):
             else:
                 return False
 
+        altitude_source = kwargs.get("altitude_source", None)
+
         self.wait_and_maintain(
             value_name="Altitude",
             target=altitude_min,
             current_value_getter=lambda: self.get_altitude(
                 relative=relative,
                 timeout=timeout,
+                altitude_source=altitude_source,
             ),
             accuracy=(altitude_max - altitude_min),
             validator=lambda value2, target2: validator(value2, target2),
@@ -5840,13 +5852,16 @@ class AutoTest(ABC):
             **kwargs
         )
 
-    def watch_altitude_maintained(self, altitude_min, altitude_max, minimum_duration=5, relative=True):
+    def watch_altitude_maintained(self, altitude_min, altitude_max, minimum_duration=5, relative=True, altitude_source=None):
         """Watch altitude is maintained or not between altitude_min and altitude_max during minimum_duration"""
-        return self.wait_altitude(altitude_min=altitude_min,
+        return self.wait_altitude(
-                                  altitude_max=altitude_max,
+            altitude_min=altitude_min,
-                                  relative=relative,
+            altitude_max=altitude_max,
-                                  minimum_duration=minimum_duration,
+            relative=relative,
-                                  timeout=minimum_duration + 1)
+            minimum_duration=minimum_duration,
+            timeout=minimum_duration + 1,
+            altitude_source=altitude_source,
+        )
 
     def wait_climbrate(self, speed_min, speed_max, timeout=30, **kwargs):
         """Wait for a given vertical rate."""
@@ -6041,6 +6056,7 @@ class AutoTest(ABC):
         self.validate_kwargs(kwargs, valid=frozenset([
             "called_function",
             "minimum_duration",
+            "altitude_source",
         ]))
 
         if print_diagnostics_as_target_not_range:
@@ -9284,7 +9300,7 @@ Also, ignores heartbeats not from our target system'''
         if ex is not None:
             raise ex
 
-    def send_poll_message(self, message_id, target_sysid=None, target_compid=None):
+    def send_poll_message(self, message_id, target_sysid=None, target_compid=None, quiet=False):
         if type(message_id) == str:
             message_id = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % message_id)
         self.send_cmd(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE,
@@ -9296,18 +9312,19 @@ Also, ignores heartbeats not from our target system'''
                       0,
                       0,
                       target_sysid=target_sysid,
-                      target_compid=target_compid)
+                      target_compid=target_compid,
+                      quiet=quiet)
 
-    def poll_message(self, message_id, timeout=10):
+    def poll_message(self, message_id, timeout=10, quiet=False):
         if type(message_id) == str:
             message_id = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % message_id)
         tstart = self.get_sim_time() # required for timeout in run_cmd_get_ack to work
-        self.send_poll_message(message_id)
+        self.send_poll_message(message_id, quiet=quiet)
         self.run_cmd_get_ack(
             mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE,
             mavutil.mavlink.MAV_RESULT_ACCEPTED,
             timeout,
-            quiet=False
+            quiet=quiet,
         )
         while True:
             if self.get_sim_time_cached() - tstart > timeout: