autotest: implement a more efficient set_parameters, implement set_parameter in terms of it

Tools/autotest/common.py

@@ -2165,7 +2165,8 @@ class AutoTest(ABC):
 
         self.progress("Drained %u messages from mav (%s)" % (count, rate), send_statustext=False)
 
-    def do_timesync_roundtrip(self):
+    def do_timesync_roundtrip(self, quiet=False):
+        if not quiet:
             self.progress("Doing timesync roundtrip")
         tstart = self.get_sim_time()
         self.mav.mav.timesync_send(0, self.timesync_number * 1000 + self.mav.source_system)
@@ -2174,6 +2175,7 @@ class AutoTest(ABC):
             if now - tstart > 1:
                 raise AutoTestTimeoutException("Did not get timesync response")
             m = self.mav.recv_match(type='TIMESYNC', blocking=True, timeout=1)
+            if not quiet:
                 self.progress("Received: %s" % str(m))
             if m is None:
                 continue
@@ -2193,6 +2195,7 @@ class AutoTest(ABC):
 #            if m.get_srcComponent() != self.mav.target_component:
 #                self.progress("response from component other than our target (got=%u want=%u)" % (m.get_srcComponent(), self.mav.target_component))
 #                continue
+            if not quiet:
                 self.progress("Received TIMESYNC response after %fs" % (now - tstart))
             self.timesync_number += 1
             break
@@ -3506,36 +3509,103 @@ class AutoTest(ABC):
             return self.send_set_parameter_mavproxy(name, value)
         return self.send_set_parameter_direct(name, value)
 
-    def set_parameter(self, name, value, add_to_context=True, epsilon=0.0002, retries=10, verbose=True):
+    def set_parameter(self, name, value, **kwargs):
+        self.set_parameters({name: value }, **kwargs)
+
+
+    def set_parameters(self, parameters, add_to_context=True, epsilon=0.000012, retries=None, verbose=True):
         """Set parameters from vehicle."""
-        if verbose:
-            self.progress("Setting %s to %f" % (name, value))
-        old_value = self.get_parameter(name, attempts=2)
-        for i in range(1, retries+2):
-            self.send_set_parameter(name, value)
-            # ArduPilot instantly volunteers the new value:
-            m = self.mav.recv_match(type='PARAM_VALUE', blocking=True, timeout=5)
-            if verbose:
-                self.progress("set_parameter(%s): %s" % (name, str(m), ))
-            if m is None:
-                raise NotAchievedException("Did not receive volunteered parameter %s" % str(name))
-            returned_value = m.param_value
-            delta = float(value) - returned_value
-            if abs(delta) < epsilon:
-                # yes, near-enough-to-equal.
+        want = copy.copy(parameters)
+        self.progress("set_parameters: (%s)" % str(want))
+        self.drain_mav_unparsed()
+        if len(want) == 0:
+            return
+
+        if retries is None:
+            # we can easily fill ArduPilot's param-set/param-get queue
+            # which is quite short.  So we retry *a lot*.
+            retries = (len(want)+1) * 5
+
+        param_value_messages = []
+        def add_param_value(mav, m):
+            t = m.get_type()
+            if t != "PARAM_VALUE":
+                return
+            param_value_messages.append(m)
+
+        self.install_message_hook(add_param_value)
+
+        original_values = {}
+        autopilot_values = {}
+        tstart = self.get_sim_time()
+        for i in range(retries):
+            self.drain_mav(quiet=True)
+            received = set()
+            for (name, value) in want.items():
+#                self.progress("%s want=%f autopilot=%s" % (name, value, autopilot_values.get(name, 'None')))
+                if name not in autopilot_values:
+                    self.send_get_parameter_direct(name)
+#                    self.progress("Requesting (%s) (retry=%u)" % (name, i))
+                    continue
+                delta = abs(autopilot_values[name] - value)
+                if delta < epsilon:
+                    # correct value
+                    self.progress("%s is now %f" % (name, autopilot_values[name]))
                     if add_to_context:
                         context_param_name_list = [p[0] for p in self.context_get().parameters]
                         if name.upper() not in context_param_name_list:
