Add ARM/DISARM handler

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'

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):
+    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)
+        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):
+    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)
+        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)
+

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 .common import abort_on_crash, exit_status
-from telemetry.proxy import UDPProxyProtocol, SerialProxyProtocol
+from .proxy import UDPProxyProtocol, SerialProxyProtocol, ARMProtocol, call_and_check_rc, ExecError
-from telemetry.tuntap import TUNTAPProtocol, TUNTAPTransport
+from .tuntap import TUNTAPProtocol, TUNTAPTransport
-from telemetry.conf import settings
+from .conf import settings
 
 connect_re = re.compile(r'^connect://(?P<addr>[0-9]+\.[0-9]+\.[0-9]+\.[0-9]+):(?P<port>[0-9]+)$', re.IGNORECASE)
 listen_re = re.compile(r'^listen://(?P<addr>[0-9]+\.[0-9]+\.[0-9]+\.[0-9]+):(?P<port>[0-9]+)$', re.IGNORECASE)
 serial_re = re.compile(r'^serial:(?P<dev>[a-z0-9\-\_/]+):(?P<baud>[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: