From a0e5f4fa8a724ac57b3a2be968dfd293e861b351 Mon Sep 17 00:00:00 2001 From: Vasily Evseenko Date: Thu, 23 May 2019 18:00:36 +0300 Subject: [PATCH] Add storm32 controller --- scripts/storm32.service | 13 ++++ setup.py | 3 +- telemetry/mavlink_protocol.py | 76 +++++++++++++++++++++++ telemetry/storm32_controller.py | 106 ++++++++++++++++++++++++++++++++ 4 files changed, 197 insertions(+), 1 deletion(-) create mode 100644 scripts/storm32.service create mode 100644 telemetry/mavlink_protocol.py create mode 100644 telemetry/storm32_controller.py diff --git a/scripts/storm32.service b/scripts/storm32.service new file mode 100644 index 0000000..88341ce --- /dev/null +++ b/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 \ No newline at end of file diff --git a/setup.py b/setup.py index cb837da..720d9dc 100644 --- a/setup.py +++ b/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'])], diff --git a/telemetry/mavlink_protocol.py b/telemetry/mavlink_protocol.py new file mode 100644 index 0000000..482d693 --- /dev/null +++ b/telemetry/mavlink_protocol.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright (C) 2018, 2019 Vasily Evseenko + +# +# 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) diff --git a/telemetry/storm32_controller.py b/telemetry/storm32_controller.py new file mode 100644 index 0000000..a743930 --- /dev/null +++ b/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(' 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()