mavproxy_param: handle param set requests asynchronously

MAVProxy/modules/mavproxy_param.py

@@ -28,6 +28,13 @@
     # py3
     from io import BytesIO as SIO
 
+try:
+    import queue as Queue
+    from queue import Empty
+except ImportError:
+    import Queue
+    from Queue import Empty
+
 
 class ParamState:
     '''this class is separated to make it possible to use the parameter
@@ -56,6 +63,141 @@ def __init__(self, mav_param, logdir, vehicle_name, parm_file, mpstate, sysid):
         self.default_params = None
         self.watch_patterns = set()
 
+        # dictionary of ParamSet objects we are processing:
+        self.parameters_to_set = {}
+        # a Queue which onto which ParamSet objects can be pushed in a
+        # thread-safe manner:
+        self.parameters_to_set_input_queue = Queue.Queue()
+
+    class ParamSet():
+        '''class to hold information about a parameter set being attempted'''
+        def __init__(self, master, name, value, param_type=None, attempts=None):
+            self.master = master
+            self.name = name
+            self.value = value
+            self.param_type = param_type
+            self.attempts_remaining = attempts
+            self.retry_interval = 1  # seconds
+            self.last_value_received = None
+
+            if self.attempts_remaining is None:
+                self.attempts_remaining = 3
+
+            self.request_sent = 0  # this is a timestamp
+
+        def normalize_parameter_for_param_set_send(self, name, value, param_type):
+            '''uses param_type to convert value into a value suitable for passing
+            into the mavlink param_set_send binding.  Note that this
+            is a copy of a method in pymavlink, in case the user has
+            an older version of that library.
+            '''
+            if param_type is not None and param_type != mavutil.mavlink.MAV_PARAM_TYPE_REAL32:
+                # need to encode as a float for sending
+                if param_type == mavutil.mavlink.MAV_PARAM_TYPE_UINT8:
+                    vstr = struct.pack(">xxxB", int(value))
+                elif param_type == mavutil.mavlink.MAV_PARAM_TYPE_INT8:
+                    vstr = struct.pack(">xxxb", int(value))
+                elif param_type == mavutil.mavlink.MAV_PARAM_TYPE_UINT16:
+                    vstr = struct.pack(">xxH", int(value))
+                elif param_type == mavutil.mavlink.MAV_PARAM_TYPE_INT16:
+                    vstr = struct.pack(">xxh", int(value))
+                elif param_type == mavutil.mavlink.MAV_PARAM_TYPE_UINT32:
+                    vstr = struct.pack(">I", int(value))
+                elif param_type == mavutil.mavlink.MAV_PARAM_TYPE_INT32:
+                    vstr = struct.pack(">i", int(value))
+                else:
+                    print("can't send %s of type %u" % (name, param_type))
+                    return None
+                numeric_value, = struct.unpack(">f", vstr)
+            else:
+                if isinstance(value, str) and value.lower().startswith('0x'):
+                    numeric_value = int(value[2:], 16)
+                else:
+                    try:
+                        numeric_value = float(value)
+                    except ValueError:
+                        print(f"can't convert {name} ({value}, {type(value)}) to float")
+                        return None
+
+            return numeric_value
+
+        def send_set(self):
+            numeric_value = self.normalize_parameter_for_param_set_send(self.name, self.value, self.param_type)
+            if numeric_value is None:
+                print(f"can't send {self.name} of type {self.param_type}")
+                self.attempts_remaining = 0
+                return
+            # print(f"Sending set attempts-remaining={self.attempts_remaining}")
+            self.master.param_set_send(
+                self.name.upper(),
+                numeric_value,
+                parm_type=self.param_type,
+            )
+            self.request_sent = time.time()
+            self.attempts_remaining -= 1
+
+        def expired(self):
+            if self.attempts_remaining > 0:
+                return False
+            return time.time() - self.request_sent > self.retry_interval
+
+        def due_for_retry(self):
+            if self.attempts_remaining <= 0:
+                return False
+            return time.time() - self.request_sent > self.retry_interval
+
+        def handle_PARAM_VALUE(self, m, value):
+            '''handle PARAM_VALUE packet m which has already been checked for a
+            match against self.name.  Returns true if this Set is now
+            satisfied.  value is the value extracted and potentially
+            manipulated from the packet
+            '''
+            self.last_value_received = value
+            if abs(value - float(self.value)) > 0.00001:
+                return False
+
+            return True
+
+        def print_expired_message(self):
+            reason = ""
+            if self.last_value_received is None:
+                reason = " (no PARAM_VALUE received)"
+            else:
+                reason = f" (invalid returned value {self.last_value_received})"
+            print(f"Failed to set {self.name} to {self.value}{reason}")
+
+    def run_parameter_set_queue(self):
+        # firstly move anything from the input queue into our
+        # collection of things to send:
+        try:
+            while True:
+                new_parameter_to_set = self.parameters_to_set_input_queue.get(block=False)
+                self.parameters_to_set[new_parameter_to_set.name] = new_parameter_to_set
+        except Empty:
+            pass
+
+        # now send any parameter-sets which are due to be sent out,
+        # either because they are new or because we need to retry:
+        count = 0
+        keys_to_remove = []  # remove entries after iterating the dict
+        for (key, parameter_to_set) in self.parameters_to_set.items():
+            if parameter_to_set.expired():
+                parameter_to_set.print_expired_message()
+                keys_to_remove.append(key)
+                continue
+            if not parameter_to_set.due_for_retry():
+                continue
+            # send parameter set:
+            parameter_to_set.send_set()
+            # rate-limit to 10 items per call:
+            count += 1
+            if count > 10:
+                break
+
+        # complete purging of expired parameter-sets:
+        for key in keys_to_remove:
+            del self.parameters_to_set[key]
+
     def use_ftp(self):
         '''return true if we should try ftp for download'''
         if self.ftp_failed:
@@ -132,6 +274,16 @@ def handle_mavlink_packet(self, master, m):
                 self.fetch_set = None
             if self.fetch_set is not None and len(self.fetch_set) == 0:
                 self.fetch_check(master, force=True)
+
+            # if we were setting this parameter then check it's the
+            # value we want and, if so, stop setting the parameter
+            try:
+                if self.parameters_to_set[param_id].handle_PARAM_VALUE(m, value):
+                    # print(f"removing set of param_id ({self.parameters_to_set[param_id].value} vs {value})")
+                    del self.parameters_to_set[param_id]
+            except KeyError:
+                pass
+
         elif m.get_type() == 'HEARTBEAT':
             if m.get_srcComponent() == 1:
                 # remember autopilot types so we can handle PX4 parameters
@@ -493,7 +645,21 @@ def param_bitmask_modify(self, master, args):
             return
 
         # Update the parameter
-        self.mav_param.mavset(master, uname, value, retries=3, parm_type=ptype)
+        self.set_parameter(master, uname, value, attempts=3, parm_type=ptype)
+
+    def set_parameter(self, master, name, value, attempts=None, param_type=None):
+        '''convenient intermediate method which determines parameter type for
+        lazy callers'''
+        if param_type is None:
+            param_type = self.param_types.get(name, None)
+
+        self.parameters_to_set_input_queue.put(ParamState.ParamSet(
+            master,
+            name,
+            value,
+            attempts=attempts,
+            param_type=param_type,
+        ))
 
     def param_revert(self, master, args):
         '''handle param revert'''
@@ -519,10 +685,7 @@ def param_revert(self, master, args):
             if s1 == s2:
                 continue
             print("Reverting %-16.16s  %s -> %s" % (p, s1, s2))
-            ptype = None
-            if p in self.param_types:
-                ptype = self.param_types[p]
-            self.mav_param.mavset(master, p, defaults[p], retries=3, parm_type=ptype)
+            self.set_parameter(master, p, defaults[p], attempts=3)
             count += 1
         print("Reverted %u parameters" % count)
 
@@ -596,10 +759,7 @@ def handle_command(self, master, mpstate, args):
                 print("Unable to find parameter '%s'" % param)
                 return
             uname = param.upper()
-            ptype = None
-            if uname in self.param_types:
-                ptype = self.param_types[uname]
-            self.mav_param.mavset(master, uname, value, retries=3, parm_type=ptype)
+            self.set_parameter(master, uname, value, attempts=3)
 
             if (param.upper() == "WP_LOITER_RAD" or param.upper() == "LAND_BREAK_PATH"):
                 # need to redraw rally points
@@ -881,6 +1041,12 @@ def idle_task(self):
         else:
             self.menu_added_console = False
 
+        self.run_parameter_set_queues()
+
+    def run_parameter_set_queues(self):
+        for pstate in self.pstate.values():
+            pstate.run_parameter_set_queue()
+
     def cmd_param(self, args):
         '''control parameters'''
         self.check_new_target_system()