Allow mavfwd and mavfwd_disarmed to be set on a per-link basis

MAVProxy/mavproxy.py

@@ -872,8 +872,21 @@ def process_mavlink(slave):
     if msgs is None:
         return
     allow_fwd = mpstate.settings.mavfwd
-    if not allow_fwd and mpstate.settings.mavfwd_disarmed and not mpstate.master(-1).motors_armed():
+    is_armed = mpstate.master(-1).motors_armed()
+    if not allow_fwd and mpstate.settings.mavfwd_disarmed and not is_armed:
         allow_fwd = True
+
+    # check this specific link to see if it has a more-specific
+    # setting for denying (or allowing!) forwarding.
+    if hasattr(slave, "mavproxy_attributes"):
+        link_allows_mavfwd = getattr(slave.mavproxy_attributes, "mavproxy_mavfwd", None)
+        if link_allows_mavfwd is not None:
+            allow_fwd = link_allows_mavfwd
+        if not allow_fwd and not is_armed:
+            link_allows_mavfwd_disarmed = getattr(slave.mavproxy_attributes, "mavproxy_mavfwd_disarmed", None)
+            if link_allows_mavfwd_disarmed is not None:
+                allow_fwd = link_allows_mavfwd_disarmed
+
     if mpstate.status.setup_mode:
         allow_fwd = False
     if allow_fwd:

MAVProxy/modules/mavproxy_output.py

@@ -15,8 +15,11 @@
 class OutputModule(mp_module.MPModule):
     def __init__(self, mpstate):
         super(OutputModule, self).__init__(mpstate, "output", "output control", public=True)
-        self.add_command('output', self.cmd_output, "output control",
-                         ["<list|add|remove|sysid>"])
+        self.add_command('output', self.cmd_output, "output control", [
+            "<list|add|remove|sysid|mavfwd|mavfwd_disarmed>",
+            'set_mavfwd (OUTPUT) <1|0>',
+            'set_mavfwd_disarmed (OUTPUT) <1|0>',
+        ])
 
     def cmd_output(self, args):
         '''handle output commands'''
@@ -37,8 +40,12 @@ def cmd_output(self, args):
                 print("Usage: output sysid SYSID OUTPUT")
                 return
             self.cmd_output_sysid(args[1:])
+        elif args[0] == "set_mavfwd":
+            self.cmd_set_mavfwd(args[1:])
+        elif args[0] == "set_mavfwd_disarmed":
+            self.cmd_set_mavfwd_disarmed(args[1:])
         else:
-            print("usage: output <list|add|remove|sysid>")
+            print("usage: output <list|add|remove|sysid|set_mavfwd|set_mavfwd_disarmed>")
 
     def cmd_output_list(self):
         '''list outputs'''
@@ -52,6 +59,9 @@ def cmd_output_list(self):
                 conn = self.mpstate.sysid_outputs[sysid]
                 print("%u: %s" % (sysid, conn.address))
 
+    class ConnectionAttributes():
+        pass
+
     def cmd_output_add(self, args):
         '''add new output'''
         device = args[0]
@@ -67,6 +77,7 @@ def cmd_output_add(self, args):
             mp_util.child_fd_list_add(conn.port.fileno())
         except Exception:
             pass
+        conn.mavproxy_attributes = OutputModule.ConnectionAttributes()
 
     def cmd_output_sysid(self, args):
         '''add new output for a specific MAVLink sysID'''
@@ -87,20 +98,59 @@ def cmd_output_sysid(self, args):
             self.mpstate.sysid_outputs[sysid].close()
         self.mpstate.sysid_outputs[sysid] = conn
 
-    def cmd_output_remove(self, args):
-        '''remove an output'''
-        device = args[0]
+    def find_output(self, device):
         for i in range(len(self.mpstate.mav_outputs)):
             conn = self.mpstate.mav_outputs[i]
             if str(i) == device or conn.address == device:
-                print("Removing output %s" % conn.address)
-                try:
-                    mp_util.child_fd_list_add(conn.port.fileno())
-                except Exception:
-                    pass
-                conn.close()
-                self.mpstate.mav_outputs.pop(i)
-                return
+                return conn, i
+        return None, None
+
+    def cmd_output_remove(self, args):
+        '''remove an output'''
+        device = args[0]
+        conn, i = self.find_output(device)
+        if conn is None:
+            return
+
+        print("Removing output %s" % conn.address)
+        try:
+            mp_util.child_fd_list_add(conn.port.fileno())
+        except Exception:
+            pass
+        conn.close()
+        self.mpstate.mav_outputs.pop(i)
+
+    def cmd_set_mavfwd(self, args):
+        self.set_mavfwd_attribute('set_mavfwd', 'mavproxy_mavfwd', args)
+
+    def cmd_set_mavfwd_disarmed(self, args):
+        self.set_mavfwd_attribute('set_mavfwd_attribute', 'mavproxy_mavfwd_disarmed', args)
+
+    def set_conn_attribute(self, conn, attribute, value):
+        if not hasattr(conn, "mavproxy_attributes"):
+            conn.mavproxy_attributes = OutputModule.ConnectionAttributes()
+        setattr(conn.mavproxy_attributes, attribute, bool(value))
+
+    def set_mavfwd_attribute(self, cmd, attribute, args):
+        if len(args) != 2:
+            print("Usage: output %s OUTPUT <0|1>" % cmd)
+            return
+        (device, value) = args
+        if str(value) not in frozenset(["0", "1"]):
+            print("Usage: output %s OUTPUT <0|1>" % cmd)
+            return
+        output, _ = self.find_output(device)
+        if output is None:
+            print("Bad OUTPUT")
+            return
+
+        # same conversions as in mp_settings.py:
+        if str(value).lower() in ['1', 'true', 'yes']:
+            value = True
+        elif str(value).lower() in ['0', 'false', 'no']:
+            value = False
+
+        self.set_conn_attribute(output, attribute, bool(value))
 
     def idle_task(self):
         '''called on idle'''