Add storm32 controller

scripts/storm32.service

@@ -0,0 +1,13 @@
+[Unit]
+Description=WFB storm32 controller
+After=network.target
+
+[Service]
+Type=simple
+ExecStart=/usr/bin/python -m telemetry.storm32_controller
+TimeoutStopSec=5s
+Restart=on-failure
+RestartSec=5s
+
+[Install]
+WantedBy=multi-user.target

setup.py

@@ -69,7 +69,8 @@ setup(
                                       'wfb-test-latency=telemetry.latency_test:main']},
     package_data={'telemetry.conf': ['master.cfg', 'site.cfg']},
     data_files = [('/usr/bin', ['wfb_tx', 'wfb_rx', 'wfb_keygen']),
-                  ('/lib/systemd/system', ['scripts/wifibroadcast.service',
+                  ('/lib/systemd/system', ['scripts/storm32.service',
+                                           'scripts/wifibroadcast.service',
                                            'scripts/wifibroadcast@.service']),
                   ('/etc/default', ['scripts/default/wifibroadcast'])],
 

telemetry/mavlink_protocol.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+# Copyright (C) 2018, 2019 Vasily Evseenko <svpcom@p2ptech.org>
+
+#
+#   This program is free software; you can redistribute it and/or modify
+#   it under the terms of the GNU General Public License as published by
+#   the Free Software Foundation; version 3.
+#
+#   This program is distributed in the hope that it will be useful,
+#   but WITHOUT ANY WARRANTY; without even the implied warranty of
+#   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#   GNU General Public License for more details.
+#
+#   You should have received a copy of the GNU General Public License along
+#   with this program; if not, write to the Free Software Foundation, Inc.,
+#   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+#
+
+from __future__ import absolute_import
+from __future__ import unicode_literals
+from __future__ import print_function
+from __future__ import division
+
+from future import standard_library
+standard_library.install_aliases()
+
+from builtins import *
+
+from . import mavlink
+from twisted.python import log
+from twisted.internet import reactor, defer
+from twisted.internet.protocol import Protocol, DatagramProtocol
+
+
+class MAVLinkProtocol(Protocol):
+    def __init__(self, src_system, src_component):
+        self.mav = mavlink.MAVLink(self, srcSystem=src_system, srcComponent=src_component)
+
+    def write(self, msg):
+        raise NotImplementedError
+
+    def messageReceived(self, message):
+        raise NotImplementedError
+
+    def dataReceived(self, data):
+        try:
+            m_list = self.mav.parse_buffer(data)
+        except mavlink.MAVError as e:
+            log.msg('Mavlink error: %s' % (e,))
+            return
+
+        if m_list is not None:
+            for m in m_list:
+                self.messageReceived(m)
+
+
+class MAVLinkUDPProtocol(MAVLinkProtocol, DatagramProtocol):
+    def __init__(self, src_system, src_component, peer=None):
+        MAVLinkProtocol.__init__(self, src_system, src_component)
+        self.reply_addr = peer
+
+    def write(self, msg):
+        if self.transport is not None and self.reply_addr is not None:
+            self.transport.write(msg, self.reply_addr)
+
+    def datagramReceived(self, data, addr):
+        self.reply_addr = addr
+        self.dataReceived(data)
+
+
+class MAVLinkSerialProtocol(MAVLinkProtocol):
+    def write(self, msg):
+        if self.transport is not None:
+            self.transport.write(msg)

telemetry/storm32_controller.py

@@ -0,0 +1,106 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+from __future__ import absolute_import
+from __future__ import unicode_literals
+from __future__ import print_function
+from __future__ import division
+
+from future import standard_library
+standard_library.install_aliases()
+
+from builtins import *
+
+import sys
+import time
+import os
+import struct
+import random
+from math import fabs
+
+from twisted.python import log
+from twisted.internet import reactor, defer, utils
+from twisted.internet.protocol import ReconnectingClientFactory
+from twisted.protocols.basic import LineReceiver
+from twisted.internet.error import ReactorNotRunning
+
+from telemetry.common import abort_on_crash, exit_status
+from telemetry.proxy import UDPProxyProtocol
+
+from . import mavlink
+from .mavlink_protocol import MAVLinkSerialProtocol
+from telemetry.conf import settings
+
+
+class ST32Message(mavlink.MAVLink_command_long_message):
+    fieldnames = ['target_system', 'target_component', 'roll', 'pitch', 'yaw']
+
+    def __init__(self, target_system, target_component, pitch, roll, yaw):
+        """ Pitch, roll, yaw in degrees """
+        mavlink.MAVLink_message.__init__(self, ST32Message.id, ST32Message.name)
+        self._fieldnames = ST32Message.fieldnames
+        self.target_system = target_system
+        self.target_component = target_component
+        self.roll = roll
+        self.pitch = pitch
+        self.yaw = yaw
+
+    def pack(self, mav, force_mavlink1=False):
+        hdr = b'\xfa\x0e\x11' + struct.pack('<fffBB', self.pitch, self.roll, self.yaw, 7, 0)
+        msg = [ hdr, b'\0' * ( 28 - len(hdr)), struct.pack('<HBBB', 1235, self.target_system, self.target_component, 0)]
+        return mavlink.MAVLink_message.pack(self, mav, self.crc_extra, b''.join(msg), force_mavlink1=True)
+
+
+class ST32Protocol(MAVLinkSerialProtocol):
+    def messageReceived(self, message):
+        if message.id != mavlink.MAVLINK_MSG_ID_RC_CHANNELS:
+            return
+
+        pitch = (message.chan6_raw - 1500.0) / 500.0 * 90.0
+        yaw = (message.chan7_raw - 1500.0) / 500.0 * 90.0
+        pitch = max(min(pitch, 90.0), -90.0)
+        yaw = max(min(yaw, 90.0), -90.0)
+        is_fpv = message.chan8_raw < 1500
+
+        if is_fpv:
+            pitch = 0.0
+            yaw = 0.0
+
+        # Ignore changes less than one degree
+        if self.pitch is None or fabs(pitch - self.pitch) > 1:
+            self.pitch = pitch
+
+        if self.yaw is None or fabs(yaw - self.yaw) > 1:
+            self.yaw = yaw
+
+        self.mav.send(ST32Message(1, 154, self.pitch, 0, self.yaw))
+
+    def connectionMade(self):
+        self.pitch = None
+        self.yaw = None
+        print('connection made')
+
+
+class MAVLinkClientFactory(ReconnectingClientFactory):
+    maxDelay = 1
+    sysid = 1
+    compid = mavlink.MAV_COMP_ID_PERIPHERAL
+
+    def buildProtocol(self, addr):
+        p = ST32Protocol(self.sysid, self.compid)
+        p.factory = self
+        return p
+
+
+def main():
+    log.startLogging(sys.stdout)
+    reactor.connectTCP('127.0.0.1', 5760, MAVLinkClientFactory())
+    reactor.run()
+
+    rc = exit_status()
+    log.msg('Exiting with code %d' % rc)
+    sys.exit(rc)
+
+
+if __name__ == '__main__':
+    main()