def joystick():
"""
Function to update commands and attitude to be called by a thread.
"""
try:
while True:
if udp.active:
current = time.time()
elapsed = 0
# Part for applying commands to the vehicle.
# Channel order in mavlink: roll, pitch, throttle, yaw
# Channel order in optitrack: roll, pitch, yaw, throttle
roll = udp.message[0]
pitch = utils.mapping(udp.message[1],1000,2000,2000,1000) # To invert channel, maybe add function
throttle = utils.mapping(udp.message[3],1000,2000,968,1998) # Map it to match RC configuration
yaw = utils.mapping(udp.message[2],1000,2000,968,2062) # Map it to match RC configuration
vehicle.channel_override = { "1" : roll, "2" : pitch, "3" : throttle, "4" : yaw }
vehicle.flush()
#print "%s" % vehicle.attitude
print "%s" % vehicle.channel_readback
#print modules.UDPserver.message
# 100hz loop
while elapsed < 0.01:
elapsed = time.time() - current
# End of the main loop
except Exception,error:
print "Error on joystick thread: "+str(error)
joystick()
def boxAngle():
global board, canvas, a, c, b, running
canvas.delete(a)
canvas.delete(c)
canvas.delete(b)
board.getData(MultiWii.ATTITUDE)
running = 1
#attitude = {'angx':0,'angy':0,'heading':0,'elapsed':0,'timestamp':0}
arcStart = board.attitude['angx']+arcwidth+90
arcEnd = board.attitude['angx']-arcwidth+90
#r = int(utils.limit(utils.mapping(board.attitude['angx'], -90, 10, 255, 0),0,255))
#g = int(utils.limit(utils.mapping(board.attitude['angx'], -20, 20, 200, 255),0,255))
#b = int(utils.limit(utils.mapping(board.attitude['angx'], -10, 90, 0, 255),0,255))
r = int(utils.limit(utils.mapping(board.attitude['angx'], -100, 10, 255, 0),0,255))
g = int(utils.limit(utils.mapping(board.attitude['angx'], -20, 20, 200, 255),0,255))
b = int(utils.limit(utils.mapping(board.attitude['angx'], -10, 100, 0, 255),0,255))
#print "angle= %0.2f r= %d g= %d b= %d" % (board.attitude['angx'],r,g,b)
#canvas.create_circle_arc(100, 120, 50, fill="black", style="arc", outline=utils.rgb_to_hex((r, g, b)), width=10, start=0, end=180)
c = canvas.create_circle(sizeX/2, sizeY/2, circleradius, fill="black", outline=utils.rgb_to_hex((r, g, b)), width=ringwidth)
a = canvas.create_circle_arc(sizeX/2, sizeY/2, arcradius, style="arc", outline="white", width=7, start=arcStart, end=arcEnd)
b = canvas.create_circle(sizeX/2, sizeY/2, 30, fill="#ED1F24")
t = canvas.create_text(sizeX/2, sizeY/2,text="GO", fill="white", font=("Helvetica", 30))
canvas.tag_bind(b, "<ButtonPress-1>", click)
canvas.tag_bind(t, "<ButtonPress-1>", click)
tk.after(150, boxAngle)
def boxAngle():
global board, canvas, a, c, b, running
canvas.delete(a)
canvas.delete(c)
canvas.delete(b)
board.getData(MultiWii.ATTITUDE)
running = 1
#attitude = {'angx':0,'angy':0,'heading':0,'elapsed':0,'timestamp':0}
arcStart = board.attitude['angx'] + arcwidth + 90
arcEnd = board.attitude['angx'] - arcwidth + 90
#r = int(utils.limit(utils.mapping(board.attitude['angx'], -90, 10, 255, 0),0,255))
#g = int(utils.limit(utils.mapping(board.attitude['angx'], -20, 20, 200, 255),0,255))
#b = int(utils.limit(utils.mapping(board.attitude['angx'], -10, 90, 0, 255),0,255))
r = int(
utils.limit(utils.mapping(board.attitude['angx'], -100, 10, 255, 0), 0,
255))
g = int(
utils.limit(utils.mapping(board.attitude['angx'], -20, 20, 200, 255),
0, 255))
b = int(
utils.limit(utils.mapping(board.attitude['angx'], -10, 100, 0, 255), 0,
255))
#print "angle= %0.2f r= %d g= %d b= %d" % (board.attitude['angx'],r,g,b)
#canvas.create_circle_arc(100, 120, 50, fill="black", style="arc", outline=utils.rgb_to_hex((r, g, b)), width=10, start=0, end=180)
c = canvas.create_circle(sizeX / 2,
sizeY / 2,
circleradius,
fill="black",
outline=utils.rgb_to_hex((r, g, b)),
width=ringwidth)
a = canvas.create_circle_arc(sizeX / 2,
sizeY / 2,
arcradius,
style="arc",
outline="white",
width=7,
start=arcStart,
end=arcEnd)
b = canvas.create_circle(sizeX / 2, sizeY / 2, 30, fill="#ED1F24")
t = canvas.create_text(sizeX / 2,
sizeY / 2,
text="GO",
fill="white",
font=("Helvetica", 30))
canvas.tag_bind(b, "<ButtonPress-1>", click)
canvas.tag_bind(t, "<ButtonPress-1>", click)
tk.after(150, boxAngle)
def click(event):
global b, update, running, printer, events, board
canvas.delete(b)
b = canvas.create_circle(sizeX / 2,
sizeY / 2,
30,
fill="red",
outline="white",
width=1)
if board.attitude['angx'] == 0.0:
valor = random.randint(0, len(events) - 1)
else:
valor = int(
utils.limit(
utils.mapping(board.attitude['angx'], -110, 110, 0,
len(events) - 1), 0,
len(events) - 1))
print "\nStart printing... ang %0.2f, value %d" % (board.attitude['angx'],
valor)
try:
url = events[valor][3]
except:
url = "No URL... sorry"
message = "%s\nOn %s\n%s\nMore info: %s\n" % (
events[valor][1], events[valor][0], events[valor][2], url)
print message
try:
printer.justify("C")
printer.inverse_on()
printer.bold_on()
printer.print_text("TIME BOX")
printer.inverse_off()
printer.print_text("\n______________\n")
printer.bold_off()
printer.print_text("According to:\n")
printer.bold_on()
printer.print_text(str(board.attitude['angx']))
printer.print_text(" degrees")
printer.bold_off()
printer.print_text("\nof inclination\n______________\n\n")
printer.bold_on()
printer.print_text(events[valor][1])
printer.bold_off()
printer.print_text("\n")
printer.print_text(events[valor][0])
printer.print_text("\n\n")
printer.print_text(events[valor][2])
printer.print_text("\n\nMore info:\n")
printer.print_text(events[valor][3])
printer.print_text("\n\n:)\n____________________________\n\n\n\n\n\n")
except:
print "Error while printing, perhaps no printer attached."
def click(event):
global b, update, running, printer, events, board
canvas.delete(b)
b = canvas.create_circle(sizeX/2, sizeY/2, 30, fill="red", outline="white", width=1)
if board.attitude['angx'] == 0.0:
valor = random.randint(0,len(events)-1)
else:
valor = int(utils.limit(utils.mapping(board.attitude['angx'],-110,110,0,len(events)-1),0,len(events)-1))
print "\nStart printing... ang %0.2f, value %d" % (board.attitude['angx'],valor)
try:
url = events[valor][3]
except:
url = "No URL... sorry"
message = "%s\nOn %s\n%s\nMore info: %s\n" % (events[valor][1],events[valor][0],events[valor][2],url)
print message
try:
printer.justify("C")
printer.inverse_on()
printer.bold_on()
printer.print_text("TIME BOX")
printer.inverse_off()
printer.print_text("\n______________\n")
printer.bold_off()
printer.print_text("According to:\n")
printer.bold_on()
printer.print_text(str(board.attitude['angx']))
printer.print_text(" degrees")
printer.bold_off()
printer.print_text("\nof inclination\n______________\n\n")
printer.bold_on()
printer.print_text(events[valor][1])
printer.bold_off()
printer.print_text("\n")
printer.print_text(events[valor][0])
printer.print_text("\n\n")
printer.print_text(events[valor][2])
printer.print_text("\n\nMore info:\n")
printer.print_text(events[valor][3])
printer.print_text("\n\n:)\n____________________________\n\n\n\n\n\n")
except:
print "Error while printing, perhaps no printer attached."
def sendCommands():
"""
Function to update commands and attitude to be called by a thread.
"""
try:
while True:
if udp.active:
# Timers
current = time.time()
elapsed = 0
# Part for applying commands to the vehicle.
# Channel order in mavlink: roll, pitch, throttle, yaw
# Channel order in optitrack: roll, pitch, yaw, throttle
# Joystick manual commands:
roll = udp.message[0]
pitch = utils.mapping(udp.message[1], 1000, 2000, 2000, 1000) # To invert channel, maybe add function
yaw = utils.mapping(udp.message[2], 1000, 2000, 968, 2062) # Map it to match RC configuration
throttle = utils.mapping(udp.message[3], 1000, 2000, 968, 1998) # Map it to match RC configuration
# Simulink automatic control:
if udp.message[7] == 1:
roll = udp.message[12]
pitch = utils.mapping(
udp.message[13], 1000, 2000, 2000, 1000
) # To invert channel, maybe add function
yaw = utils.mapping(udp.message[14], 1000, 2000, 968, 2062) # Map it to match RC configuration
throttle = utils.mapping(udp.message[15], 1000, 2000, 968, 1998) # Map it to match RC configuration
# Vehicle communication
vehicle.channel_override = {"1": roll, "2": pitch, "3": throttle, "4": yaw}
vehicle.flush()
# Debug
# print "%s" % vehicle.attitude
# print "%s" % vehicle.channel_readback
print udp.message
# 100hz loop
while elapsed < 0.01:
elapsed = time.time() - current
# End of the main loop
except Exception, error:
print "Error on sendCommands thread: " + str(error)
sendCommands()
