From c365bbd2ff8182f347664f4bdca543083a2318b9 Mon Sep 17 00:00:00 2001 From: Michal Podhradsky Date: Sat, 8 Oct 2016 00:51:40 -0700 Subject: [PATCH] Added sbus fakerator for simulating SBUS transmitters on your laptop. (#1890) Useful for HITL testing. --- sw/tools/sbus_fakerator/README.md | 34 ++++ sw/tools/sbus_fakerator/lib/__init__.py | 22 +++ sw/tools/sbus_fakerator/lib/channel.py | 227 ++++++++++++++++++++++ sw/tools/sbus_fakerator/lib/controller.py | 152 +++++++++++++++ sw/tools/sbus_fakerator/lib/sbus.py | 87 +++++++++ sw/tools/sbus_fakerator/main.py | 160 +++++++++++++++ sw/tools/sbus_fakerator/sbus_fakerator | 3 + 7 files changed, 685 insertions(+) create mode 100644 sw/tools/sbus_fakerator/README.md create mode 100644 sw/tools/sbus_fakerator/lib/__init__.py create mode 100644 sw/tools/sbus_fakerator/lib/channel.py create mode 100644 sw/tools/sbus_fakerator/lib/controller.py create mode 100644 sw/tools/sbus_fakerator/lib/sbus.py create mode 100644 sw/tools/sbus_fakerator/main.py create mode 100755 sw/tools/sbus_fakerator/sbus_fakerator diff --git a/sw/tools/sbus_fakerator/README.md b/sw/tools/sbus_fakerator/README.md new file mode 100644 index 00000000000..12c44f2fde1 --- /dev/null +++ b/sw/tools/sbus_fakerator/README.md @@ -0,0 +1,34 @@ +#SBUS Fakerator +###Using +```bash +python main.py -$OPTIONS +``` + +###Options +There are the following options. + + + + + + + + + + + + + + + + + + + + + + +
optiondescriptionargs
-iActivates the SIM's Ivy Interface and send data on it.
-lArtifically introduce latencyAmount of latency introduced.
-pPortSerial port address that the RC of the plane is connected to.
+ +If there is no -i or -p (the output options) an error is thrown that terminates the program. + diff --git a/sw/tools/sbus_fakerator/lib/__init__.py b/sw/tools/sbus_fakerator/lib/__init__.py new file mode 100644 index 00000000000..7cb763bd75a --- /dev/null +++ b/sw/tools/sbus_fakerator/lib/__init__.py @@ -0,0 +1,22 @@ +''' + Copyright (C) 2016 Kason Bennett, Michal Podhradsky + + This file is part of paparazzi. + + paparazzi 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; either version 2, or (at your option) + any later version. + + paparazzi 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 paparazzi; see the file COPYING. If not, see + . + + Sbus fakerator: simulated SBUS radio for HITL testing +''' + diff --git a/sw/tools/sbus_fakerator/lib/channel.py b/sw/tools/sbus_fakerator/lib/channel.py new file mode 100644 index 00000000000..2a8203cb459 --- /dev/null +++ b/sw/tools/sbus_fakerator/lib/channel.py @@ -0,0 +1,227 @@ +''' + Copyright (C) 2016 Kason Bennett, Michal Podhradsky + + This file is part of paparazzi. + + paparazzi 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; either version 2, or (at your option) + any later version. + + paparazzi 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 paparazzi; see the file COPYING. If not, see + . + + Sbus fakerator: simulated SBUS radio for HITL testing +''' +from Tkinter import * +from lxml import etree +from _hotshot import resolution +from Tkconstants import HORIZONTAL, VERTICAL +import os + +c_types = ["Bi-switch", "Tri-switch", "Trigger-switch", "Dial-360", "Dial-180", "Throttle", "Hor-stick", "Vert-stick"] + +class Channel(Scale): + ''' + @var value: current value of Channel + + ''' + + def __init__(self, child, number, master=None, cnf={}, **kw): + ''' + Constructor + @param number: Channel number + ''' + self.bind = "" + self.number = number + self.c_type = "" + kw['to'] = -100 + kw['from_'] = 100 + kw['variable'] = self.value + Scale.__init__(self, cnf, kw) + + def add_value(self, val): + ''' + Add a number to val. To subtract, enter a negative number. + @param val: value to add to Channel value + ''' + self.value += val + self.set(self.value) + + def rebind(self, key): + self.bind = key + + def set_type(self, new_type): + if new_type in c_types: + self.c_type = new_type + else: + print "Type not known for {}".format(self.name) + + def repr_xml(self): + c_root = etree.Element("Channel") + c_root.attrib['key'] = self.bind + c_root.attrib['name'] = self.name + c_root.attrib['type'] = self.c_type + return c_root + + def parse_xml(self, xml): + self.bind = xml['key'] + self.name = xml['name'] + self.set_type(xml['type']) + + +class bi_switch(Channel): + + __name__ = "bi_switch" + + def __init__(self, number, orientation, name, scale_length, master=None, cnf={}, **kw): + ''' + Constructor + @param number: Channel number + ''' + self.value = IntVar(value=-100) + self.state = 0 + self.bind = "" + self.number = number + self.c_type = "" + self.name = name + Channel.__init__(self, number, master, cnf, label=name, length = scale_length, orient = orientation, variable=self.value) + self.set(-100) + + + def add_value(self, num): + self.state = (self.state + 1) % 2 + if self.state == 0: + self.value.set(-100) + elif self.state == 1: + self.value.set(100) + + def key_off(self): + pass + + +class tri_switch(Channel): + + __name__ = "tri_switch" + + def __init__(self, number, orientation, name, scale_length, master=None, cnf={}, **kw): + ''' + Constructor + @param number: Channel number + ''' + self.value = IntVar(value=-100) + self.state = 0 + self.bind = "" + self.number = number + self.c_type = "" + self.name = name + Channel.__init__(self, number, master, cnf, label = name, length = scale_length, resolution=100, orient = orientation, variable=self.value) + + def add_value(self, in_state): + ''' + @param val: value to add to Channel value + ''' + # 0=-100 + # 1=0 + # 2=100 + + if in_state > 0: + if self.value.get() == -100: + self.value.set(0) + elif self.value.get() == 0: + self.value.set(100) + elif in_state < 0: + if self.value.get() == 100: + self.value.set(0) + elif self.value.get() == 0: + self.value.set(-100) + + def key_off(self): + pass + +class trigger_switch(Channel): + + __name__ = "trigger_switch" + + def __init__(self, number, orientation, name, scale_length, master=None, cnf={}, **kw): + ''' + Constructor + @param number: Channel number + ''' + self.value = IntVar(value=-100) + self.state = 0 + self.bind = "" + self.number = number + self.c_type = "" + self.name = name + Channel.__init__(self, number, master, cnf, label = name, length = scale_length, orient=orientation, variable=self.value) + + def add_value(self, state): + ''' + @param state: state of the switch (i.e., flipped or not) + ''' + os.system('xset r off') + if state < 0: + self.value.set(-100) + else: + self.value.set(100) + + def key_off(self): + os.system('xset r on') + self.add_value(-1) + +class dial(Channel): + + __name__ = "dial" + def __init__(self, number, total_degrees, orientation, name, scale_length, master=None, cnf={}, **kw): + ''' + Constructor + @param number: Channel number + ''' + self.value = DoubleVar(0) + self.t_degrees = total_degrees + self.resolution = 200.0 / total_degrees + self.bind = "" + self.number = number + self.c_type = "" + self.name = name + Channel.__init__(self, number, master, cnf, label = name, length = scale_length, orient = orientation, resolution=self.resolution, variable=self.value) + + def add_value(self, in_value): + ''' + @param state: state of the switch (i.e., flipped or not) + ''' + self.value.get() + self.value.set((in_value * self.resolution) + self.value.get()) + + def key_off(self): + pass + +class stick(Channel): + __name__ = "stick" + def __init__(self, number, orientation, name, scale_length, master=None, cnf={}, **kw): + ''' + Constructor + @param number: Channel number + ''' + self.value = IntVar(0) + self.bind = "" + self.number = number + self.c_type = "" + Channel.__init__(self, number, master, cnf, orient = orientation, label = name, length = scale_length, variable=self.value) + + def add_value(self, value): + ''' + @param state: state of the switch (i.e., flipped or not) + ''' + self.value.set(self.value.get() + value) + + def key_off(self): + pass + diff --git a/sw/tools/sbus_fakerator/lib/controller.py b/sw/tools/sbus_fakerator/lib/controller.py new file mode 100644 index 00000000000..6df4d7d9fae --- /dev/null +++ b/sw/tools/sbus_fakerator/lib/controller.py @@ -0,0 +1,152 @@ +''' + Copyright (C) 2016 Kason Benett, Michal Podhradsky + + This file is part of paparazzi. + + paparazzi 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; either version 2, or (at your option) + any later version. + + paparazzi 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 paparazzi; see the file COPYING. If not, see + . + + Sbus fakerator: simulated SBUS radio for HITL testing +''' +import Tkinter as tk + +_key_bindings_ = dict() + +class Controller(tk.Frame): + + def __init__(self, parent, *args, **kwargs): + ''' + Constructor + ''' + tk.Frame.__init__(self, parent, *args, width = 1000, height = 1000) + self.num_of_channels = 0 + self.channels = [] + + def add_channel(self, channel_type, *args, **kwargs): + ''' + @param orientation = tk.VERTICAL : Either tk.VERTICAL or tk.HORIZONTAL + @param name = "" : name of the channel + @param x = 0 : x location in the frame + @param y = 0: y location in the frame + @param length = 100: length of the representation of the scale + @param add_key_binding = 'a' : which button press adds to the channel + @param sub_key_binding = 'z' : which button press subtracts from the channel + @param key_binding = 'q' : which button press activates the button + ''' + print "adding channels {}".format(channel_type.__name__) + #kwarg unpacking + orientation = 0 + name = "" + x = 0 + y = 0 + length = 100 + if 'orientation' in kwargs: + orientation = kwargs['orientation'] + else: + orientation = tk.VERTICAL + if 'name' in kwargs: + name = kwargs['name'] + if 'x' in kwargs: + x = kwargs['x'] + if 'y' in kwargs: + y = kwargs['y'] + if 'length' in kwargs: + length = kwargs['length'] + + + #channel adding + if channel_type.__name__ == 'bi_switch': + self.channels.append(channel_type(self.num_of_channels, orientation, name, length, self)) + if 'key_binding' in kwargs: + self.channels[self.num_of_channels].bind = kwargs['key_binding'] + _key_bindings_[kwargs['key_binding']] = (self.num_of_channels, 1) + elif channel_type.__name__ == 'tri_switch': + self.channels.append(channel_type(self.num_of_channels, orientation, name, length, self)) + if 'add_key_binding' in kwargs: + self.channels[self.num_of_channels].bind = kwargs['add_key_binding'] + _key_bindings_[kwargs['add_key_binding']] = (self.num_of_channels, 1) + if 'sub_key_binding' in kwargs: + self.channels[self.num_of_channels].bind = kwargs['sub_key_binding'] + _key_bindings_[kwargs['sub_key_binding']] = (self.num_of_channels, -1) + elif channel_type.__name__ == 'trigger_switch': + self.channels.append(channel_type(self.num_of_channels, orientation, name, length, self)) + if 'key_binding' in kwargs: + self.channels[self.num_of_channels].bind = kwargs['key_binding'] + _key_bindings_[kwargs['key_binding']] = (self.num_of_channels, 1) + elif channel_type.__name__ == 'dial': + self.channels.append(channel_type(self.num_of_channels, args[0], orientation, name, length, self)) + if 'add_key_binding' in kwargs: + self.channels[self.num_of_channels].bind = kwargs['add_key_binding'] + _key_bindings_[kwargs['add_key_binding']] = (self.num_of_channels, 1) + if 'sub_key_binding' in kwargs: + self.channels[self.num_of_channels].bind = kwargs['sub_key_binding'] + _key_bindings_[kwargs['sub_key_binding']] = (self.num_of_channels, -1) + elif channel_type.__name__ == 'stick': + self.channels.append(channel_type(self.num_of_channels, orientation, name, length, self)) + if 'add_key_binding' in kwargs: + self.channels[self.num_of_channels].bind = kwargs['add_key_binding'] + _key_bindings_[kwargs['add_key_binding']] = (self.num_of_channels, 1) + if 'sub_key_binding' in kwargs: + self.channels[self.num_of_channels].bind = kwargs['sub_key_binding'] + _key_bindings_[kwargs['sub_key_binding']] = (self.num_of_channels, -1) + + self.channels[self.num_of_channels].pack() + self.channels[self.num_of_channels].place(x = x, y=y) + self.num_of_channels += 1 + + + def add_value_of_channel(self, channel_no, *amount): + + if self.channels[channel_no].__name__ == 'bi_switch': + self.channels[channel_no].add_value() + elif self.channels[channel_no].__name__ == 'tri_switch': + self.channels[channel_no].add_value(amount[0]) + elif self.channels[channel_no].__name__ == 'trigger_switch': + self.channels[channel_no].add_value(amount[0]) + elif self.channels[channel_no].__name__ == 'dial': + self.channels[channel_no].add_value(amount[0]) + + self.channels[channel_no].pack() + + def handle_button_press(self, key): + if key in _key_bindings_: + self.channels[_key_bindings_[key][0]].add_value(_key_bindings_[key][1]) + else: + print key, " not bound" + + def handle_button_release(self, key): + if key in _key_bindings_: + self.channels[_key_bindings_[key][0]].key_off() + + def generate_package(self): + ''' + OldRange = (OldMax - OldMin) + NewRange = (NewMax - NewMin) + NewValue = (((OldValue - OldMin) * NewRange) / OldRange) + NewMin + + range of sbus control = 172 - 2047 + ''' + packet = [] + for x in self.channels: + base_max = 100.0 + base_min = -100.0 + limit_max = 2047 + limit_min = 172.0 + old_range = (base_max - (base_min)) + new_range = (limit_max - limit_min) + new_value = (((x.value.get() - base_min) * new_range) / old_range) + limit_min + packet.append(int(new_value)) + return packet + + diff --git a/sw/tools/sbus_fakerator/lib/sbus.py b/sw/tools/sbus_fakerator/lib/sbus.py new file mode 100644 index 00000000000..9decf7df6ae --- /dev/null +++ b/sw/tools/sbus_fakerator/lib/sbus.py @@ -0,0 +1,87 @@ +''' + Copyright (C) 2016 Kason Bennett, Michal Podhradsky + + This file is part of paparazzi. + + paparazzi 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; either version 2, or (at your option) + any later version. + + paparazzi 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 paparazzi; see the file COPYING. If not, see + . + + Sbus fakerator: simulated SBUS radio for HITL testing +''' +import numpy as np + +SBUS_NB_CHANNEL = 16 +SBUS_BUFF_LENGTH = 22 +SBUS_START_BYTE = 0x0f +SBUS_END_BYTE = 0x00 +SBUS_BIT_PER_CHANNEL = 11 +SBUS_BIT_PER_BYTE = 8 + +def sbus_decode(buff): + decoded = [0 for x in range(SBUS_NB_CHANNEL)] + byte_in_raw_buff = 0 + bit_in_raw_buff = 0 + channel = 0 + bit_in_channel = 0 + + for c in range(SBUS_NB_CHANNEL * SBUS_BIT_PER_CHANNEL): + if buff[byte_in_raw_buff] & (1 << bit_in_raw_buff): + decoded[channel] |= (1 << bit_in_channel) + + bit_in_raw_buff += 1 + bit_in_channel += 1 + + if(bit_in_raw_buff == SBUS_BIT_PER_BYTE): + bit_in_raw_buff = 0 + byte_in_raw_buff += 1 + + if(bit_in_channel == SBUS_BIT_PER_CHANNEL): + bit_in_channel = 0 + channel += 1 + + return decoded + + + +def sbus_encode(buff): + decoded = [0 for x in range(SBUS_BUFF_LENGTH)] + decoded[0] = 0 + channel = 0 + cur_byte = 0 + bit_in_byte = 0 + bit_in_channel = 0 + for x in range(SBUS_NB_CHANNEL * SBUS_BIT_PER_CHANNEL): + if int(buff[channel]) & (1 << bit_in_channel): + decoded[cur_byte] |= (1 << bit_in_byte) + + bit_in_channel += 1 + bit_in_byte += 1 + + if(bit_in_byte == SBUS_BIT_PER_BYTE): + bit_in_byte = 0 + cur_byte += 1 + + if(bit_in_channel == SBUS_BIT_PER_CHANNEL): + bit_in_channel = 0 + channel += 1 + + return [SBUS_START_BYTE] + decoded + [0, SBUS_END_BYTE] + + +if __name__ == "__main__": + control = [169, 84, 149, 170, 84, 197, 10, 86, 168, 138, 21, 172, 80, 21, 43, 88, 193, 10, 86, 168, 74, 85, 0, 0] + print sbus_decode(control) + print control + print sbus_encode(sbus_decode(control)) + diff --git a/sw/tools/sbus_fakerator/main.py b/sw/tools/sbus_fakerator/main.py new file mode 100644 index 00000000000..fde9990a00f --- /dev/null +++ b/sw/tools/sbus_fakerator/main.py @@ -0,0 +1,160 @@ +''' + Copyright (C) 2016 Kason Bennett, Michal Podhradsky + + This file is part of paparazzi. + + paparazzi 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; either version 2, or (at your option) + any later version. + + paparazzi 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 paparazzi; see the file COPYING. If not, see + . + + Sbus fakerator: simulated SBUS radio for HITL testing +''' +import serial +import struct +from threading import Thread, Lock +import sys + + +import time + +import Tkinter as tk + +from lib import controller +from lib import channel +from lib import sbus + +ser_rc = serial.Serial(None) + +options = [] +msg = [] +delay = 0 + +latency_packet_behind = 0 +latency_packet_sent = 0 + + +locker = Lock() + + +def new_controller(): + pass + +root = tk.Tk() + +menubar = tk.Menu(root) + +file_menu = tk.Menu(menubar, tearoff=0) +file_menu.add_command(label="Exit", command=root.quit()) + +new_menu = tk.Menu(file_menu, tearoff=1) +new_menu.add_command(label="Controller", command=new_controller) + +menubar.add_cascade(label="File", menu=file_menu) +menubar.add_cascade +root.config(menu=menubar) + +w = controller.Controller(root) +w.add_channel(channel.stick, name="Throttle", x=110, y=500, add_key_binding = 'w', sub_key_binding = 's') +w.add_channel(channel.stick, name="Roll", orientation = tk.HORIZONTAL, x = 100, y = 440, add_key_binding = 'a', sub_key_binding = 'd') +w.add_channel(channel.stick, name="Pitch", x = 500, y = 500, add_key_binding = 'i', sub_key_binding = 'k') +w.add_channel(channel.stick, name="Yaw", orientation=tk.HORIZONTAL, x = 490, y = 440, add_key_binding = 'j', sub_key_binding = 'l') +w.add_channel(channel.tri_switch, name="SA", length = 50, x = 100, y= 200, add_key_binding = "r", sub_key_binding = "f") +w.add_channel(channel.tri_switch, name="SB", length = 50, x = 180, y = 200, add_key_binding = "v", sub_key_binding = "c") +w.add_channel(channel.dial, 230, orientation = tk.HORIZONTAL, length = 150, name="S1", x = 100, y = 250, add_key_binding = "q", sub_key_binding = "e") +w.add_channel(channel.tri_switch, name="SC", length = 50, x = 440, y = 200, add_key_binding = "y", sub_key_binding = "h") +w.add_channel(channel.tri_switch, name="SD", length = 50, x = 520, y = 200, add_key_binding = "m", sub_key_binding = "n") +w.add_channel(channel.dial, 230, orientation = tk.HORIZONTAL, name="S2", length = 150, x = 440, y = 250, add_key_binding = "u", sub_key_binding = "o") +w.add_channel(channel.bi_switch, name = "SF", length = 50, x = 100, y = 0, key_binding = '1') +w.add_channel(channel.tri_switch, name="SE", length = 50, x = 100, y = 50, add_key_binding = '2', sub_key_binding = '3') +w.add_channel(channel.trigger_switch, name="SH", length = 50, x = 520, y = 0, key_binding = 'p') +w.add_channel(channel.tri_switch, name="SG", length = 50, x = 520, y = 50, add_key_binding = '[', sub_key_binding = ']') +w.add_channel(channel.dial, 180, name = "Slide_L", length = 300, x = 0, y = 0, add_key_binding = '4', sub_key_binding = '5') +w.add_channel(channel.dial, 180, name = "Slide_R", length = 300, x = 600, y = 0, add_key_binding = '6', sub_key_binding = '7') + + +def keypress(event): + x = event.char + w.handle_button_press(x) + +def keyrelease(event): + x = event.char + w.handle_button_release(x) + +main_frame = tk.Frame(root) +w.bind("", keypress) +w.bind("", keyrelease) + +def sbus_loop(): + + while True: + locker.acquire() + encoded = sbus.sbus_encode(w.generate_package()) + locker.release() + msg.append(encoded) + write_sbus() + time.sleep(.05) + +def write_sbus(): + global latency_packet_sent + if 'serial' in options: + for x in msg[latency_packet_sent]: + ser_rc.write(struct.pack('=0: + print "IVY: {}".format(msg[0][1:]) + i_interface.send_ivy_msg('ground_dl', 'SBUS', msg[0][1:]) + del msg[0] + if latency_packet_behind-latency_packet_sent <=0: + latency_packet_sent = latency_packet_behind - 1 + latency_packet_sent += 1 + +if __name__ == '__main__': + w.pack(side="top", fill="both", expand=True) + w.focus_set() + main_frame.pack() + + t = Thread(target=sbus_loop) + t.start() + print sys.argv + if not ('-i' in sys.argv or '-p' in sys.argv): + print "ERROR: no input found" + sys.exit() + for x in range(1, len(sys.argv)): + if sys.argv[x] == '-i': + import os + #Add the lib folder to the system path. + sys.path.append(os.getcwd() + "/../../lib/") + from sim_vars import i_interface + options.append('ivy') + + elif sys.argv[x] == '-l': + x += 1 + delay = float(sys.argv[x]) + latency_packet_behind = int(delay/.05) + + elif sys.argv[x] == '-p': + options.append('serial') + x += 1 + port = str(sys.argv[x]) + baud = 100000 + + ser_rc.port = port + ser_rc.baudrate = baud + ser_rc.bytesize = serial.EIGHTBITS + ser_rc.timeout = 1 + ser_rc.parity = serial.PARITY_EVEN + ser_rc.stopbits = serial.STOPBITS_TWO + ser_rc.open() + + root.mainloop() diff --git a/sw/tools/sbus_fakerator/sbus_fakerator b/sw/tools/sbus_fakerator/sbus_fakerator new file mode 100755 index 00000000000..83c60a68fb3 --- /dev/null +++ b/sw/tools/sbus_fakerator/sbus_fakerator @@ -0,0 +1,3 @@ +#!/bin/bash +python sw/tools/sbus_fakerator/main.py $1 $2 +