class MethodCheckButton(Frame):
def __init__(self, master):
Frame.__init__(self, master, width = 15)
self.initComplete = False
self.getVar = IntVar()
self.postVar = IntVar()
self.getCheckButton = Checkbutton(self, \
text = "GET", width = 6, variable = self.getVar, \
command = self.__singleSelectCommandGet, onvalue = 1, offvalue = 0)
self.postCheckButton = Checkbutton(self, \
text = "POST", width = 6, variable = self.postVar, \
command = self.__singleSelectCommandPost, onvalue = 1, offvalue = 0)
self.label = Label(self, text = "Use Method:", padx = 3)
self.__boxComponents()
self.initComplete = True
def __boxComponents(self):
self.label.grid(row = 0, column = 0)
self.getCheckButton.grid(row = 0, column = 1)
self.postCheckButton.grid(row = 0, column = 2)
def __singleSelectCommandGet(self):
if self.initComplete == False:
return
self.postCheckButton.deselect()
def __singleSelectCommandPost(self):
if self.initComplete == False:
return
self.getCheckButton.deselect()
def getCurrentMethod(self):
if self.getVar.get() == 1:
return "get"
if self.postVar.get() == 1:
return "post"
return None
class MethodCheckButton(Frame):
def __init__(self, master):
Frame.__init__(self, master, width=15)
self.initComplete = False
self.getVar = IntVar()
self.postVar = IntVar()
self.getCheckButton = Checkbutton(self, \
text = "GET", width = 6, variable = self.getVar, \
command = self.__singleSelectCommandGet, onvalue = 1, offvalue = 0)
self.postCheckButton = Checkbutton(self, \
text = "POST", width = 6, variable = self.postVar, \
command = self.__singleSelectCommandPost, onvalue = 1, offvalue = 0)
self.label = Label(self, text="Use Method:", padx=3)
self.__boxComponents()
self.initComplete = True
def __boxComponents(self):
self.label.grid(row=0, column=0)
self.getCheckButton.grid(row=0, column=1)
self.postCheckButton.grid(row=0, column=2)
def __singleSelectCommandGet(self):
if self.initComplete == False:
return
self.postCheckButton.deselect()
def __singleSelectCommandPost(self):
if self.initComplete == False:
return
self.getCheckButton.deselect()
def getCurrentMethod(self):
if self.getVar.get() == 1:
return "get"
if self.postVar.get() == 1:
return "post"
return None
class Screen(Observer):
def __init__(self, parent, model_x, model_y, bg="white"):
self.canvas = Canvas(parent, bg=bg)
self.model_x = model_x
self.model_y = model_y
print("parent", parent.cget("width"), parent.cget("height"))
self.showX = True
self.showY = True
self.frame = Frame(parent)
# Signal X
self.magnitude_x = Scale(self.frame,
length=250,
orient="horizontal",
name="m_x",
label="Magnitude X",
sliderlength=20,
showvalue=0,
from_=0,
to=5,
tickinterval=25)
self.frequency_x = Scale(self.frame,
length=250,
orient="horizontal",
name="f_x",
label="Frequency X",
sliderlength=20,
showvalue=0,
from_=0,
to=5,
tickinterval=25)
self.phase_x = Scale(self.frame,
length=250,
orient="horizontal",
name="p_x",
label="Phase X",
sliderlength=20,
showvalue=0,
from_=0,
to=5,
tickinterval=25)
# Signal Y
self.magnitude_y = Scale(self.frame,
length=250,
orient="horizontal",
name="m_y",
label="Magnitude Y",
sliderlength=20,
showvalue=0,
from_=0,
to=5,
tickinterval=25)
self.frequency_y = Scale(self.frame,
length=250,
orient="horizontal",
name="f_y",
label="Frequency Y",
sliderlength=20,
showvalue=0,
from_=0,
to=5,
tickinterval=25)
self.phase_y = Scale(self.frame,
length=250,
orient="horizontal",
name="p_y",
label="Phase Y",
sliderlength=20,
showvalue=0,
from_=0,
to=5,
tickinterval=25)
self.frame2 = Frame(parent, bg="black")
self.varX = IntVar()
self.varY = IntVar()
self.varXY = IntVar()
self.lbl = Label(self.frame2, text="Courbes", fg="black")
# Boutons de sélection (X, Y ou X-Y)
self.caseX = Checkbutton(self.frame2,
text="X",
variable=self.varX,
command=self.getX)
self.caseY = Checkbutton(self.frame2,
text="Y",
variable=self.varY,
command=self.getY)
self.caseXY = Checkbutton(self.frame2,
text="XY",
variable=self.varXY,
command=self.getXY)
self.caseXY.select()
self.wi = self.canvas.cget("width")
self.hi = self.canvas.cget("height")
self.stepx = 0
self.stepy = 0
# Step x
self.step_x = Entry(parent, name="x")
# Step y
self.step_y = Entry(parent, name="y")
def update(self, model):
print("View update")
if model.getId() == 0:
signal = model.get_signal()
self.plot_signal(signal)
elif model.getId() == 1:
signal = model.get_signal()
self.plot_signal(signal, "blue")
else:
raise ("Error")
# Signal X
def get_magnitude(self, whichOne):
if whichOne == 0:
return self.magnitude_x
elif whichOne == 1:
return self.magnitude_y
else:
raise ("Error")
def get_frequency(self, whichOne):
if whichOne == 0:
return self.frequency_x
elif whichOne == 1:
return self.frequency_y
else:
raise ("Error")
def get_phase(self, whichOne):
if whichOne == 0:
return self.phase_x
elif whichOne == 1:
return self.phase_y
else:
raise ("Error")
def get_step_x(self):
return self.step_x
def get_step_y(self):
return self.step_y
def getX(self):
print("update_X(self,event)")
self.caseY.deselect()
self.caseXY.deselect()
self.showX = True
self.showY = False
self.update(self.model_x)
if self.canvas.find_withtag("signal_y"):
self.canvas.delete("signal_y")
def getY(self):
print("update_Y(self,event)")
self.caseX.deselect()
self.caseXY.deselect()
self.showX = False
self.showY = True
self.update(self.model_y)
if self.canvas.find_withtag("signal_x"):
self.canvas.delete("signal_x")
def getXY(self):
print("update_XY(self,event)")
self.caseX.deselect()
self.caseY.deselect()
self.showX = True
self.showY = True
self.update(self.model_x)
self.update(self.model_y)
def plot_signal(self, signal, color="red"):
w, h = self.wi, self.hi
width, height = int(w), int(h)
if color == "red" and self.showX == True:
if self.canvas.find_withtag("signal_x"):
self.canvas.delete("signal_x")
if signal and len(signal) > 1:
plot = [(x * width, height / 2.0 * (y + 1))
for (x, y) in signal]
signal_id = self.canvas.create_line(plot,
fill=color,
smooth=1,
width=3,
tags="signal_x")
elif color == "blue" and self.showY == True:
if self.canvas.find_withtag("signal_y"):
self.canvas.delete("signal_y")
if signal and len(signal) > 1:
plot = [(x * width, height / 2.0 * (y + 1))
for (x, y) in signal]
signal_id = self.canvas.create_line(plot,
fill=color,
smooth=1,
width=3,
tags="signal_y")
def grid(self, step_x, step_y):
w, h = self.wi, self.hi
width, height = int(w), int(h)
self.stepx = (width - 10) / step_x * 1.
self.stepy = (height - 10) / step_y * 1.
for t in range(1, step_x + 2):
x = t * self.stepx
self.canvas.create_line(x, 0, x, height, tags="grid")
#self.canvas.create_line(x,height/2-4,x,height/2+4)
for t in range(1, step_y + 2):
y = t * self.stepy
self.canvas.create_line(0, y, width, y, tags="grid")
#self.canvas.create_line(width/2-4,y,width/2+4,y)
def resize(self, event):
if event:
self.wi = event.width
self.hi = event.height
self.canvas.delete("grid")
self.plot_signal(self.model_x.get_signal())
self.plot_signal(self.model_y.get_signal(), "blue")
self.grid(25, 25)
def packing(self):
self.canvas.pack(fill="both", expand=1)
self.step_x.pack(expand=1, fill="both")
self.step_y.pack(expand=1, fill="both")
self.frame.pack(expand=1, fill="both")
self.magnitude_x.grid(row=0, column=0)
self.magnitude_y.grid(row=0, column=1)
self.frequency_x.grid(row=1, column=0)
self.frequency_y.grid(row=1, column=1)
self.phase_x.grid(row=2, column=0)
self.phase_y.grid(row=2, column=1)
self.frame2.pack(side="bottom", expand=1)
self.lbl.grid(row=0, column=0)
self.caseX.grid(row=0, column=1)
self.caseY.grid(row=0, column=2)
self.caseXY.grid(row=0, column=3)
class MainWindow:
def __init__(self):
self.root = Tk()
self.input_type = Tkinter.IntVar()
self.input_type.set(1)
self.normalize_data = Tkinter.IntVar()
self.normalize_data.set(1)
self.root.title("Code energy calculator")
self.left_frame = LabelFrame(self.root, text="Input and output")
self.left_frame.pack(side=Tkinter.LEFT,
fill=Tkinter.BOTH,
expand=True,
padx=(10, 5),
pady=10)
self.right_frame = LabelFrame(self.root, text="Code")
self.right_frame.pack(side=Tkinter.RIGHT,
fill=Tkinter.BOTH,
expand=True,
padx=(5, 10),
pady=10)
code_hscroll = Scrollbar(self.right_frame, orient=Tkinter.HORIZONTAL)
code_hscroll.pack(side=Tkinter.BOTTOM, fill=Tkinter.X)
code_vscroll = Scrollbar(self.right_frame)
code_vscroll.pack(side=Tkinter.RIGHT, fill=Tkinter.Y)
self.code_text = Text(self.right_frame,
wrap=Tkinter.NONE,
xscrollcommand=code_hscroll.set,
yscrollcommand=code_vscroll.set)
self.code_text.pack()
self.code_text.insert(Tkinter.INSERT, DEFAULT_CODE)
code_hscroll.config(command=self.code_text.xview)
code_vscroll.config(command=self.code_text.yview)
self.input_file_entry =\
self.create_and_add_file_field(self.left_frame, "Input file", 5, False)
self.spherical_coord_option =\
Radiobutton(self.left_frame, text="Spherical coordinates",
variable=self.input_type, value=1)
self.spherical_coord_option.pack(anchor=Tkinter.W)
self.cartesian_coord_option =\
Radiobutton(self.left_frame, text="Cartesian coordinates",
variable=self.input_type, value=2)
self.cartesian_coord_option.pack(anchor=Tkinter.W)
self.spherical_coord_option.select()
self.output_file_entry =\
self.create_and_add_file_field(self.left_frame, "Output file", 5, True)
self.normalize_check = Checkbutton(self.left_frame,
text="Normalize data",
variable=self.normalize_data,
offvalue=0,
onvalue=1)
self.normalize_check.pack()
self.normalize_check.deselect()
self.do_button = Button(self.left_frame, text="Run", command=self.run)
self.do_button.pack(side=Tkinter.BOTTOM, pady=(0, 10))
def create_and_add_file_field(self, parent, title, pad, is_save):
title_label = Label(parent, text=title)
title_label.pack(side=Tkinter.TOP, padx=pad)
container_frame = Frame(parent)
container_frame.pack(side=Tkinter.TOP, padx=pad, pady=(0, pad))
filename_entry = Entry(container_frame)
filename_entry.pack(side=Tkinter.LEFT)
browse_button = \
Button(container_frame, text="Browse...",
command=lambda: self.select_file(filename_entry, is_save))
browse_button.pack(side=Tkinter.RIGHT)
return filename_entry
@staticmethod
def select_file(text_field, is_save):
text_field.delete(0, Tkinter.END)
if is_save:
filename = asksaveasfilename()
else:
filename = askopenfilename()
text_field.insert(0, filename)
def run(self):
input_fname = self.input_file_entry.get()
output_fname = self.output_file_entry.get()
code = self.code_text.get(1.0, Tkinter.END)
do_work(input_fname, output_fname, code, self.input_type.get(),
self.normalize_data.get())
def show(self):
self.root.mainloop()
class BoardConfigAdvance(Frame):
def __init__(self, master=None, main=None):
Frame.__init__(self, master)
self.parent = master
self.main = main
self.parent.geometry("280x174")
self.parent.title(os.getenv("NAME") + " - Advance Board Config")
self.master.configure(padx=10, pady=10)
self.HEAPSIZE = {"512 byte": 512,
"1024 byte": 1024,}
self.OPTIMIZATION = "-O2 -O3 -Os".split()
self.mips16_var = IntVar()
self.checkBox_mips16 = Checkbutton(self.parent, text="Mips16", anchor="w", variable=self.mips16_var)
self.checkBox_mips16.pack(expand=True, fill=BOTH, side=TOP)
frame_heap = Frame(self.parent)
Label(frame_heap, text="Heap size:", anchor="w", width=12).pack(side=LEFT, fill=X, expand=True)
self.comboBox_heapsize = Combobox(frame_heap, values=self.HEAPSIZE.keys())
self.comboBox_heapsize.pack(fill=X, expand=True, side=RIGHT)
frame_heap.pack(fill=X, expand=True, side=TOP)
frame_opt = Frame(self.parent)
Label(frame_opt, text="Optimization:", anchor="w", width=12).pack(side=LEFT, fill=X, expand=True)
self.comboBox_optimization = Combobox(frame_opt, values=self.OPTIMIZATION)
self.comboBox_optimization.pack(fill=X, expand=True, side=RIGHT)
frame_opt.pack(fill=X, expand=True, side=TOP)
frame_buttons = Frame(self.parent)
Button(frame_buttons, text="Accept", command=self.accept_config).pack(fill=X, expand=True, side=LEFT)
Button(frame_buttons, text="Restore Default", command=self.restore_default).pack(fill=X, expand=True, side=RIGHT)
frame_buttons.pack(fill=X, expand=True, side=BOTTOM)
self.load_config()
#----------------------------------------------------------------------
def quit(self):
self.master.destroy()
#----------------------------------------------------------------------
def load_config(self):
#Called in the parent frame
#self.main.configIDE.load_config()
if self.main.configIDE.config("Board", "mips16", True):
self.checkBox_mips16.select()
else:
self.checkBox_mips16.deselect()
heapsize = self.main.configIDE.config("Board", "heapsize", 512)
for key in self.HEAPSIZE.keys():
if self.HEAPSIZE[key] == heapsize:
self.comboBox_heapsize.set(key)
break
optimization = self.main.configIDE.config("Board", "optimization", "-O3")
self.comboBox_optimization.set(optimization)
#----------------------------------------------------------------------
def restore_default(self):
self.checkBox_mips16.select()
self.comboBox_heapsize.set(self.HEAPSIZE.keys()[1])
self.comboBox_optimization.set(self.OPTIMIZATION[1])
#----------------------------------------------------------------------
def accept_config(self):
self.save_config()
self.main.configIDE.save_config()
self.quit()
#----------------------------------------------------------------------
def save_config(self):
self.main.configIDE.set("Board", "mips16", self.mips16_var.get()==1)
heapsize = self.HEAPSIZE[self.comboBox_heapsize.get()]
self.main.configIDE.set("Board", "heapsize", heapsize)
self.main.configIDE.set("Board", "optimization", self.comboBox_optimization.get())
class Cockpit(ttkFrame):
'''
Remote device GUI
'''
#TODO: 20160415 DPM - Set these values from configuration file
#--- config
THROTTLE_BY_USER = True
THROTTLE_RESOLUTION = 0.1
# Joystick enabled or not, if any
JOYSTICK_ENABLED = True
DEFAULT_DRONE_IP = "192.168.1.130"
DEFAULT_DRONE_PORT = 2121
#--- end config
KEY_ANG_SPEED = "ang-speed"
KEY_ANGLES = "angles"
KEY_ACCEL = "accel"
PID_KEYS = ["P", "I", "D"]
DIR_NONE = 0
DIR_VERTICAL = 1
DIR_HORIZONTAL = 2
def __init__(self, parent, isDummy = False, droneIp = DEFAULT_DRONE_IP, dronePort = DEFAULT_DRONE_PORT):
'''
Constructor
'''
ttkFrame.__init__(self, parent)
self._target = [0.0] * 4
self._selectedPidConstats = "--"
self._pidConstants = {
Cockpit.KEY_ANG_SPEED:{
"X":{
"P": 0.0,
"I": 0.0,
"D": 0.0
},
"Y":{
"P": 0.0,
"I": 0.0,
"D": 0.0
},
"Z":{
"P": 0.0,
"I": 0.0,
"D": 0.0
}
},
Cockpit.KEY_ANGLES: {
"X":{
"P": 0.0,
"I": 0.0,
"D": 0.0
},
"Y":{
"P": 0.0,
"I": 0.0,
"D": 0.0
}
},
Cockpit.KEY_ACCEL:{
"X":{
"P": 0.0,
"I": 0.0,
"D": 0.0
},
"Y":{
"P": 0.0,
"I": 0.0,
"D": 0.0
},
"Z":{
"P": 0.0,
"I": 0.0,
"D": 0.0
}
}
}
self.parent = parent
self.initUI()
self._controlKeysLocked = False
if not isDummy:
self._link = INetLink(droneIp, dronePort)
else:
self._link = ConsoleLink()
self._link.open()
self._updateInfoThread = Thread(target=self._updateInfo)
self._updateInfoThreadRunning = False
self._readingState = False
self._start()
def initUI(self):
self.parent.title("Drone control")
self.style = Style()
self.style.theme_use("default")
self.pack(fill=BOTH, expand=1)
self.parent.bind_all("<Key>", self._keyDown)
self.parent.bind_all("<KeyRelease>", self._keyUp)
if system() == "Linux":
self.parent.bind_all("<Button-4>", self._onMouseWheelUp)
self.parent.bind_all("<Button-5>", self._onMouseWheelDown)
else:
#case of Windows
self.parent.bind_all("<MouseWheel>", self._onMouseWheel)
#Commands
commandsFrame = tkFrame(self)
commandsFrame.grid(column=0, row=0, sticky="WE")
self._started = IntVar()
self._startedCB = Checkbutton(commandsFrame, text="On", variable=self._started, command=self._startedCBChanged)
self._startedCB.pack(side=LEFT, padx=4)
# self._integralsCB = Checkbutton(commandsFrame, text="Int.", variable=self._integralsEnabled, \
# command=self._integralsCBChanged, state=DISABLED)
# self._integralsCB.pack(side=LEFT, padx=4)
self._quitButton = Button(commandsFrame, text="Quit", command=self.exit)
self._quitButton.pack(side=LEFT, padx=2, pady=2)
# self._angleLbl = Label(commandsFrame, text="Angle")
# self._angleLbl.pack(side=LEFT, padx=4)
#
# self._angleEntry = Entry(commandsFrame, state=DISABLED)
# self._angleEntry.pack(side=LEFT)
#Info
infoFrame = tkFrame(self)
infoFrame.grid(column=1, row=1, sticky="NE", padx=4)
#Throttle
Label(infoFrame, text="Throttle").grid(column=0, row=0, sticky="WE")
self._throttleTexts = [StringVar(),StringVar(),StringVar(),StringVar()]
Entry(infoFrame, textvariable=self._throttleTexts[3], state=DISABLED, width=5).grid(column=0, row=1)
Entry(infoFrame, textvariable=self._throttleTexts[0], state=DISABLED, width=5).grid(column=1, row=1)
Entry(infoFrame, textvariable=self._throttleTexts[2], state=DISABLED, width=5).grid(column=0, row=2)
Entry(infoFrame, textvariable=self._throttleTexts[1], state=DISABLED, width=5).grid(column=1, row=2)
#Angles
Label(infoFrame, text="Angles").grid(column=0, row=3, sticky="WE")
self._angleTexts = [StringVar(),StringVar(),StringVar()]
for index in range(3):
Entry(infoFrame, textvariable=self._angleTexts[index], state=DISABLED, width=5).grid(column=index, row=4)
#Accels
Label(infoFrame, text="Accels").grid(column=0, row=5, sticky="WE")
self._accelTexts = [StringVar(),StringVar(),StringVar()]
for index in range(3):
Entry(infoFrame, textvariable=self._accelTexts[index], state=DISABLED, width=5).grid(column=index, row=6)
#Speeds
Label(infoFrame, text="Speeds").grid(column=0, row=7, sticky="WE")
self._speedTexts = [StringVar(),StringVar(),StringVar()]
for index in range(3):
Entry(infoFrame, textvariable=self._speedTexts[index], state=DISABLED, width=5).grid(column=index, row=8)
#Height
Label(infoFrame, text="Height").grid(column=0, row=9, sticky="E")
self._heightText = StringVar()
Entry(infoFrame, textvariable=self._heightText, state=DISABLED, width=5).grid(column=1, row=9)
#Loop rate
Label(infoFrame, text="Loop @").grid(column=0, row=10, sticky="E")
self._loopRateText = StringVar()
Entry(infoFrame, textvariable=self._loopRateText, state=DISABLED, width=5).grid(column=1, row=10)
Label(infoFrame, text="Hz").grid(column=2, row=10, sticky="W")
#control
controlFrame = tkFrame(self)
controlFrame.grid(column=0, row=1, sticky="W")
self._throttle = DoubleVar()
if Cockpit.THROTTLE_BY_USER:
self._thrustScale = Scale(controlFrame, orient=VERTICAL, from_=100.0, to=0.0, \
tickinterval=0, variable=self._throttle, resolution=Cockpit.THROTTLE_RESOLUTION, \
length=200, showvalue=1, \
state=DISABLED,
command=self._onThrustScaleChanged)
else:
self._thrustScale = Scale(controlFrame, orient=VERTICAL, from_=100.0, to=-100.0, \
tickinterval=0, variable=self._throttle, \
length=200, showvalue=1, \
state=DISABLED,
command=self._onThrustScaleChanged)
self._thrustScale.bind("<Double-Button-1>", self._onThrustScaleDoubleButton1, "+")
self._thrustScale.grid(column=0)
self._shiftCanvas = Canvas(controlFrame, bg="white", height=400, width=400, \
relief=SUNKEN)
self._shiftCanvas.bind("<Button-1>", self._onMouseButton1)
#self._shiftCanvas.bind("<ButtonRelease-1>", self._onMouseButtonRelease1)
self._shiftCanvas.bind("<B1-Motion>", self._onMouseButton1Motion)
self._shiftCanvas.bind("<Double-Button-1>", self._onMouseDoubleButton1)
self._shiftCanvas.bind("<Button-3>", self._onMouseButton3)
#self._shiftCanvas.bind("<ButtonRelease-3>", self._onMouseButtonRelease3)
self._shiftCanvas.bind("<B3-Motion>", self._onMouseButton3Motion)
self._shiftCanvas.grid(row=0,column=1, padx=2, pady=2)
self._shiftCanvas.create_oval(1, 1, 400, 400, outline="#ff0000")
self._shiftCanvas.create_line(200, 2, 200, 400, fill="#ff0000")
self._shiftCanvas.create_line(2, 200, 400, 200, fill="#ff0000")
self._shiftMarker = self._shiftCanvas.create_oval(196, 196, 204, 204, outline="#0000ff", fill="#0000ff")
self._yaw = DoubleVar()
self._yawScale = Scale(controlFrame, orient=HORIZONTAL, from_=-100.0, to=100.0, \
tickinterval=0, variable=self._yaw, \
length=200, showvalue=1, \
command=self._onYawScaleChanged)
self._yawScale.bind("<Double-Button-1>", self._onYawScaleDoubleButton1, "+")
self._yawScale.grid(row=1, column=1)
self._controlKeyActive = False
#PID calibration
pidCalibrationFrame = tkFrame(self)
pidCalibrationFrame.grid(column=0, row=2, sticky="WE");
self._pidSelected = StringVar()
self._pidSelected.set("--")
self._pidListBox = OptionMenu(pidCalibrationFrame, self._pidSelected, "--", \
Cockpit.KEY_ANG_SPEED, Cockpit.KEY_ANGLES, Cockpit.KEY_ACCEL, \
command=self._onPidListBoxChanged)
self._pidListBox.pack(side=LEFT, padx=2)
self._pidListBox.config(width=10)
self._axisSelected = StringVar()
self._axisSelected.set("--")
self._axisListBox = OptionMenu(pidCalibrationFrame, self._axisSelected, "--", "X", "Y", "Z", \
command=self._onAxisListBoxChanged)
self._axisListBox.pack(side=LEFT, padx=2)
self._axisListBox.config(state=DISABLED)
Label(pidCalibrationFrame, text="P").pack(side=LEFT, padx=(14, 2))
self._pidPString = StringVar()
self._pidPString.set("0.00")
self._pidPSpinbox = Spinbox(pidCalibrationFrame, width=5, from_=0.0, to=10000.0, increment=0.01, state=DISABLED, \
textvariable=self._pidPString, command=self._onPidSpinboxChanged)
self._pidPSpinbox.pack(side=LEFT, padx=2)
Label(pidCalibrationFrame, text="I").pack(side=LEFT, padx=(14, 2))
self._pidIString = StringVar()
self._pidIString.set("0.00")
self._pidISpinbox = Spinbox(pidCalibrationFrame, width=5, from_=0.0, to=10000.0, increment=0.01, state=DISABLED, \
textvariable=self._pidIString, command=self._onPidSpinboxChanged)
self._pidISpinbox.pack(side=LEFT, padx=2)
Label(pidCalibrationFrame, text="D").pack(side=LEFT, padx=(14, 2))
self._pidDString = StringVar()
self._pidDString.set("0.00")
self._pidDSpinbox = Spinbox(pidCalibrationFrame, width=5, from_=0.0, to=10000.0, increment=0.01, state=DISABLED, \
textvariable=self._pidDString, command=self._onPidSpinboxChanged)
self._pidDSpinbox.pack(side=LEFT, padx=2)
#debug
debugFrame = tkFrame(self)
debugFrame.grid(column=0, row=3, sticky="WE")
self._debugMsg = Message(debugFrame, anchor="nw", justify=LEFT, relief=SUNKEN, width=300)
self._debugMsg.pack(fill=BOTH, expand=1)
def _start(self):
self._readDroneConfig()
if Cockpit.JOYSTICK_ENABLED:
self._joystickManager = JoystickManager.getInstance()
self._joystickManager.start()
joysticks = self._joystickManager.getJoysticks()
if len(joysticks) != 0:
self._joystick = joysticks[0]
self._joystick.onAxisChanged += self._onJoystickAxisChanged
self._joystick.onButtonPressed += self._onJoystickButtonPressed
else:
self._joystick = None
def _onJoystickAxisChanged(self, sender, index):
if self._started.get() and sender == self._joystick:
axisValue = self._joystick.getAxisValue(index)
if index == 0:
self._yaw.set(axisValue)
self._updateTarget()
elif index == 1 and not Cockpit.THROTTLE_BY_USER:
thrust = -axisValue
self._throttle.set(thrust)
self._updateTarget()
elif index == 2 and Cockpit.THROTTLE_BY_USER:
rowThrottle = (axisValue + 100.0)/2.0
if rowThrottle < 10.0:
throttle = rowThrottle * 6.0
elif rowThrottle < 90.0:
throttle = 60.0 + ((rowThrottle - 10.0) / 8.0)
else:
throttle = 70.0 + (rowThrottle - 90.0) * 3.0
self._throttle.set(throttle)
self._sendThrottle()
elif index == 3:
x = 196 + axisValue * 2
lastCoords = self._shiftCanvas.coords(self._shiftMarker)
coords = (x, lastCoords[1])
self._plotShiftCanvasMarker(coords)
elif index == 4:
y = 196 + axisValue * 2
lastCoords = self._shiftCanvas.coords(self._shiftMarker)
coords = (lastCoords[0], y)
self._plotShiftCanvasMarker(coords)
def _onJoystickButtonPressed(self, sender, index):
if sender == self._joystick and index == 7:
if self._started.get() == 0:
self._startedCB.select()
else:
self._startedCB.deselect()
# Tkinter's widgets seem not to be calling the event-handler
# when they are changed programmatically. Therefore, the
# even-handler is called explicitly here.
self._startedCBChanged()
def exit(self):
self._link.send({"key": "close", "data": None})
self._stopUpdateInfoThread()
self._link.close()
if Cockpit.JOYSTICK_ENABLED:
self._joystickManager.stop()
self.quit()
def _updateTarget(self):
markerCoords = self._shiftCanvas.coords(self._shiftMarker)
coords = ((markerCoords[0] + markerCoords[2]) / 2, (markerCoords[1] + markerCoords[3]) / 2)
self._target[0] = float(coords[1] - 200) / 2.0 # X-axis angle / X-axis acceleration
self._target[1] = float(coords[0] - 200) / 2.0 # Y-axis angle / Y-axis acceleration
#Remote control uses clockwise angle, but the drone's referece system uses counter-clockwise angle
self._target[2] = -self._yaw.get() # Z-axis angular speed
# Z-axis acceleration (thrust). Only when the motor throttle is not controlled by user directly
if Cockpit.THROTTLE_BY_USER:
self._target[3] = 0.0
else:
self._target[3] = self._throttle.get()
self._sendTarget()
def _keyDown(self, event):
if event.keysym == "Escape":
self._throttle.set(0)
self._started.set(0)
self._thrustScale.config(state=DISABLED)
self._stopUpdateInfoThread()
self._sendIsStarted()
elif event.keysym.startswith("Control"):
self._controlKeyActive = True
elif not self._controlKeysLocked and self._controlKeyActive:
if event.keysym == "Up":
self._thrustScaleUp()
elif event.keysym == "Down":
self._thrustScaleDown()
elif event.keysym == "Left":
self._yawLeft()
elif event.keysym == "Right":
self._yawRight()
elif event.keysym == "space":
self._yawReset()
if not Cockpit.THROTTLE_BY_USER:
self._thrustReset()
elif not self._controlKeysLocked and not self._controlKeyActive:
if event.keysym == "Up":
self._moveShiftCanvasMarker((0,-5))
elif event.keysym == "Down":
self._moveShiftCanvasMarker((0,5))
elif event.keysym == "Left":
self._moveShiftCanvasMarker((-5,0))
elif event.keysym == "Right":
self._moveShiftCanvasMarker((5,0))
elif event.keysym == "space":
self._resetShiftCanvasMarker()
def _keyUp(self, eventArgs):
if eventArgs.keysym.startswith("Control"):
self._controlKeyActive = False
def _onMouseButton1(self, eventArgs):
self._lastMouseCoords = (eventArgs.x, eventArgs.y)
def _onMouseButtonRelease1(self, eventArgs):
self._shiftCanvas.coords(self._shiftMarker, 196, 196, 204, 204)
def _limitCoordsToSize(self, coords, size, width):
maxSize = size-(width/2.0)
minSize = -(width/2.0)
if coords[0] > maxSize:
x = maxSize
elif coords[0] < minSize:
x = minSize
else:
x = coords[0]
if coords[1] > maxSize:
y = maxSize
elif coords[1] < minSize:
y = minSize
else:
y = coords[1]
return (x,y)
def _plotShiftCanvasMarker(self, coords):
coords = self._limitCoordsToSize(coords, 400, 8)
self._shiftCanvas.coords(self._shiftMarker, coords[0], coords[1], coords[0] + 8, coords[1] + 8)
self._updateTarget()
def _moveShiftCanvasMarker(self, shift):
lastCoords = self._shiftCanvas.coords(self._shiftMarker)
newCoords = (lastCoords[0] + shift[0], lastCoords[1] + shift[1])
self._plotShiftCanvasMarker(newCoords)
def _resetShiftCanvasMarker(self):
self._shiftCanvas.coords(self._shiftMarker, 196, 196, 204, 204)
self._updateTarget()
def _onMouseButton1Motion(self, eventArgs):
deltaCoords = (eventArgs.x - self._lastMouseCoords[0], eventArgs.y - self._lastMouseCoords[1])
self._moveShiftCanvasMarker(deltaCoords)
self._lastMouseCoords = (eventArgs.x, eventArgs.y)
def _onMouseDoubleButton1(self, eventArgs):
self._resetShiftCanvasMarker()
def _onMouseButton3(self, eventArgs):
self._lastMouseCoords = (eventArgs.x, eventArgs.y)
self._mouseDirection = Cockpit.DIR_NONE
def _onMouseButtonRelease3(self, eventArgs):
self._shiftCanvas.coords(self._shiftMarker, 196, 196, 204, 204)
def _onMouseButton3Motion(self, eventArgs):
deltaCoords = (eventArgs.x - self._lastMouseCoords[0], eventArgs.y - self._lastMouseCoords[1])
if self._mouseDirection == Cockpit.DIR_NONE:
if abs(deltaCoords[0]) > abs(deltaCoords[1]):
self._mouseDirection = Cockpit.DIR_HORIZONTAL
else:
self._mouseDirection = Cockpit.DIR_VERTICAL
if self._mouseDirection == Cockpit.DIR_HORIZONTAL:
deltaCoords = (deltaCoords[0], 0)
else:
deltaCoords = (0, deltaCoords[1])
self._moveShiftCanvasMarker(deltaCoords)
self._lastMouseCoords = (eventArgs.x, eventArgs.y)
def _thrustScaleUp(self):
#TODO: 20160526 DPM: El valor de incremento de aceleración (1.0) puede ser muy alto
if self._started.get():
newValue = self._thrustScale.get() \
+ (Cockpit.THROTTLE_RESOLUTION if Cockpit.THROTTLE_BY_USER else 1.0)
self._thrustScale.set(newValue)
self._updateTarget()
def _thrustScaleDown(self):
#TODO: 20160526 DPM: El valor de decremento de aceleración (1.0) puede ser muy alto
if self._started.get():
newValue = self._thrustScale.get() \
- (Cockpit.THROTTLE_RESOLUTION if Cockpit.THROTTLE_BY_USER else 1.0)
self._thrustScale.set(newValue)
self._updateTarget()
def _thrustReset(self):
if self._started.get():
self._thrustScale.set(0.0)
self._updateTarget()
def _onThrustScaleDoubleButton1(self, eventArgs):
self._thrustReset()
return "break"
def _yawRight(self):
newValue = self._yaw.get() + 1
self._yaw.set(newValue)
self._updateTarget()
def _yawLeft(self):
newValue = self._yaw.get() - 1
self._yaw.set(newValue)
self._updateTarget()
def _yawReset(self):
self._yaw.set(0)
self._updateTarget()
def _onMouseWheelUp(self, eventArgs):
if not self._controlKeyActive:
self._thrustScaleUp()
else:
self._yawRight()
def _onMouseWheelDown(self, eventArgs):
if not self._controlKeyActive:
self._thrustScaleDown()
else:
self._yawLeft()
def _onMouseWheel(self, eventArgs):
factor = eventArgs.delta/(1200.0 if Cockpit.THROTTLE_BY_USER and not self._controlKeyActive else 120.0)
if not self._controlKeyActive:
if self._started.get():
newValue = self._thrustScale.get() + factor
self._thrustScale.set(newValue)
self._updateTarget()
else:
newValue = self._yaw.get() + factor
self._yaw.set(newValue)
self._updateTarget()
def _onYawScaleChanged(self, eventArgs):
self._updateTarget()
def _onYawScaleDoubleButton1(self, eventArgs):
self._yawReset()
return "break"
def _startedCBChanged(self):
if not self._started.get():
self._throttle.set(0)
self._thrustScale.config(state=DISABLED)
#self._integralsCB.config(state=DISABLED)
self._stopUpdateInfoThread()
else:
self._thrustScale.config(state="normal")
#self._integralsCB.config(state="normal")
self._startUpdateInfoThread()
self._sendIsStarted()
# def _integralsCBChanged(self):
#
# self._link.send({"key": "integrals", "data":self._integralsEnabled.get() != 0})
#
def _onThrustScaleChanged(self, eventArgs):
if Cockpit.THROTTLE_BY_USER:
self._sendThrottle()
else:
self._updateTarget()
def _sendThrottle(self):
self._link.send({"key": "throttle", "data": self._throttle.get()})
def _sendTarget(self):
self._link.send({"key": "target", "data": self._target})
def _sendIsStarted(self):
isStarted = self._started.get() != 0
self._link.send({"key": "is-started", "data": isStarted})
def _sendPidCalibrationData(self):
if self._pidSelected.get() != "--" and self._axisSelected.get() != "--":
pidData = {
"pid": self._pidSelected.get(),
"axis": self._axisSelected.get(),
"p": float(self._pidPSpinbox.get()),
"i": float(self._pidISpinbox.get()),
"d": float(self._pidDSpinbox.get())}
self._link.send({"key": "pid-calibration", "data": pidData})
def _updatePidCalibrationData(self):
pid = self._pidSelected.get()
axis = self._axisSelected.get()
if pid != "--" and axis != "--":
self._pidConstants[pid][axis]["P"] = float(self._pidPSpinbox.get())
self._pidConstants[pid][axis]["I"] = float(self._pidISpinbox.get())
self._pidConstants[pid][axis]["D"] = float(self._pidDSpinbox.get())
def _readDroneConfig(self):
self._link.send({"key": "read-drone-config", "data": None}, self._onDroneConfigRead)
def _readDroneState(self):
if not self._readingState:
self._readingState = True
self._link.send({"key": "read-drone-state", "data": None}, self._onDroneStateRead)
def _readPidConfigItem(self, message, cockpitKey, axises, configKeys):
for i in range(len(axises)):
for j in range(len(Cockpit.PID_KEYS)):
self._pidConstants[cockpitKey][axises[i]][Cockpit.PID_KEYS[j]] = message[configKeys[j]][i]
def _onDroneConfigRead(self, message):
#TODO Show current configuration within the GUI (at least relevant settings)
if message:
#Angle-speeds
self._readPidConfigItem(message, Cockpit.KEY_ANG_SPEED, ["X", "Y", "Z"], \
[Configuration.PID_ANGLES_SPEED_KP, \
Configuration.PID_ANGLES_SPEED_KI, \
Configuration.PID_ANGLES_SPEED_KD])
#Angles
self._readPidConfigItem(message, Cockpit.KEY_ANGLES, ["X", "Y"], \
[Configuration.PID_ANGLES_KP, \
Configuration.PID_ANGLES_KI, \
Configuration.PID_ANGLES_KD])
#Accels
self._readPidConfigItem(message, Cockpit.KEY_ACCEL, ["X", "Y", "Z"], \
[Configuration.PID_ACCEL_KP, \
Configuration.PID_ACCEL_KI, \
Configuration.PID_ACCEL_KD])
def _onDroneStateRead(self, state):
if state:
for index in range(4):
self._throttleTexts[index].set("{0:.3f}".format(state["_throttles"][index]))
for index in range(3):
self._accelTexts[index].set("{0:.3f}".format(state["_accels"][index]))
self._angleTexts[index].set("{0:.3f}".format(state["_angles"][index]))
currentPeriod = state["_currentPeriod"]
if currentPeriod > 0.0:
freq = 1.0/currentPeriod
self._loopRateText.set("{0:.3f}".format(freq))
else:
self._loopRateText.set("--")
else:
self._stopUpdateInfoThread()
self._readingState = False
def _onPidSpinboxChanged(self):
self._updatePidCalibrationData()
self._sendPidCalibrationData()
def _onPidListBoxChanged(self, pid):
self._axisSelected.set("--")
self._pidPString.set("--")
self._pidIString.set("--")
self._pidDString.set("--")
self._pidPSpinbox.config(state=DISABLED)
self._pidISpinbox.config(state=DISABLED)
self._pidDSpinbox.config(state=DISABLED)
self._selectedPidConstats = pid
if pid == "--":
self._axisListBox.config(state=DISABLED)
self._controlKeysLocked = False
else:
self._axisListBox.config(state="normal")
self._controlKeysLocked = True
def _onAxisListBoxChanged(self, axis):
if axis == "--" or (self._selectedPidConstats == Cockpit.KEY_ANGLES and axis == "Z"):
self._pidPString.set("--")
self._pidIString.set("--")
self._pidDString.set("--")
self._pidPSpinbox.config(state=DISABLED)
self._pidISpinbox.config(state=DISABLED)
self._pidDSpinbox.config(state=DISABLED)
self._controlKeysLocked = axis != "--"
else:
self._pidPString.set("{:.2f}".format(self._pidConstants[self._selectedPidConstats][axis]["P"]))
self._pidIString.set("{:.2f}".format(self._pidConstants[self._selectedPidConstats][axis]["I"]))
self._pidDString.set("{:.2f}".format(self._pidConstants[self._selectedPidConstats][axis]["D"]))
self._pidPSpinbox.config(state="normal")
self._pidISpinbox.config(state="normal")
self._pidDSpinbox.config(state="normal")
self._controlKeysLocked = True
def _updateInfo(self):
while self._updateInfoThreadRunning:
self._readDroneState()
time.sleep(1.0)
def _startUpdateInfoThread(self):
self._updateInfoThreadRunning = True
if not self._updateInfoThread.isAlive():
self._updateInfoThread.start()
def _stopUpdateInfoThread(self):
self._updateInfoThreadRunning = False
if self._updateInfoThread.isAlive():
self._updateInfoThread.join()
class Cockpit(ttkFrame):
'''
Remote device GUI
'''
#TODO: 20160415 DPM - Set these values from configuration file
#--- config
THROTTLE_BY_USER = True
THROTTLE_RESOLUTION = 0.1
# Joystick enabled or not, if any
JOYSTICK_ENABLED = True
DEFAULT_DRONE_IP = "192.168.1.130"
DEFAULT_DRONE_PORT = 2121
#--- end config
KEY_ANG_SPEED = "ang-speed"
KEY_ANGLES = "angles"
KEY_ACCEL = "accel"
PID_KEYS = ["P", "I", "D"]
DIR_NONE = 0
DIR_VERTICAL = 1
DIR_HORIZONTAL = 2
def __init__(self,
parent,
isDummy=False,
droneIp=DEFAULT_DRONE_IP,
dronePort=DEFAULT_DRONE_PORT):
'''
Constructor
'''
ttkFrame.__init__(self, parent)
self._target = [0.0] * 4
self._selectedPidConstats = "--"
self._pidConstants = {
Cockpit.KEY_ANG_SPEED: {
"X": {
"P": 0.0,
"I": 0.0,
"D": 0.0
},
"Y": {
"P": 0.0,
"I": 0.0,
"D": 0.0
},
"Z": {
"P": 0.0,
"I": 0.0,
"D": 0.0
}
},
Cockpit.KEY_ANGLES: {
"X": {
"P": 0.0,
"I": 0.0,
"D": 0.0
},
"Y": {
"P": 0.0,
"I": 0.0,
"D": 0.0
}
},
Cockpit.KEY_ACCEL: {
"X": {
"P": 0.0,
"I": 0.0,
"D": 0.0
},
"Y": {
"P": 0.0,
"I": 0.0,
"D": 0.0
},
"Z": {
"P": 0.0,
"I": 0.0,
"D": 0.0
}
}
}
self.parent = parent
self.initUI()
self._controlKeysLocked = False
if not isDummy:
self._link = INetLink(droneIp, dronePort)
else:
self._link = ConsoleLink()
self._link.open()
self._updateInfoThread = Thread(target=self._updateInfo)
self._updateInfoThreadRunning = False
self._readingState = False
self._start()
def initUI(self):
self.parent.title("Drone control")
self.style = Style()
self.style.theme_use("default")
self.pack(fill=BOTH, expand=1)
self.parent.bind_all("<Key>", self._keyDown)
self.parent.bind_all("<KeyRelease>", self._keyUp)
if system() == "Linux":
self.parent.bind_all("<Button-4>", self._onMouseWheelUp)
self.parent.bind_all("<Button-5>", self._onMouseWheelDown)
else:
#case of Windows
self.parent.bind_all("<MouseWheel>", self._onMouseWheel)
#Commands
commandsFrame = tkFrame(self)
commandsFrame.grid(column=0, row=0, sticky="WE")
self._started = IntVar()
self._startedCB = Checkbutton(commandsFrame,
text="On",
variable=self._started,
command=self._startedCBChanged)
self._startedCB.pack(side=LEFT, padx=4)
# self._integralsCB = Checkbutton(commandsFrame, text="Int.", variable=self._integralsEnabled, \
# command=self._integralsCBChanged, state=DISABLED)
# self._integralsCB.pack(side=LEFT, padx=4)
self._quitButton = Button(commandsFrame,
text="Quit",
command=self.exit)
self._quitButton.pack(side=LEFT, padx=2, pady=2)
# self._angleLbl = Label(commandsFrame, text="Angle")
# self._angleLbl.pack(side=LEFT, padx=4)
#
# self._angleEntry = Entry(commandsFrame, state=DISABLED)
# self._angleEntry.pack(side=LEFT)
#Info
infoFrame = tkFrame(self)
infoFrame.grid(column=1, row=1, sticky="NE", padx=4)
#Throttle
Label(infoFrame, text="Throttle").grid(column=0, row=0, sticky="WE")
self._throttleTexts = [
StringVar(), StringVar(),
StringVar(), StringVar()
]
Entry(infoFrame,
textvariable=self._throttleTexts[3],
state=DISABLED,
width=5).grid(column=0, row=1)
Entry(infoFrame,
textvariable=self._throttleTexts[0],
state=DISABLED,
width=5).grid(column=1, row=1)
Entry(infoFrame,
textvariable=self._throttleTexts[2],
state=DISABLED,
width=5).grid(column=0, row=2)
Entry(infoFrame,
textvariable=self._throttleTexts[1],
state=DISABLED,
width=5).grid(column=1, row=2)
#Angles
Label(infoFrame, text="Angles").grid(column=0, row=3, sticky="WE")
self._angleTexts = [StringVar(), StringVar(), StringVar()]
for index in range(3):
Entry(infoFrame,
textvariable=self._angleTexts[index],
state=DISABLED,
width=5).grid(column=index, row=4)
#Accels
Label(infoFrame, text="Accels").grid(column=0, row=5, sticky="WE")
self._accelTexts = [StringVar(), StringVar(), StringVar()]
for index in range(3):
Entry(infoFrame,
textvariable=self._accelTexts[index],
state=DISABLED,
width=5).grid(column=index, row=6)
#Speeds
Label(infoFrame, text="Speeds").grid(column=0, row=7, sticky="WE")
self._speedTexts = [StringVar(), StringVar(), StringVar()]
for index in range(3):
Entry(infoFrame,
textvariable=self._speedTexts[index],
state=DISABLED,
width=5).grid(column=index, row=8)
#Height
Label(infoFrame, text="Height").grid(column=0, row=9, sticky="E")
self._heightText = StringVar()
Entry(infoFrame,
textvariable=self._heightText,
state=DISABLED,
width=5).grid(column=1, row=9)
#Loop rate
Label(infoFrame, text="Loop @").grid(column=0, row=10, sticky="E")
self._loopRateText = StringVar()
Entry(infoFrame,
textvariable=self._loopRateText,
state=DISABLED,
width=5).grid(column=1, row=10)
Label(infoFrame, text="Hz").grid(column=2, row=10, sticky="W")
#control
controlFrame = tkFrame(self)
controlFrame.grid(column=0, row=1, sticky="W")
self._throttle = DoubleVar()
if Cockpit.THROTTLE_BY_USER:
self._thrustScale = Scale(controlFrame, orient=VERTICAL, from_=100.0, to=0.0, \
tickinterval=0, variable=self._throttle, resolution=Cockpit.THROTTLE_RESOLUTION, \
length=200, showvalue=1, \
state=DISABLED,
command=self._onThrustScaleChanged)
else:
self._thrustScale = Scale(controlFrame, orient=VERTICAL, from_=100.0, to=-100.0, \
tickinterval=0, variable=self._throttle, \
length=200, showvalue=1, \
state=DISABLED,
command=self._onThrustScaleChanged)
self._thrustScale.bind("<Double-Button-1>",
self._onThrustScaleDoubleButton1, "+")
self._thrustScale.grid(column=0)
self._shiftCanvas = Canvas(controlFrame, bg="white", height=400, width=400, \
relief=SUNKEN)
self._shiftCanvas.bind("<Button-1>", self._onMouseButton1)
#self._shiftCanvas.bind("<ButtonRelease-1>", self._onMouseButtonRelease1)
self._shiftCanvas.bind("<B1-Motion>", self._onMouseButton1Motion)
self._shiftCanvas.bind("<Double-Button-1>", self._onMouseDoubleButton1)
self._shiftCanvas.bind("<Button-3>", self._onMouseButton3)
#self._shiftCanvas.bind("<ButtonRelease-3>", self._onMouseButtonRelease3)
self._shiftCanvas.bind("<B3-Motion>", self._onMouseButton3Motion)
self._shiftCanvas.grid(row=0, column=1, padx=2, pady=2)
self._shiftCanvas.create_oval(1, 1, 400, 400, outline="#ff0000")
self._shiftCanvas.create_line(200, 2, 200, 400, fill="#ff0000")
self._shiftCanvas.create_line(2, 200, 400, 200, fill="#ff0000")
self._shiftMarker = self._shiftCanvas.create_oval(196,
196,
204,
204,
outline="#0000ff",
fill="#0000ff")
self._yaw = DoubleVar()
self._yawScale = Scale(controlFrame, orient=HORIZONTAL, from_=-100.0, to=100.0, \
tickinterval=0, variable=self._yaw, \
length=200, showvalue=1, \
command=self._onYawScaleChanged)
self._yawScale.bind("<Double-Button-1>", self._onYawScaleDoubleButton1,
"+")
self._yawScale.grid(row=1, column=1)
self._controlKeyActive = False
#PID calibration
pidCalibrationFrame = tkFrame(self)
pidCalibrationFrame.grid(column=0, row=2, sticky="WE")
self._pidSelected = StringVar()
self._pidSelected.set("--")
self._pidListBox = OptionMenu(pidCalibrationFrame, self._pidSelected, "--", \
Cockpit.KEY_ANG_SPEED, Cockpit.KEY_ANGLES, Cockpit.KEY_ACCEL, \
command=self._onPidListBoxChanged)
self._pidListBox.pack(side=LEFT, padx=2)
self._pidListBox.config(width=10)
self._axisSelected = StringVar()
self._axisSelected.set("--")
self._axisListBox = OptionMenu(pidCalibrationFrame, self._axisSelected, "--", "X", "Y", "Z", \
command=self._onAxisListBoxChanged)
self._axisListBox.pack(side=LEFT, padx=2)
self._axisListBox.config(state=DISABLED)
Label(pidCalibrationFrame, text="P").pack(side=LEFT, padx=(14, 2))
self._pidPString = StringVar()
self._pidPString.set("0.00")
self._pidPSpinbox = Spinbox(pidCalibrationFrame, width=5, from_=0.0, to=10000.0, increment=0.01, state=DISABLED, \
textvariable=self._pidPString, command=self._onPidSpinboxChanged)
self._pidPSpinbox.pack(side=LEFT, padx=2)
Label(pidCalibrationFrame, text="I").pack(side=LEFT, padx=(14, 2))
self._pidIString = StringVar()
self._pidIString.set("0.00")
self._pidISpinbox = Spinbox(pidCalibrationFrame, width=5, from_=0.0, to=10000.0, increment=0.01, state=DISABLED, \
textvariable=self._pidIString, command=self._onPidSpinboxChanged)
self._pidISpinbox.pack(side=LEFT, padx=2)
Label(pidCalibrationFrame, text="D").pack(side=LEFT, padx=(14, 2))
self._pidDString = StringVar()
self._pidDString.set("0.00")
self._pidDSpinbox = Spinbox(pidCalibrationFrame, width=5, from_=0.0, to=10000.0, increment=0.01, state=DISABLED, \
textvariable=self._pidDString, command=self._onPidSpinboxChanged)
self._pidDSpinbox.pack(side=LEFT, padx=2)
#debug
debugFrame = tkFrame(self)
debugFrame.grid(column=0, row=3, sticky="WE")
self._debugMsg = Message(debugFrame,
anchor="nw",
justify=LEFT,
relief=SUNKEN,
width=300)
self._debugMsg.pack(fill=BOTH, expand=1)
def _start(self):
self._readDroneConfig()
if Cockpit.JOYSTICK_ENABLED:
self._joystickManager = JoystickManager.getInstance()
self._joystickManager.start()
joysticks = self._joystickManager.getJoysticks()
if len(joysticks) != 0:
self._joystick = joysticks[0]
self._joystick.onAxisChanged += self._onJoystickAxisChanged
self._joystick.onButtonPressed += self._onJoystickButtonPressed
else:
self._joystick = None
def _onJoystickAxisChanged(self, sender, index):
if self._started.get() and sender == self._joystick:
axisValue = self._joystick.getAxisValue(index)
if index == 0:
self._yaw.set(axisValue)
self._updateTarget()
elif index == 1 and not Cockpit.THROTTLE_BY_USER:
thrust = -axisValue
self._throttle.set(thrust)
self._updateTarget()
elif index == 2 and Cockpit.THROTTLE_BY_USER:
rowThrottle = (axisValue + 100.0) / 2.0
if rowThrottle < 10.0:
throttle = rowThrottle * 6.0
elif rowThrottle < 90.0:
throttle = 60.0 + ((rowThrottle - 10.0) / 8.0)
else:
throttle = 70.0 + (rowThrottle - 90.0) * 3.0
self._throttle.set(throttle)
self._sendThrottle()
elif index == 3:
x = 196 + axisValue * 2
lastCoords = self._shiftCanvas.coords(self._shiftMarker)
coords = (x, lastCoords[1])
self._plotShiftCanvasMarker(coords)
elif index == 4:
y = 196 + axisValue * 2
lastCoords = self._shiftCanvas.coords(self._shiftMarker)
coords = (lastCoords[0], y)
self._plotShiftCanvasMarker(coords)
def _onJoystickButtonPressed(self, sender, index):
if sender == self._joystick and index == 7:
if self._started.get() == 0:
self._startedCB.select()
else:
self._startedCB.deselect()
# Tkinter's widgets seem not to be calling the event-handler
# when they are changed programmatically. Therefore, the
# even-handler is called explicitly here.
self._startedCBChanged()
def exit(self):
self._link.send({"key": "close", "data": None})
self._stopUpdateInfoThread()
self._link.close()
if Cockpit.JOYSTICK_ENABLED:
self._joystickManager.stop()
self.quit()
def _updateTarget(self):
markerCoords = self._shiftCanvas.coords(self._shiftMarker)
coords = ((markerCoords[0] + markerCoords[2]) / 2,
(markerCoords[1] + markerCoords[3]) / 2)
self._target[0] = float(
coords[1] - 200) / 2.0 # X-axis angle / X-axis acceleration
self._target[1] = float(
coords[0] - 200) / 2.0 # Y-axis angle / Y-axis acceleration
#Remote control uses clockwise angle, but the drone's referece system uses counter-clockwise angle
self._target[2] = -self._yaw.get() # Z-axis angular speed
# Z-axis acceleration (thrust). Only when the motor throttle is not controlled by user directly
if Cockpit.THROTTLE_BY_USER:
self._target[3] = 0.0
else:
self._target[3] = self._throttle.get()
self._sendTarget()
def _keyDown(self, event):
if event.keysym == "Escape":
self._throttle.set(0)
self._started.set(0)
self._thrustScale.config(state=DISABLED)
self._stopUpdateInfoThread()
self._sendIsStarted()
elif event.keysym.startswith("Control"):
self._controlKeyActive = True
elif not self._controlKeysLocked and self._controlKeyActive:
if event.keysym == "Up":
self._thrustScaleUp()
elif event.keysym == "Down":
self._thrustScaleDown()
elif event.keysym == "Left":
self._yawLeft()
elif event.keysym == "Right":
self._yawRight()
elif event.keysym == "space":
self._yawReset()
if not Cockpit.THROTTLE_BY_USER:
self._thrustReset()
elif not self._controlKeysLocked and not self._controlKeyActive:
if event.keysym == "Up":
self._moveShiftCanvasMarker((0, -5))
elif event.keysym == "Down":
self._moveShiftCanvasMarker((0, 5))
elif event.keysym == "Left":
self._moveShiftCanvasMarker((-5, 0))
elif event.keysym == "Right":
self._moveShiftCanvasMarker((5, 0))
elif event.keysym == "space":
self._resetShiftCanvasMarker()
def _keyUp(self, eventArgs):
if eventArgs.keysym.startswith("Control"):
self._controlKeyActive = False
def _onMouseButton1(self, eventArgs):
self._lastMouseCoords = (eventArgs.x, eventArgs.y)
def _onMouseButtonRelease1(self, eventArgs):
self._shiftCanvas.coords(self._shiftMarker, 196, 196, 204, 204)
def _limitCoordsToSize(self, coords, size, width):
maxSize = size - (width / 2.0)
minSize = -(width / 2.0)
if coords[0] > maxSize:
x = maxSize
elif coords[0] < minSize:
x = minSize
else:
x = coords[0]
if coords[1] > maxSize:
y = maxSize
elif coords[1] < minSize:
y = minSize
else:
y = coords[1]
return (x, y)
def _plotShiftCanvasMarker(self, coords):
coords = self._limitCoordsToSize(coords, 400, 8)
self._shiftCanvas.coords(self._shiftMarker, coords[0], coords[1],
coords[0] + 8, coords[1] + 8)
self._updateTarget()
def _moveShiftCanvasMarker(self, shift):
lastCoords = self._shiftCanvas.coords(self._shiftMarker)
newCoords = (lastCoords[0] + shift[0], lastCoords[1] + shift[1])
self._plotShiftCanvasMarker(newCoords)
def _resetShiftCanvasMarker(self):
self._shiftCanvas.coords(self._shiftMarker, 196, 196, 204, 204)
self._updateTarget()
def _onMouseButton1Motion(self, eventArgs):
deltaCoords = (eventArgs.x - self._lastMouseCoords[0],
eventArgs.y - self._lastMouseCoords[1])
self._moveShiftCanvasMarker(deltaCoords)
self._lastMouseCoords = (eventArgs.x, eventArgs.y)
def _onMouseDoubleButton1(self, eventArgs):
self._resetShiftCanvasMarker()
def _onMouseButton3(self, eventArgs):
self._lastMouseCoords = (eventArgs.x, eventArgs.y)
self._mouseDirection = Cockpit.DIR_NONE
def _onMouseButtonRelease3(self, eventArgs):
self._shiftCanvas.coords(self._shiftMarker, 196, 196, 204, 204)
def _onMouseButton3Motion(self, eventArgs):
deltaCoords = (eventArgs.x - self._lastMouseCoords[0],
eventArgs.y - self._lastMouseCoords[1])
if self._mouseDirection == Cockpit.DIR_NONE:
if abs(deltaCoords[0]) > abs(deltaCoords[1]):
self._mouseDirection = Cockpit.DIR_HORIZONTAL
else:
self._mouseDirection = Cockpit.DIR_VERTICAL
if self._mouseDirection == Cockpit.DIR_HORIZONTAL:
deltaCoords = (deltaCoords[0], 0)
else:
deltaCoords = (0, deltaCoords[1])
self._moveShiftCanvasMarker(deltaCoords)
self._lastMouseCoords = (eventArgs.x, eventArgs.y)
def _thrustScaleUp(self):
#TODO: 20160526 DPM: El valor de incremento de aceleración (1.0) puede ser muy alto
if self._started.get():
newValue = self._thrustScale.get() \
+ (Cockpit.THROTTLE_RESOLUTION if Cockpit.THROTTLE_BY_USER else 1.0)
self._thrustScale.set(newValue)
self._updateTarget()
def _thrustScaleDown(self):
#TODO: 20160526 DPM: El valor de decremento de aceleración (1.0) puede ser muy alto
if self._started.get():
newValue = self._thrustScale.get() \
- (Cockpit.THROTTLE_RESOLUTION if Cockpit.THROTTLE_BY_USER else 1.0)
self._thrustScale.set(newValue)
self._updateTarget()
def _thrustReset(self):
if self._started.get():
self._thrustScale.set(0.0)
self._updateTarget()
def _onThrustScaleDoubleButton1(self, eventArgs):
self._thrustReset()
return "break"
def _yawRight(self):
newValue = self._yaw.get() + 1
self._yaw.set(newValue)
self._updateTarget()
def _yawLeft(self):
newValue = self._yaw.get() - 1
self._yaw.set(newValue)
self._updateTarget()
def _yawReset(self):
self._yaw.set(0)
self._updateTarget()
def _onMouseWheelUp(self, eventArgs):
if not self._controlKeyActive:
self._thrustScaleUp()
else:
self._yawRight()
def _onMouseWheelDown(self, eventArgs):
if not self._controlKeyActive:
self._thrustScaleDown()
else:
self._yawLeft()
def _onMouseWheel(self, eventArgs):
factor = eventArgs.delta / (1200.0 if Cockpit.THROTTLE_BY_USER
and not self._controlKeyActive else 120.0)
if not self._controlKeyActive:
if self._started.get():
newValue = self._thrustScale.get() + factor
self._thrustScale.set(newValue)
self._updateTarget()
else:
newValue = self._yaw.get() + factor
self._yaw.set(newValue)
self._updateTarget()
def _onYawScaleChanged(self, eventArgs):
self._updateTarget()
def _onYawScaleDoubleButton1(self, eventArgs):
self._yawReset()
return "break"
def _startedCBChanged(self):
if not self._started.get():
self._throttle.set(0)
self._thrustScale.config(state=DISABLED)
#self._integralsCB.config(state=DISABLED)
self._stopUpdateInfoThread()
else:
self._thrustScale.config(state="normal")
#self._integralsCB.config(state="normal")
self._startUpdateInfoThread()
self._sendIsStarted()
# def _integralsCBChanged(self):
#
# self._link.send({"key": "integrals", "data":self._integralsEnabled.get() != 0})
#
def _onThrustScaleChanged(self, eventArgs):
if Cockpit.THROTTLE_BY_USER:
self._sendThrottle()
else:
self._updateTarget()
def _sendThrottle(self):
self._link.send({"key": "throttle", "data": self._throttle.get()})
def _sendTarget(self):
self._link.send({"key": "target", "data": self._target})
def _sendIsStarted(self):
isStarted = self._started.get() != 0
self._link.send({"key": "is-started", "data": isStarted})
def _sendPidCalibrationData(self):
if self._pidSelected.get() != "--" and self._axisSelected.get(
) != "--":
pidData = {
"pid": self._pidSelected.get(),
"axis": self._axisSelected.get(),
"p": float(self._pidPSpinbox.get()),
"i": float(self._pidISpinbox.get()),
"d": float(self._pidDSpinbox.get())
}
self._link.send({"key": "pid-calibration", "data": pidData})
def _updatePidCalibrationData(self):
pid = self._pidSelected.get()
axis = self._axisSelected.get()
if pid != "--" and axis != "--":
self._pidConstants[pid][axis]["P"] = float(self._pidPSpinbox.get())
self._pidConstants[pid][axis]["I"] = float(self._pidISpinbox.get())
self._pidConstants[pid][axis]["D"] = float(self._pidDSpinbox.get())
def _readDroneConfig(self):
self._link.send({
"key": "read-drone-config",
"data": None
}, self._onDroneConfigRead)
def _readDroneState(self):
if not self._readingState:
self._readingState = True
self._link.send({
"key": "read-drone-state",
"data": None
}, self._onDroneStateRead)
def _readPidConfigItem(self, message, cockpitKey, axises, configKeys):
for i in range(len(axises)):
for j in range(len(Cockpit.PID_KEYS)):
self._pidConstants[cockpitKey][axises[i]][
Cockpit.PID_KEYS[j]] = message[configKeys[j]][i]
def _onDroneConfigRead(self, message):
#TODO Show current configuration within the GUI (at least relevant settings)
if message:
#Angle-speeds
self._readPidConfigItem(message, Cockpit.KEY_ANG_SPEED, ["X", "Y", "Z"], \
[Configuration.PID_ANGLES_SPEED_KP, \
Configuration.PID_ANGLES_SPEED_KI, \
Configuration.PID_ANGLES_SPEED_KD])
#Angles
self._readPidConfigItem(message, Cockpit.KEY_ANGLES, ["X", "Y"], \
[Configuration.PID_ANGLES_KP, \
Configuration.PID_ANGLES_KI, \
Configuration.PID_ANGLES_KD])
#Accels
self._readPidConfigItem(message, Cockpit.KEY_ACCEL, ["X", "Y", "Z"], \
[Configuration.PID_ACCEL_KP, \
Configuration.PID_ACCEL_KI, \
Configuration.PID_ACCEL_KD])
def _onDroneStateRead(self, state):
if state:
for index in range(4):
self._throttleTexts[index].set("{0:.3f}".format(
state["_throttles"][index]))
for index in range(3):
self._accelTexts[index].set("{0:.3f}".format(
state["_accels"][index]))
self._angleTexts[index].set("{0:.3f}".format(
state["_angles"][index]))
currentPeriod = state["_currentPeriod"]
if currentPeriod > 0.0:
freq = 1.0 / currentPeriod
self._loopRateText.set("{0:.3f}".format(freq))
else:
self._loopRateText.set("--")
else:
self._stopUpdateInfoThread()
self._readingState = False
def _onPidSpinboxChanged(self):
self._updatePidCalibrationData()
self._sendPidCalibrationData()
def _onPidListBoxChanged(self, pid):
self._axisSelected.set("--")
self._pidPString.set("--")
self._pidIString.set("--")
self._pidDString.set("--")
self._pidPSpinbox.config(state=DISABLED)
self._pidISpinbox.config(state=DISABLED)
self._pidDSpinbox.config(state=DISABLED)
self._selectedPidConstats = pid
if pid == "--":
self._axisListBox.config(state=DISABLED)
self._controlKeysLocked = False
else:
self._axisListBox.config(state="normal")
self._controlKeysLocked = True
def _onAxisListBoxChanged(self, axis):
if axis == "--" or (self._selectedPidConstats == Cockpit.KEY_ANGLES
and axis == "Z"):
self._pidPString.set("--")
self._pidIString.set("--")
self._pidDString.set("--")
self._pidPSpinbox.config(state=DISABLED)
self._pidISpinbox.config(state=DISABLED)
self._pidDSpinbox.config(state=DISABLED)
self._controlKeysLocked = axis != "--"
else:
self._pidPString.set("{:.2f}".format(
self._pidConstants[self._selectedPidConstats][axis]["P"]))
self._pidIString.set("{:.2f}".format(
self._pidConstants[self._selectedPidConstats][axis]["I"]))
self._pidDString.set("{:.2f}".format(
self._pidConstants[self._selectedPidConstats][axis]["D"]))
self._pidPSpinbox.config(state="normal")
self._pidISpinbox.config(state="normal")
self._pidDSpinbox.config(state="normal")
self._controlKeysLocked = True
def _updateInfo(self):
while self._updateInfoThreadRunning:
self._readDroneState()
time.sleep(1.0)
def _startUpdateInfoThread(self):
self._updateInfoThreadRunning = True
if not self._updateInfoThread.isAlive():
self._updateInfoThread.start()
def _stopUpdateInfoThread(self):
self._updateInfoThreadRunning = False
if self._updateInfoThread.isAlive():
self._updateInfoThread.join()