Tools: autotest: better streamrate reset

Tools/autotest/common.py

@@ -287,26 +287,37 @@ class AutoTest(ABC):
 
         self.initialise_after_reboot_sitl()
 
-    def initialise_after_reboot_sitl(self):
-
-        # after reboot stream-rates may be zero.  Prompt MAVProxy to
-        # send a rate-change message by changing away from our normal
-        # stream rates and back again:
-        tstart = self.get_sim_time()
+    def set_streamrate(self, streamrate):
+        tstart = time.time()
         while True:
+            if time.time() - tstart > 10:
+                raise AutoTestTimeoutException("stream rate change failed")
+
+            self.mavproxy.send("set streamrate %u\n" % (streamrate))
+            self.mavproxy.send("set streamrate\n")
+            self.mavproxy.expect('.*streamrate ([0-9]+)', timeout=1)
+            rate = self.mavproxy.match.group(1)
+            print("rate: %s" % str(rate))
+            if int(rate) == int(streamrate):
+                break
 
-            self.mavproxy.send("set streamrate %u\n" % (self.sitl_streamrate()+1))
-
-            if self.get_sim_time() - tstart > 10:
-                raise AutoTestTimeoutException("SYSTEM_TIME not received")
+        if streamrate <= 0:
+            return
 
         m = self.mav.recv_match(type='SYSTEM_TIME',
                                 blocking=True,
-                                    timeout=1)
-            if m is not None:
+                                timeout=10)
         print("Received (%s)" % str(m))
-                break
-        self.mavproxy.send("set streamrate %u\n" % self.sitl_streamrate())
+        if m is None:
+            raise NotAchievedException("Did not get SYSTEM_TIME")
+
+    def initialise_after_reboot_sitl(self):
+
+        # after reboot stream-rates may be zero.  Prompt MAVProxy to
+        # send a rate-change message by changing away from our normal
+        # stream rates and back again:
+        self.set_streamrate(self.sitl_streamrate()+1)
+        self.set_streamrate(self.sitl_streamrate())
         self.progress("Reboot complete")
 
     def close(self):