├── .gitignore ├── LICENSE ├── README.md ├── demo.py ├── drone.conf ├── drone.default.conf ├── droneconfig.py ├── dronedict.py └── minidrone.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | drone-201* 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2014 Gergely Szell 2 | 3 | Licensed under the Apache License, Version 2.0 (the "License"); 4 | you may not use this file except in compliance with the License. 5 | You may obtain a copy of the License at 6 | 7 | http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | Unless required by applicable law or agreed to in writing, software 10 | distributed under the License is distributed on an "AS IS" BASIS, 11 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | See the License for the specific language governing permissions and 13 | limitations under the License. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # py-minidrone # 2 | 3 | This is an unfinished Python library to control Parrot Rolling Spiders (one of the minidrones announced in 2014) over BLE. 4 | 5 | I've started working on this project in September 2014, then stopped due to lack of free time. 6 | The code is _NOT_ clean (especially the demo), I was still in the middle of testing and implementing several functions. There are things that just do not work. 7 | 8 | Expect something like this: https://www.youtube.com/watch?v=zAfGR75qia8 9 | 10 | I am planning to finish the library (at least what I planned originally), though. 11 | 12 | #### I am not responsible for any harm done by/to your drone! Use this at your own risk. #### 13 | 14 | 15 | ### How do I get set up? ### 16 | 17 | * Install Python 2.7+, Pexpect (pip install pexpect) 18 | * Install BlueZ 5+ (probably you will need to compile it from source, watch out for dependencies) 19 | + If you compiled it from source, don't forget to copy the %BlueZ%/attrib/gatttool binary to your PATH 20 | * Clone repo 21 | * Edit top of demo.py, replace MAC with yours 22 | + You can check it from CLI with "sudo hcitool lescan" 23 | * ./demo.py 24 | * Press 'c' 25 | * If the top menu gets filled with data from the drone, you're all set 26 | * FLY! 27 | * If anything goes wrong, press Esc (emergency cut-off) 28 | 29 | BT chips (both in the drone and in adapters) are pretty nitpicking regarding BLE connections, you might need to turn off/on your drone sometimes, also don't forget to use 'sudo hciconfig hciX reset'. 30 | 31 | 32 | ### Contribution ### 33 | 34 | * Pull requests are more than welcome 35 | 36 | -------------------------------------------------------------------------------- /demo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import curses 4 | import time 5 | import threading 6 | import minidrone 7 | import dronedict 8 | 9 | right_events = [curses.KEY_RIGHT, curses.KEY_LEFT, curses.KEY_UP, curses.KEY_DOWN] 10 | left_events = [ord('d'), ord('a'), ord('w'), ord('s')] 11 | 12 | S_DISCONNECTED = 0 13 | S_CONNECTING = 1 14 | S_CONNECTED = 2 15 | 16 | DRONEMAC = 'E0:14:D6:A9:3D:28' 17 | CB_MSG = 0 18 | CB_BATTERY = 1 19 | CB_DATA_UPDATE = 2 20 | CB_SPEED = 3 21 | CB_STATE = 4 22 | 23 | mutex = threading.Lock() 24 | 25 | def refresh_data(t, data): 26 | global message, config, battery, speed, state 27 | if t == CB_MSG: 28 | mutex.acquire() 29 | message = data 30 | mutex.release() 31 | elif t == CB_BATTERY: 32 | mutex.acquire() 33 | battery = data 34 | mutex.release() 35 | elif t == CB_SPEED: 36 | mutex.acquire() 37 | speed = data 38 | mutex.release() 39 | elif t == CB_DATA_UPDATE: 40 | mutex.acquire() 41 | config = data 42 | mutex.release() 43 | elif t == CB_STATE: 44 | mutex.acquire() 45 | state = S_CONNECTED if data == 'y' else S_DISCONNECTED 46 | mutex.release() 47 | 48 | def draw_joy(win): 49 | win.erase() 50 | for i in range(0, 5): 51 | win.addch(i, 5, '|') 52 | win.addstr(2, 0, '---- o ----') 53 | win.refresh() 54 | 55 | def hl_dir(win, dir): 56 | if dir in [curses.KEY_UP, ord('w')]: 57 | for i in range(0, 2): 58 | win.chgat(i, 5, 1, curses.color_pair(1)) 59 | elif dir in [curses.KEY_DOWN, ord('s')]: 60 | for i in range(3, 5): 61 | win.chgat(i, 5, 1, curses.color_pair(1)) 62 | elif dir in [curses.KEY_LEFT, ord('a')]: 63 | win.chgat(2, 0, 4, curses.color_pair(1)) 64 | elif dir in [curses.KEY_RIGHT, ord('d')]: 65 | win.chgat(2, 7, 4, curses.color_pair(1)) 66 | win.chgat(2, 5, 1, curses.color_pair(2)) 67 | win.refresh() 68 | 69 | def move_drone(event): 70 | if event == ord(' '): 71 | drone.takeoff() 72 | elif event in [13, curses.KEY_ENTER]: 73 | drone.land() 74 | elif event == 27: # ESC 75 | drone.emergency() 76 | elif event == curses.KEY_UP: 77 | drone.ascend() 78 | elif event == curses.KEY_DOWN: 79 | drone.descend() 80 | elif event == curses.KEY_RIGHT: 81 | drone.turn_right() 82 | elif event == curses.KEY_LEFT: 83 | drone.turn_left() 84 | elif event == ord('w'): 85 | drone.move_fw() 86 | elif event == ord('s'): 87 | drone.move_bw() 88 | elif event == ord('d'): 89 | drone.move_right() 90 | elif event == ord('a'): 91 | drone.move_left() 92 | elif event == ord('+'): 93 | drone.incr_speed() 94 | elif event == ord('-'): 95 | drone.decr_speed() 96 | elif event == ord('x'): 97 | drone.disconnect() 98 | 99 | def main_loop(stdscr): 100 | screen = curses.initscr() 101 | showing_info = False 102 | screen.timeout(100) 103 | win = screen.subwin(24, 80, 0, 0) 104 | curses.curs_set(0) 105 | curses.nonl() 106 | curses.start_color() 107 | curses.init_pair(1, curses.COLOR_BLUE, curses.COLOR_BLACK) 108 | curses.init_pair(2, curses.COLOR_RED, curses.COLOR_BLACK) 109 | w_status = win.subwin(9, 80, 0, 0) 110 | w_help = win.subwin(6, 80, 18, 0) 111 | w_leftjoy = win.subwin(5, 11, 11, 13) 112 | w_rightjoy = win.subwin(5, 11, 11, 55) 113 | w_status.box() 114 | w_help.box() 115 | w_help.vline(1, 39, '|', 4) 116 | w_help.vline(1, 59, '|', 4) 117 | w_help.addstr(1, 2, "w-a-s-d: Move horizontally") 118 | w_help.addstr(1, 41, "Space: Take off") 119 | w_help.addstr(2, 41, "Enter: Land") 120 | w_help.addstr(1, 61, "Esc: Emergency") 121 | w_help.addstr(3, 2, "+/-: Incr/decr speed") 122 | w_help.addstr(2, 2, "Arrows: Move vertically & rotate") 123 | w_help.addstr(2, 61, "c: Connect") 124 | w_help.addstr(3, 61, "x: Disconnect") 125 | w_help.addstr(4, 61, "q: Quit") 126 | w_help.addstr(4, 2, "u: Update config") 127 | w_help.addstr(3, 41, "i: Toggle Wheels") 128 | w_help.addstr(4, 41, "o: Toggle CutOut") 129 | win.box() 130 | win.addstr(0, 25, " MiniDrone remote controller ") 131 | w_status.vline(1, 39, '|', 5) 132 | w_status.hline(6, 1, '-', 78) 133 | w_status.addstr(2, 2, "Battery: ") 134 | w_status.addstr(1, 2, "ID: ") 135 | w_status.addstr(1, 41, "Serial: ") 136 | w_status.addstr(2, 41, "FW/HW ver: ") 137 | w_status.addstr(3, 2, "Speed: ") 138 | w_status.addstr(3, 41, "Wheels/CutOut: ") 139 | w_status.addstr(4, 2, "MaxAlt: ") 140 | w_status.addstr(4, 41, "MaxTilt: ") 141 | w_status.addstr(5, 2, "MaxVert: ") 142 | w_status.addstr(5, 41, "MaxRot: ") 143 | w_status.addstr(7, 2, "Status: ") 144 | win.refresh() 145 | global state 146 | while True: 147 | mutex.acquire() 148 | s = state 149 | w_status.addstr(7, 10, message + (68-len(message))*' ') 150 | mutex.release() 151 | w_status.refresh() 152 | if s == S_DISCONNECTED: 153 | event = screen.getch() 154 | if event == ord('q'): 155 | break 156 | elif event == ord('c'): 157 | mutex.acquire() 158 | state = S_CONNECTING 159 | mutex.release() 160 | drone.connect() 161 | elif s == S_CONNECTING: 162 | curses.napms(50) 163 | elif s == S_CONNECTED: 164 | draw_joy(w_leftjoy) 165 | draw_joy(w_rightjoy) 166 | win.chgat(0, 26, 27, curses.color_pair(1)) 167 | mutex.acquire() 168 | if len(config) >= 10: 169 | w_status.addstr(2, 11, battery + '% ') 170 | w_status.addstr(3, 9, speed + '% ') 171 | w_status.addstr(3, 56, dronedict.onoff(config['wheels']) + '/' + dronedict.onoff(config['cutout']) + ' ') 172 | w_status.addstr(1, 6, config['name']) 173 | w_status.addstr(1, 49, config['serial']) 174 | w_status.addstr(2, 52, config['fw'] + ', ' + config['hw']) 175 | w_status.addstr(4, 10, dronedict.get_pretty(config, dronedict.S_MAX_ALT) + ' ') 176 | w_status.addstr(4, 50, dronedict.get_pretty(config, dronedict.S_MAX_TILT) + ' ') 177 | w_status.addstr(5, 11, dronedict.get_pretty(config, dronedict.S_MAX_VERT) + ' ') 178 | w_status.addstr(5, 49, dronedict.get_pretty(config, dronedict.S_MAX_ROT) + ' ') 179 | mutex.release() 180 | w_status.refresh() 181 | event = screen.getch() 182 | if event == ord('q'): 183 | break 184 | move_drone(event) 185 | if event in right_events: 186 | hl_dir(w_rightjoy, event) 187 | elif event in left_events: 188 | hl_dir(w_leftjoy, event) 189 | elif event == ord('o'): 190 | drone.cutout(not config['cutout']) 191 | elif event == ord('i'): 192 | drone.wheels(not config['wheels']) 193 | curses.napms(70) 194 | 195 | if __name__ == '__main__': 196 | global drone, state, message, config, speed, battery 197 | state = S_DISCONNECTED 198 | message = speed = battery = '' 199 | config = dict() 200 | drone = minidrone.MiniDrone(mac=DRONEMAC, callback=refresh_data) 201 | curses.wrapper(main_loop) 202 | drone.die() 203 | curses.curs_set(1) 204 | curses.nl() 205 | 206 | -------------------------------------------------------------------------------- /drone.conf: -------------------------------------------------------------------------------- 1 | [drone] 2 | 3 | ; meter - min: 2, max: 10 4 | max_altitude = 5 5 | 6 | ; meter/sec - min: 0.5, max: 2.5 7 | max_vertical_speed = 1 8 | 9 | ; degree - min: 5, max: 25 10 | max_tilt = 15 11 | 12 | ; degree/sec - min: 50, max: 360 13 | max_rotation_speed = 150 14 | 15 | ; boolean 16 | wheels = False 17 | 18 | ; boolean 19 | cutoff = True 20 | -------------------------------------------------------------------------------- /drone.default.conf: -------------------------------------------------------------------------------- 1 | [drone] 2 | 3 | ; meter - min: 2, max: 10 4 | max_altitude = 5 5 | 6 | ; meter/sec - min: 0.5, max: 2.5 7 | max_vertical_speed = 1 8 | 9 | ; degree - min: 5, max: 25 10 | max_tilt = 15 11 | 12 | ; degree/sec - min: 50, max: 360 13 | max_rotation_speed = 150 14 | 15 | ; boolean 16 | wheels = False 17 | 18 | ; boolean 19 | cutoff = True 20 | -------------------------------------------------------------------------------- /droneconfig.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from configparser import ConfigParser 4 | import dronedict 5 | 6 | CONFIG_FILE = 'drone.conf' 7 | SECTION = 'drone' 8 | 9 | class C(object): 10 | 11 | def __init__(self): 12 | self.config = ConfigParser.ConfigParser() 13 | self.config.readfp(open(CONFIG_FILE)) 14 | 15 | def flush(self): 16 | self.config.write(open(CONFIG_FILE, 'w')) 17 | 18 | def get_max_alt(self): 19 | return self.config.getfloat(SECTION, dronedict.S_MAX_ALT) 20 | 21 | def get_max_rot_speed(self): 22 | return self.config.getfloat(SECTION, dronedict.S_MAX_ROT) 23 | 24 | def get_max_tilt(self): 25 | return self.config.getfloat(SECTION, dronedict.S_MAX_TILT) 26 | 27 | def get_max_vert_speed(self): 28 | return self.config.getfloat(SECTION, dronedict.S_MAX_VERT) 29 | 30 | def get_wheels(self): 31 | return self.config.getboolean(SECTION, dronedict.S_WHEELS) 32 | 33 | def get_cutoff(self): 34 | return self.config.getboolean(SECTION, dronedict.S_CUTOUT) 35 | 36 | def set_max_alt(self, value): 37 | self.config.set(SECTION, dronedict.S_MAX_ALT, value) 38 | self.flush() 39 | 40 | def set_max_rot_speed(self, value): 41 | self.config.set(SECTION, dronedict.S_MAX_ROT_SPEED, value) 42 | self.flush() 43 | 44 | def set_max_tilt(self, value): 45 | self.config.set(SECTION, dronedict.S_MAX_TILT, value) 46 | self.flush() 47 | 48 | def set_max_vert_speed(self, value): 49 | self.config.set(SECTION, dronedict.S_MAX_VERT_SPEED, value) 50 | self.flush() 51 | 52 | def set_wheels(self, value): 53 | self.config.set(SECTION, dronedict.S_WHEELS, value) 54 | self.flush() 55 | 56 | def set_cutoff(self, value): 57 | self.config.set(SECTION, dronedict.S_CUTOUT, value) 58 | self.flush() 59 | -------------------------------------------------------------------------------- /dronedict.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import binascii 5 | import re 6 | import struct 7 | 8 | #### INCOMING NOTIFICATIONS 9 | 10 | P_BATTERY = '0x00bf value:[0-9a-f ]+\r\n' 11 | P_NOTIFICATION = '0x00bc value:[0-9a-f ]+\r\n' 12 | P_CONNECTED = 'Connection successful' 13 | 14 | P_NAME = '04 .. 00 03 02 00 [0-9a-f ]+ 00' 15 | P_SERIALP1 = '04 .. 00 03 04 00 [0-9a-f ]+ 00' 16 | P_SERIALP2 = '04 .. 00 03 05 00 [0-9a-f ]+ 00' 17 | P_FW_HW = '04 .. 00 03 03 00 [0-9a-f ]+ 00 [0-9a-f ]+ 00' 18 | 19 | P_PILOT_WHEELS_IN = '04 .. 02 05 02 00 [0-9a-f ]+' 20 | P_PILOT_CUTOUT_IN = '04 .. 02 0b 02 00 [0-9a-f ]+' 21 | P_PILOT_MAXVERT_IN = '04 .. 02 05 00 00 [0-9a-f ]+' 22 | P_PILOT_MAXROT_IN = '04 .. 02 05 01 00 [0-9a-f ]+' 23 | P_PILOT_MAXALT_IN = '04 .. 02 09 00 00 [0-9a-f ]+' 24 | P_PILOT_MAXTILT_IN = '04 .. 02 09 01 00 [0-9a-f ]+' 25 | 26 | #### OUTGOING MESSAGES 27 | 28 | #### OTHER CONSTANTS, UNITS 29 | S_NAME = 'name' 30 | S_SERIAL = 'serial' 31 | S_FW = 'fw' 32 | S_HW = 'hw' 33 | S_MAX_VERT = 'max_vert' 34 | S_MAX_ROT = 'max_rot' 35 | S_MAX_ALT = 'max_alt' 36 | S_MAX_TILT = 'max_tilt' 37 | S_WHEELS = 'wheels' 38 | S_CUTOUT = 'cutout' 39 | 40 | C_ON = 'On' 41 | C_OFF = 'Off' 42 | C_CONN_OK = 'Connection estabilished. Have fun!' 43 | 44 | UNITS = {S_MAX_VERT:'m/s', S_MAX_ROT:'°/s', S_MAX_ALT:'m', S_MAX_TILT:'°'} 45 | 46 | def process_battery(drone, value): 47 | drone.battery = str(int(value.split(' ')[-2], 16)) 48 | drone.cb(1, drone.battery) 49 | 50 | def process_notification(drone, value): 51 | ## Process drone information 52 | if re.search(P_NAME, value): 53 | drone.settings[S_NAME] = get_init_info(value) 54 | elif re.search(P_SERIALP1, value): 55 | drone.settings[S_SERIAL] = get_init_info(value) 56 | elif re.search(P_SERIALP2, value): 57 | drone.settings[S_SERIAL] = drone.settings[S_SERIAL] + get_init_info(value) 58 | elif re.search(P_FW_HW, value): 59 | (drone.settings[S_FW], drone.settings[S_HW]) = get_init_info(value.replace('00', '7c')).split('|') 60 | drone.cb(0, C_CONN_OK) 61 | ## Process piloting settings 62 | elif re.search(P_PILOT_MAXVERT_IN, value): 63 | drone.settings[S_MAX_VERT] = get_current_setting(value) 64 | elif re.search(P_PILOT_MAXROT_IN, value): 65 | drone.settings[S_MAX_ROT] = get_current_setting(value) 66 | elif re.search(P_PILOT_MAXALT_IN, value): 67 | drone.settings[S_MAX_ALT] = get_current_setting(value) 68 | elif re.search(P_PILOT_MAXTILT_IN, value): 69 | drone.settings[S_MAX_TILT] = get_current_setting(value) 70 | elif re.search(P_PILOT_WHEELS_IN, value): 71 | drone.settings[S_WHEELS] = get_misc(value) 72 | elif re.search(P_PILOT_CUTOUT_IN, value): 73 | drone.settings[S_CUTOUT] = get_misc(value) 74 | drone.cb(2, drone.settings) 75 | 76 | 77 | def get_init_info(pattern): 78 | return binascii.unhexlify(''.join(pattern.split(' ')[8:][:-2])) 79 | 80 | def get_current_setting(pattern): 81 | return hex2vals(''.join(pattern.split(' ')[9:12])) 82 | 83 | # We don't use the minimum or maximum values currently, these can be implemented for safety checks though. 84 | # def get_min_setting(pattern): 85 | # return hex2vals(''.join(pattern.split(' ')[13:16])) 86 | # 87 | # def get_max_setting(pattern): 88 | # return hex2vals(''.join(pattern.split(' ')[17:20])) 89 | 90 | def get_misc(pattern): 91 | return True if pattern.split(' ')[-2] == '01' else False 92 | 93 | def val2hexs(f_val): 94 | return format(struct.unpack(' 2: 64 | self.drone.cb(4, 'n') 65 | else: 66 | self.reader.terminate(True) 67 | break 68 | 69 | class WriterThread(StoppableThread): 70 | 71 | def __init__(self, drone): 72 | StoppableThread.__init__(self) 73 | self.drone = drone 74 | self.gatt = pexpect.spawn(drone.gatttool_path + ' -b ' + drone.mac + ' -I -t random', echo=False) 75 | self.t_reader = ReaderThread(drone, self.gatt) 76 | self.t_reader.daemon = True 77 | 78 | def run(self): 79 | self.t_reader.start() 80 | while True: 81 | if self.stop_event.is_set() and self.drone.q.empty(): 82 | self.drone.q.join() 83 | self.gatt.sendeof() 84 | self.gatt.terminate(True) 85 | self.t_reader.stop() 86 | self.t_reader.join() 87 | break 88 | else: 89 | cmd = self.drone.q.get() 90 | if "connect" in cmd.handle: 91 | self.gatt.sendline(cmd.handle) 92 | self.drone.q.task_done() 93 | continue 94 | if not cmd.response: 95 | self.gatt.sendline(" ".join(["char-write-cmd", cmd.handle, cmd.value])) 96 | else: 97 | self.gatt.sendline(" ".join(["char-write-req", cmd.handle, cmd.value])) 98 | self.drone.q.task_done() 99 | 100 | class MiniDrone(object): 101 | 102 | def __init__(self, mac=None, callback=None): 103 | self.mac = mac 104 | self.callback = callback 105 | req_missing = self.req_check() 106 | if req_missing: 107 | self.cb(0, req_missing) 108 | return 109 | self.seq_joy = 1 # 0x0040 110 | self.seq_ref = 0 # 0x0043 111 | self.timer_t = 0.3 112 | self.settings = dict() 113 | self.speed = 30 114 | self.status = S.Disconnected 115 | self.q = Queue() 116 | self.t_writer = WriterThread(self) 117 | self.t_writer.daemon = True 118 | self.wd_timer = threading.Timer(self.timer_t, self.still) 119 | 120 | def cb(self, *args, **kwargs): 121 | if self.callback: 122 | self.callback(*args, **kwargs) 123 | 124 | def req_check(self): 125 | # check pexpect 126 | 127 | # check BlueZ 128 | self.gatttool_path = pexpect.which('gatttool') 129 | if not self.gatttool_path: 130 | return "Please install the BlueZ stack: 'apt-get install bluez'" 131 | 132 | # if there is no MAC set, try to search for a drone (requires root) 133 | if not self.mac: 134 | if os.geteuid() != 0: 135 | return "Drone MAC missing. Init with 'mac=' or run as root to scan." 136 | lescan = pexpect.spawn("hcitool lescan", echo=False) 137 | index = lescan.expect(P_OUIS, timeout=5) 138 | if index != 0: 139 | return "Couldn't find any drones nearby." 140 | self.mac = lescan.after 141 | self.cb(0, "Drone found! MAC: " + self.mac) 142 | 143 | elif not re.match(P_MAC, self.mac): 144 | return "Drone MAC format incorrect, please check it. Format: XX:XX:XX:XX:XX:XX" 145 | 146 | self.cb(0, "Everything seems to be alright, time to fly!") 147 | return None 148 | 149 | def die(self): 150 | self.wd_timer.cancel() 151 | self.disconnect() 152 | if self.t_writer.is_alive(): 153 | self.t_writer.stop() 154 | self.t_writer.join() 155 | self.cb(0, "Connection closed.") 156 | self.status = S.Disconnected 157 | 158 | def connect(self): 159 | self.cb(0, "Connecting to drone...") 160 | self.t_writer.start() 161 | self.low_level('connect', '') 162 | time.sleep(1) 163 | self.status = S.Init 164 | self.send(self.send_init) 165 | 166 | def disconnect(self): 167 | self.cb(0, "Disconnecting...") 168 | self.low_level('disconnect', '') 169 | 170 | def ascend(self): 171 | self.send(self.send_joy, 0, 0, 0, self.speed) 172 | 173 | def descend(self): 174 | self.send(self.send_joy, 0, 0, 0, -self.speed) 175 | 176 | def turn_left(self): 177 | self.send(self.send_joy, 0, 0, -self.speed, 0) 178 | 179 | def turn_right(self): 180 | self.send(self.send_joy, 0, 0, self.speed, 0) 181 | 182 | def move_fw(self): 183 | self.send(self.send_joy, 0, self.speed, 0, 0) 184 | 185 | def move_bw(self): 186 | self.send(self.send_joy, 0, -self.speed, 0, 0) 187 | 188 | def move_right(self): 189 | self.send(self.send_joy, self.speed, 0, 0, 0) 190 | 191 | def move_left(self): 192 | self.send(self.send_joy, -self.speed, 0, 0, 0) 193 | 194 | def still(self): 195 | self.send(self.send_joy, 0, 0, 0, 0) 196 | 197 | def incr_speed(self): 198 | if self.speed < 100: 199 | self.speed += 10 200 | self.cb(3, str(self.speed)) 201 | 202 | def decr_speed(self): 203 | if self.speed > 0: 204 | self.speed -= 10 205 | self.cb(3, str(self.speed)) 206 | 207 | def takeoff(self): 208 | self.cb(0, "Taking off!") 209 | self.send(self.send_ref, Base.FlatTrim) 210 | time.sleep(0.5) 211 | self.send(self.send_ref, Base.TakeOff) 212 | self.cb(0, "Airborne!") 213 | 214 | def land(self): 215 | self.cb(0, "Landing!") 216 | self.send(self.send_ref, Base.Land) 217 | time.sleep(1) 218 | self.cb(0, "Back to the ground.") 219 | 220 | def emergency(self): 221 | self.cb(0, "Emergency signal sent!") 222 | self.send(self.send_ref, None, True) 223 | 224 | def setup_time(self): 225 | times = time_bin() 226 | for i in range(1, 3): 227 | self.send_ref('0004' + ('%02x' % i) + '00' + times[i-1] + '00') 228 | 229 | def wheels(self, wheels): 230 | self.send_ref('02010200' + ('01' if wheels else '00')) 231 | 232 | def cutout(self, cutout): 233 | self.send_ref('020a0000' + ('01' if cutout else '00')) 234 | 235 | def send_joy(self, hor_lr, hor_fb, rot, vert): 236 | handle = '0x0040' 237 | value = '02' + \ 238 | sq2b(self.seq_joy) + \ 239 | '02000200' + \ 240 | merge_moves(hor_lr, hor_fb, rot, vert) + \ 241 | '00000000' 242 | self.low_level(handle, value) 243 | self.seq_joy += 1 244 | 245 | def send_ref(self, t, emergency=False): 246 | if not emergency: 247 | handle = '0x0043' 248 | self.seq_ref += 1 249 | else: 250 | handle = '0x0046' 251 | value = '04' + ('%02x' % self.seq_ref) 252 | if t == Base.FlatTrim: 253 | value += '02000000' 254 | elif t == Base.TakeOff: 255 | value += '02000100' 256 | elif t == Base.Land: 257 | value += '02000300' 258 | elif emergency: 259 | value += '02000400' 260 | else: 261 | value += t 262 | self.low_level(handle, value) 263 | 264 | def send_init(self): 265 | self.cb(0, "Initializing...") 266 | switch = '0100' 267 | self.low_level('0x00c0', switch, True) 268 | self.low_level('0x00bd', switch, True) 269 | self.low_level('0x00e4', switch, True) 270 | self.low_level('0x00e7', switch, True) 271 | self.low_level('0x0116', switch) 272 | self.low_level('0x0126', switch) 273 | self.setup_time() 274 | time.sleep(1.2) 275 | self.send_ref('00020000') 276 | time.sleep(1.2) 277 | self.send_ref('00040000') 278 | time.sleep(2) 279 | self.wheels(True) 280 | self.cutout(True) 281 | 282 | def send_ack(self, seq): 283 | handle = '0x007c' 284 | value = '01' + seq + seq 285 | self.low_level(handle, value) 286 | 287 | def send(self, cmd, *args, **kwargs): 288 | self.wd_timer.cancel() 289 | cmd(*args, **kwargs) 290 | self.wd_timer = threading.Timer(self.timer_t, self.still) 291 | self.wd_timer.start() 292 | 293 | def low_level(self, handle, value, response=False): 294 | self.q.put(Cmd(handle, value, response)) 295 | 296 | def sq2b(seq): # we need the last 8 bits only 297 | return '%02x' % (seq & 0b0000000011111111) 298 | 299 | def sp2b(speed): # 0-100 300 | return '%02x' % (speed & 0b11111111) 301 | 302 | def time_bin(): 303 | return [binascii.hexlify(t) for t in time.strftime("%Y-%m-%d|T%H%M%S%z", time.localtime()).split('|')] 304 | 305 | def merge_moves(hor_lr, hor_fb, rot, vert): 306 | t = '01' if (hor_lr != 0 or hor_fb != 0) else '00' 307 | return t + sp2b(hor_lr) + sp2b(hor_fb) + sp2b(rot) + sp2b(vert) 308 | 309 | def config_value(type, seq, value): 310 | result = "04" + seq 311 | if type == droneconfig.MAX_ALT: 312 | result += "02080000" + val2hexs(value) 313 | elif type == droneconfig.MAX_TILT: 314 | result += "02080100" + val2hexs(value) 315 | elif type == droneconfig.MAX_VERT_SPEED: 316 | result += "02010000" + val2hexs(value) 317 | elif type == droneconfig.MAX_ROT_SPEED: 318 | result += "02010100" + val2hexs(value) 319 | return result 320 | --------------------------------------------------------------------------------