-                        self.context_get().parameters.append((name, old_value))
-                if self.should_fetch_all_for_parameter_change(name.upper()) and value != 0:
-                    self.fetch_parameters()
+                            self.context_get().parameters.append((name, original_values[name]))
+                    received.add(name)
+                    continue
+                self.progress("Sending set (%s) to (%f) (old=%f)" % (name, value, original_values[name]))
+                self.send_set_parameter_direct(name, value)
+            for name in received:
+                del want[name]
+            if len(want):
+                # problem here is that a reboot can happen after we
+                # send the request but before we receive the reply:
+                try:
+                    self.do_timesync_roundtrip(quiet=True)
+                except AutoTestTimeoutException:
+                    pass
+                    # now = self.get_sim_time_cached()
+                    # if tstart > now:
+                    #     self.progress("Time wrap detected")
+                    # else:
+                    #     raise
+            do_fetch_all = False
+            for m in param_value_messages:
+                if m.param_id in want:
+                    self.progress("Received wanted PARAM_VALUE %s=%f" %
+                                  (str(m.param_id), m.param_value))
+                    autopilot_values[m.param_id] = m.param_value
+                    if m.param_id not in original_values:
+                        original_values[m.param_id] = m.param_value;
+                        if (self.should_fetch_all_for_parameter_change(m.param_id.upper()) and
+                            m.param_value != 0):
+                            do_fetch_all = True
+            param_value_messages = []
+#            if do_fetch_all:
+#                self.do_fetch_all()
+
+        self.remove_message_hook(add_param_value)
+
+        if len(want) == 0:
             return
-        raise ValueError("Param fetch returned incorrect value for (%s): (%s) vs (%s)"
-                         % (name, returned_value, value))
+        raise ValueError("Failed to set parameters (%s)" % want)
 
     def get_parameter(self, *args, **kwargs):
         return self.get_parameter_direct(*args, **kwargs)
 
+    def send_get_parameter_direct(self, name):
+        encname = name
+        if sys.version_info.major >= 3 and type(encname) != bytes:
+            encname = bytes(encname, 'ascii')
+        self.mav.mav.param_request_read_send(self.sysid_thismav(),
+                                             1,
+                                             encname,
+                                             -1)
+
     def get_parameter_direct(self, name, attempts=1, timeout=60, verbose=True):
         while attempts > 0:
             if verbose:
@@ -3543,14 +3613,8 @@ class AutoTest(ABC):
             # we MUST parse here or collections fail where we need
             # them to work!
             self.drain_mav(quiet=True)
-            tstart = self.get_sim_time(timeout=timeout)
-            encname = name
-            if sys.version_info.major >= 3 and type(encname) != bytes:
-                encname = bytes(encname, 'ascii')
-            self.mav.mav.param_request_read_send(self.sysid_thismav(),
-                                                 1,
-                                                 encname,
-                                                 -1)
+            tstart = self.get_sim_time()
+            self.send_get_parameter_direct(name)
             while True:
                 now = self.get_sim_time_cached()
                 if tstart > now:
@@ -3602,7 +3666,6 @@ class AutoTest(ABC):
         def mh(mav, m):
             t = m.get_type()
             if t in context.collections:
-                print("m=%s" % str(m))
                 context.collections[t].append(m)
         self.install_message_hook_context(mh)
 
@@ -3632,13 +3695,10 @@ class AutoTest(ABC):
     def context_pop(self):
         """Set parameters to origin values in reverse order."""
         dead = self.contexts.pop()
-        dead_parameters = dead.parameters
-        dead_parameters.reverse()
-        for p in dead_parameters:
-            (name, old_value) = p
-            self.set_parameter(name,
-                               old_value,
-                               add_to_context=False)
+        dead_parameters_dict = {}
+        for p in dead.parameters:
+            dead_parameters_dict[p[0]] = p[1]
+        self.set_parameters(dead_parameters_dict, add_to_context=False)
         for hook in dead.message_hooks:
             self.remove_message_hook(hook)
 
@@ -5348,11 +5408,6 @@ Also, ignores heartbeats not from our target system'''
             self.disarm_vehicle(force=True)
         self.reboot_sitl()
 
-    def set_parameters(self, parameters):
-        '''set parameters from the supplied dict'''
-        for (name, value) in parameters.items():
-            self.set_parameter(name, value)
-
     def zero_mag_offset_parameters(self, compass_count=3):
         self.progress("Zeroing Mag OFS parameters")
         self.drain_mav()