Created
March 5, 2014 07:03
-
-
Save vo/9362500 to your computer and use it in GitHub Desktop.
Quick hack to override RC over MAVLink with arrow keys
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/env python | |
import sys, os | |
from optparse import OptionParser | |
import Tkinter as tk | |
# tell python where to find mavlink so we can import it | |
sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), '../mavlink')) | |
from pymavlink import mavutil | |
class App(tk.Frame): | |
def __init__(self, tkroot, mavmaster): | |
tk.Frame.__init__(self, tkroot) | |
self.mavmaster = mavmaster | |
self.tkroot = tkroot | |
self.validkeys = ['Right', 'Left', 'Up', 'Down'] | |
self.afterId = dict((k,None) for k in self.validkeys) | |
self.keyState = dict((k,False) for k in self.validkeys) | |
self.tkroot.bind_all('<Key>', self.keyPressed) | |
self.tkroot.bind_all('<KeyRelease>', self.keyReleased) | |
def keyPressed(self,event): | |
if event.keysym == 'Escape': | |
release_rc(self.mavmaster) | |
self.quit() | |
if event.keysym in self.validkeys: | |
if self.afterId[event.keysym] != None: | |
self.after_cancel(self.afterId[event.keysym]) | |
self.afterId[event.keysym] = None | |
else: | |
self.keyState[event.keysym] = event.time | |
processKeyCommand(self.mavmaster, self.keyState) | |
def keyReleased(self,event): | |
if event.keysym in self.validkeys: | |
self.afterId[event.keysym] = self.after_idle(self.releaseKey, event) | |
def releaseKey(self, event): | |
self.afterId[event.keysym] = None | |
self.keyState[event.keysym] = False | |
processKeyCommand(self.mavmaster, self.keyState) | |
# release control back to the radio | |
def release_rc(master): | |
# a value of 0 releases the control to what the radio says | |
values = [ 0 ] * 8 | |
for i in xrange(1): | |
master.mav.rc_channels_override_send(master.target_system, | |
master.target_component, *values) | |
# attempt to send a control. | |
# you can pass 0 to refer to what the radio says | |
# you can pass 0xFFFF to refer to keep the current value | |
def send_rc(master, data): | |
for i in xrange(1): | |
master.mav.rc_channels_override_send( | |
master.target_system, master.target_component, *data) | |
print ("sending rc: %s"%data) | |
def processKeyCommand(master, keystate): | |
amt = 100 | |
pitch = 1500 | |
roll = 1500 | |
# handle elevator stick | |
if keystate['Up']: | |
pitch -= amt # nose down (to go forward) | |
if keystate['Down']: | |
pitch += amt # nose up (to go back) | |
if pitch == 1500: | |
# if this stick is centered, just release it | |
pitch = 0 | |
# handle aile stick | |
if keystate['Left']: | |
roll -= amt # roll left | |
if keystate['Right']: | |
roll += amt # roll right | |
if roll == 1500: | |
# if this stick is centered, just release it | |
roll = 0 | |
# set data (0 means channel is released) | |
data = [ 0 ] * 8 | |
data[0] = roll | |
data[1] = pitch | |
# send the data to the rc override command | |
send_rc(master, data) | |
def main(): | |
# read command line options | |
parser = OptionParser("readdata.py [options]") | |
parser.add_option("--baudrate", dest="baudrate", type='int', | |
help="master port baud rate", default=115200) | |
parser.add_option("--device", dest="device", default=None, help="serial device") | |
parser.add_option("--rate", dest="rate", default=4, type='int', help="requested stream rate") | |
parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int', | |
default=255, help='MAVLink source system for this GCS') | |
parser.add_option("--showmessages", dest="showmessages", action='store_true', | |
help="show incoming messages", default=False) | |
(opts, args) = parser.parse_args() | |
if opts.device is None: | |
print("You must specify a serial device") | |
sys.exit(1) | |
# create a mavlink serial instance | |
master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate) | |
# wait for the heartbeat msg to find the system ID | |
master.wait_heartbeat() | |
# request data to be sent at the given rate | |
master.mav.request_data_stream_send(master.target_system, master.target_component, | |
mavutil.mavlink.MAV_DATA_STREAM_ALL, opts.rate, 1) | |
# start up the window to read arrow keys | |
tkroot = tk.Tk() | |
application = App(tkroot, master) | |
tkroot.mainloop() | |
if __name__ == '__main__': | |
main() |
Awesome! Thanks alot, I was looking for something like this. Exactly what I need, now I can have this as a reference for my own application. :-)
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Awesome example, Christopher! You saved me (and probably many others) a huge amount of time figuring out how to do this.