From a654f9f218fb746e382a6d69dae8dddbea16df97 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 9 Sep 2024 16:38:40 +1000 Subject: [PATCH] Allow mavfwd and mavfwd_disarmed to be set on a per-link basis the link settings - if present - override the global settings --- MAVProxy/mavproxy.py | 15 +++++- MAVProxy/modules/mavproxy_output.py | 78 +++++++++++++++++++++++------ 2 files changed, 78 insertions(+), 15 deletions(-) diff --git a/MAVProxy/mavproxy.py b/MAVProxy/mavproxy.py index 4b8d8e3a69..7f1773d30b 100644 --- a/MAVProxy/mavproxy.py +++ b/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: diff --git a/MAVProxy/modules/mavproxy_output.py b/MAVProxy/modules/mavproxy_output.py index e331e5264f..7303910a6c 100644 --- a/MAVProxy/modules/mavproxy_output.py +++ b/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", - [""]) + self.add_command('output', self.cmd_output, "output control", [ + "", + '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 ") + print("usage: output ") 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'''