From e7b49f07cee7eb31c7529978d4174bd9d492dba5 Mon Sep 17 00:00:00 2001 From: Vasily Evseenko Date: Tue, 24 Aug 2021 13:59:45 +0300 Subject: [PATCH] Add ARM/DISARM handler --- telemetry/conf/master.cfg | 6 +++ telemetry/proxy.py | 103 +++++++++++++++++++++++++++++++++++--- telemetry/server.py | 50 ++++++------------ 3 files changed, 116 insertions(+), 43 deletions(-) diff --git a/telemetry/conf/master.cfg b/telemetry/conf/master.cfg index 5b3044b..93bd824 100644 --- a/telemetry/conf/master.cfg +++ b/telemetry/conf/master.cfg @@ -45,6 +45,9 @@ stbc = 1 # stbc streams: 1, 2, 3 or 0 if unused ldpc = 0 # use LDPC FEC. Currently available only for 8812au and must be supported both on TX and RX. mcs_index = 1 # mcs index +call_on_arm = None # call program on arm +call_on_disarm = None # call program on disarm + [gs_video] keypair = 'gs.key' # keypair generated by wfb-keygen @@ -84,6 +87,9 @@ stbc = 1 # stbc streams: 1, 2, 3 or 0 if unused ldpc = 0 # use LDPC FEC. Currently available only for 8812au and must be supported both on TX and RX. mcs_index = 1 # mcs index +call_on_arm = None # call program on arm +call_on_disarm = None # call program on disarm + [drone_video] keypair = 'drone.key' diff --git a/telemetry/proxy.py b/telemetry/proxy.py index eb68540..2bf655a 100644 --- a/telemetry/proxy.py +++ b/telemetry/proxy.py @@ -29,20 +29,54 @@ standard_library.install_aliases() from builtins import * import struct -from . import mavlink +import os + from twisted.python import log -from twisted.internet import reactor, defer +from twisted.internet import reactor, defer, utils from twisted.internet.protocol import DatagramProtocol, Protocol -from telemetry.conf import settings + +from . import mavlink +from .conf import settings +from .mavlink_protocol import MAVLinkProtocol + + +class ExecError(Exception): + pass + + +def call_and_check_rc(cmd, *args): + def _check_rc(_args): + (stdout, stderr, rc) = _args + if rc != 0: + err = ExecError('RC %d: %s %s' % (rc, cmd, ' '.join(args))) + err.stdout = stdout.strip() + err.stderr = stderr.strip() + raise err + + log.msg('# %s' % (' '.join((cmd,) + args),)) + if stdout: + log.msg(stdout) + + def _got_signal(f): + f.trap(tuple) + stdout, stderr, signum = f.value + err = ExecError('Got signal %d: %s %s' % (signum, cmd, ' '.join(args))) + err.stdout = stdout.strip() + err.stderr = stderr.strip() + raise err + + return utils.getProcessOutputAndValue(cmd, args, env=os.environ).addCallbacks(_check_rc, _got_signal) class ProxyProtocol: - def __init__(self, agg_max_size=None, agg_timeout=None, inject_rssi=False): + def __init__(self, agg_max_size=None, agg_timeout=None, inject_rssi=False, arm_proto=None): # use self.write to send mavlink message if inject_rssi: self.radio_mav = mavlink.MAVLink(self, srcSystem=3, srcComponent=242) # WFB else: self.radio_mav = None + + self.arm_proto = arm_proto self.peer = None self.agg_max_size = agg_max_size self.agg_timeout = agg_timeout @@ -73,6 +107,9 @@ class ProxyProtocol: self.peer.write(data) def messageReceived(self, data): + if self.arm_proto: + self.arm_proto.dataReceived(data) + # send message to local transport if self.agg_max_size is None or self.agg_timeout is None: return self._send_to_peer(data) @@ -108,8 +145,8 @@ class ProxyProtocol: class UDPProxyProtocol(DatagramProtocol, ProxyProtocol): noisy = False - def __init__(self, addr=None, agg_max_size=None, agg_timeout=None, inject_rssi=False, mirror=None): - ProxyProtocol.__init__(self, agg_max_size, agg_timeout, inject_rssi) + def __init__(self, addr=None, agg_max_size=None, agg_timeout=None, inject_rssi=False, mirror=None, arm_proto=None): + ProxyProtocol.__init__(self, agg_max_size, agg_timeout, inject_rssi, arm_proto=arm_proto) self.reply_addr = addr self.fixed_addr = bool(addr) self.mirror = mirror @@ -175,8 +212,8 @@ class UDPProxyProtocol(DatagramProtocol, ProxyProtocol): class SerialProxyProtocol(Protocol, ProxyProtocol): noisy = False - def __init__(self, agg_max_size=None, agg_timeout=None, inject_rssi=False): - ProxyProtocol.__init__(self, agg_max_size, agg_timeout, inject_rssi) + def __init__(self, agg_max_size=None, agg_timeout=None, inject_rssi=False, arm_proto=None): + ProxyProtocol.__init__(self, agg_max_size, agg_timeout, inject_rssi, arm_proto=arm_proto) self.mavlink_fsm = self.mavlink_parser() self.mavlink_fsm.send(None) @@ -240,3 +277,53 @@ class SerialProxyProtocol(Protocol, ProxyProtocol): for m in m_list: self.messageReceived(m) + +class ARMProtocol(MAVLinkProtocol): + def __init__(self, call_on_arm, call_on_disarm): + MAVLinkProtocol.__init__(self, None, None) + self.call_on_arm = call_on_arm + self.call_on_disarm = call_on_disarm + self.armed = None + self.locked = False + + def messageReceived(self, message): + if (message._header.msgId, message._header.srcSystem, message._header.srcComponent) != (mavlink.MAVLINK_MSG_ID_HEARTBEAT, 1, 1): + return + + armed = bool(message.base_mode & mavlink.MAV_MODE_FLAG_SAFETY_ARMED) + + if not self.locked: + self.locked = True + + def _unlock(x): + self.locked = False + return x + + return defer.maybeDeferred(self.change_state, armed).addBoth(_unlock) + + def change_state(self, armed): + if armed == self.armed: + return + + self.armed = armed + cmd = None + + if armed: + print('State change: ARMED') + cmd = self.call_on_arm + else: + print('State change: DISARMED') + cmd = self.call_on_disarm + + def on_err(f): + log.msg('Command exec failed: %s' % (f.value,), isError=1) + + if f.value.stdout: + log.msg(f.value.stdout, isError=1) + + if f.value.stderr: + log.msg(f.value.stderr, isError=1) + + if cmd is not None: + return call_and_check_rc(cmd).addErrback(on_err) + diff --git a/telemetry/server.py b/telemetry/server.py index f6b35e8..8c400d2 100644 --- a/telemetry/server.py +++ b/telemetry/server.py @@ -36,48 +36,21 @@ import re from itertools import groupby from twisted.python import log, failure -from twisted.internet import reactor, defer, utils, main as ti_main +from twisted.internet import reactor, defer, main as ti_main from twisted.internet.protocol import ProcessProtocol, DatagramProtocol, Protocol, Factory from twisted.protocols.basic import LineReceiver from twisted.internet.error import ReactorNotRunning from twisted.internet.serialport import SerialPort -from telemetry.common import abort_on_crash, exit_status -from telemetry.proxy import UDPProxyProtocol, SerialProxyProtocol -from telemetry.tuntap import TUNTAPProtocol, TUNTAPTransport -from telemetry.conf import settings +from .common import abort_on_crash, exit_status +from .proxy import UDPProxyProtocol, SerialProxyProtocol, ARMProtocol, call_and_check_rc, ExecError +from .tuntap import TUNTAPProtocol, TUNTAPTransport +from .conf import settings connect_re = re.compile(r'^connect://(?P[0-9]+\.[0-9]+\.[0-9]+\.[0-9]+):(?P[0-9]+)$', re.IGNORECASE) listen_re = re.compile(r'^listen://(?P[0-9]+\.[0-9]+\.[0-9]+\.[0-9]+):(?P[0-9]+)$', re.IGNORECASE) serial_re = re.compile(r'^serial:(?P[a-z0-9\-\_/]+):(?P[0-9]+)$', re.IGNORECASE) -class ExecError(Exception): - pass - - -def call_and_check_rc(cmd, *args): - def _check_rc(_args): - (stdout, stderr, rc) = _args - if rc != 0: - err = ExecError('RC %d: %s %s' % (rc, cmd, ' '.join(args))) - err.stdout = stdout.strip() - err.stderr = stderr.strip() - raise err - - log.msg('# %s' % (' '.join((cmd,) + args),)) - if stdout: - log.msg(stdout) - - def _got_signal(f): - f.trap(tuple) - stdout, stderr, signum = f.value - err = ExecError('Got signal %d: %s %s' % (signum, cmd, ' '.join(args))) - err.stdout = stdout.strip() - err.stderr = stderr.strip() - raise err - - return utils.getProcessOutputAndValue(cmd, args, env=os.environ).addCallbacks(_check_rc, _got_signal) - class BadTelemetry(Exception): pass @@ -365,19 +338,26 @@ def init_mavlink(profile, wlans): mirror = m.group('addr'), int(m.group('port')) log.msg('Mirror telem stream to %s:%d' % (mirror[0], mirror[1])) + if cfg.call_on_arm or cfg.call_on_disarm: + arm_proto = ARMProtocol(cfg.call_on_arm, cfg.call_on_disarm) + else: + arm_proto = None + if serial: p_in = SerialProxyProtocol(agg_max_size=settings.common.radio_mtu, agg_timeout=settings.common.mavlink_agg_timeout, - inject_rssi=cfg.inject_rssi) + inject_rssi=cfg.inject_rssi, + arm_proto=arm_proto) else: # The first argument is not None only if we initiate mavlink connection p_in = UDPProxyProtocol(connect, agg_max_size=settings.common.radio_mtu, agg_timeout=settings.common.mavlink_agg_timeout, inject_rssi=cfg.inject_rssi, - mirror=mirror) + mirror=mirror, + arm_proto=arm_proto) p_tx_l = [UDPProxyProtocol(('127.0.0.1', cfg.port_tx + i)) for i, _ in enumerate(wlans)] - p_rx = UDPProxyProtocol() + p_rx = UDPProxyProtocol(arm_proto=arm_proto) p_rx.peer = p_in if serial: