def __init__(self, id, uri, parent):
super(dron, self).__init__()
# ustawienie gui
self.ui = Ui_Dron()
self.ui.setupUi(self)
self.ui.tabWidget.setCurrentIndex(0)
#inicjalizacja ustawien drona
self.uri = uri
self.id = id
self.cf = cf(self)
self.ui.l_conn.setText("Choosen: {}".format(self.uri))
# odswiezanie interfejsu
self.update_gui_thread = QTimer()
self.update_gui_thread.timeout.connect(self.update_interface)
self.update_gui_thread.start(100)
# ustawienia watku wyliczjacego sterowanie
self.update_control_thread = QTimer()
self.update_control_thread.timeout.connect(self.update_control)
self.timeout = 0
self.set_signals() # ustawienie sygnalow PyQt
self.set_variables()
def __init__(self,id,uri,parent):
super(dron, self).__init__()
# ustawienie gui
self.ui=Ui_Dron()
self.ui.setupUi(self)
self.ui.tabWidget.setCurrentIndex(0)
#inicjalizacja ustawien drona
self.uri=uri
self.id=id
self.cf=cf(self)
self.ui.l_conn.setText("Choosen: {}".format(self.uri))
# odswiezanie interfejsu
self.update_gui_thread=QTimer()
self.update_gui_thread.timeout.connect(self.update_interface)
self.update_gui_thread.start(100)
# ustawienia watku wyliczjacego sterowanie
self.update_control_thread=QTimer()
self.update_control_thread.timeout.connect(self.update_control)
self.timeout=0
self.set_signals() # ustawienie sygnalow PyQt
self.set_variables()
class dron(QtGui.QWidget):
control_data_sig=pyqtSignal(dict)
def __init__(self,id,uri,parent):
super(dron, self).__init__()
# ustawienie gui
self.ui=Ui_Dron()
self.ui.setupUi(self)
self.ui.tabWidget.setCurrentIndex(0)
#inicjalizacja ustawien drona
self.uri=uri
self.id=id
self.cf=cf(self)
self.ui.l_conn.setText("Choosen: {}".format(self.uri))
# odswiezanie interfejsu
self.update_gui_thread=QTimer()
self.update_gui_thread.timeout.connect(self.update_interface)
self.update_gui_thread.start(100)
# ustawienia watku wyliczjacego sterowanie
self.update_control_thread=QTimer()
self.update_control_thread.timeout.connect(self.update_control)
self.timeout=0
self.set_signals() # ustawienie sygnalow PyQt
self.set_variables()
def set_variables(self):
# inicjalizacja filtru kalmana
self.kalman=kalman12()
self.stan=np.zeros((1,12))
# zmienna logujaca dane
self.strlist=[]
self.strlist.append("x,y,z,yaw,vzcam,xk,yk,zk,rollk,pitchk,yawk,vxk,vyk,vzk,vrollk,vpitchk,vyawk,pitch_con,roll_con,yaw_con,thrust_cam,xd,yd,zd,rolld,pitchd,yawd,xt,yt,zt,yawt")
# dane otryzmywane z kamer
self.isCorrect="0"
self.z_cam_p=0
self.vzcam=0
self.x_cam=0
self.y_cam=0
self.z_cam=0
self.yaw_cam=0
self.dt=-1
# x,y,z w ukladzie drona
# pozycje
self.x_dron=0
self.y_dron=0
self.z_dron=0
#predkosci
self.vx_dron=0
self.vy_dron=0
self.vz_dron=0
self.alfa_dron=0
self.valfa_dron=0
# dane otrzymane z drona
self.thrust_dron=0
self.roll_dron=0
self.pitch_dron=0
self.yaw_dron=0
self.vyaw_dron=0
# wartosci silnikow
self.m1=0.0
self.m2=0.0
self.m3=0.0
self.m4=0.0
# inicjalizacja zmiennych do sterowania
self.thrust_control=0.0
self.pitch_control=0.0
self.roll_control=0.0
self.yaw_control=0.0
self.thrust_control_previous=0.0
self.pitch_control_previous=0.0
self.roll_control_previous=0.0
self.yaw_control_previous=0.0
self.control_package={
"thrust":self.thrust_control,
"pitch":self.pitch_control,
"roll":self.roll_control,
"yaw":self.yaw_control
}
# inicjalizacja celu
self.x_target=0.0
self.y_target=0.4
self.z_target=0.0
self.yaw_target=0.0
self.ctrl_isTrue=False
def set_signals(self):
# ustawienie interfejsu
self.ui.pb_conn_2.clicked.connect(self.connect_clicked)
self.ui.pb_disc_2.clicked.connect(self.disconnect_clicked)
self.ui.pb_start_2.clicked.connect(self.start_clicked)
self.ui.pb_stop_2.clicked.connect(self.stop_clicked)
self.ui.pb_update_2.clicked.connect(self.update_clicked)
self.ui.pb_save.clicked.connect(self.saveFile)
self.cf.cf_connected.connect(self.connected)
self.cf.cf_logs.connect(self.log_recv)
def saveFile(self):
# zapisywannie loga do .csv
txt="\n".join(self.strlist)
f=open(name="log_{}_{}.csv".format(self.id,time.strftime("%H_%M")),mode='w')
f.write(txt)
f.close()
del self.strlist[1:]
print self.strlist
print("saved")
def log_recv(self,data):
# zapisywanie danych otrzymanych od drona
self.thrust_dron=data["thrust"]
self.roll_dron=data["roll"]
self.pitch_dron=data["pitch"]
self.yaw_dron=data["yaw"]
self.m1=data["m1"]
self.m2=data["m2"]
self.m3=data["m3"]
self.m4=data["m4"]
def connected(self,status):
#funkcja wykonywana po polaczeniu z dronem
if status:
self.ui.l_conn.setText("Connected to {}".format(self.uri))
self.ui.pb_conn_2.setEnabled(False)
self.ui.pb_disc_2.setEnabled(True)
self.ui.gb_control_2.setEnabled(True)
self.ui.pb_start_2.setEnabled(True)
self.ui.pb_update_2.setEnabled(True)
else:
self.ui.l_conn.setText("Disconnected")
self.ui.pb_conn_2.setEnabled(True)
self.ui.pb_disc_2.setEnabled(False)
self.ui.gb_control_2.setEnabled(False)
def disconnect_clicked(self):
# wcisniecie przycisku Disconnect
self.cf.close()
self.stop_clicked()
def connect_clicked(self):
# wcisniecie przycisku Disconnect
self.ui.l_conn.setText("Connecting to {}".format(self.uri))
self.cf.connect(self.uri)
def update_interface(self):
# zmiana dynych w interfejsie
# pozycje z kamer
self.ui.l_x_2.setText("x: {:.3f}".format(self.x_cam))
self.ui.l_y_2.setText("y: {:.3f}".format(self.y_cam))
self.ui.l_z_2.setText("z: {:.3f}".format(self.z_cam))
self.ui.l_theta_2.setText("yaw: {:.3f}".format(self.yaw_cam))
# dane pobrane z drona
self.ui.l_vx_2.setText("thrust: {:.3f}".format(self.roll_dron))
self.ui.l_vy_2.setText("roll: {:.3f}".format(self.pitch_dron))
self.ui.l_vz_2.setText("pitch: {:.3f}".format(self.thrust_dron))
self.ui.l_vtheta_2.setText("yaw: {:.3f}".format(self.yaw_dron))
# Wyliczone bledy
self.ui.l_thrust_a_2.setText("Ez: {}".format(self.z_dron))
self.ui.l_roll_a_2.setText("Ex: {}".format(self.x_dron))
self.ui.l_pitch_a_2.setText("Ey: {}".format(self.y_dron))
self.ui.l_yaw_a_2.setText("Eyaw: {}".format(self.alfa_dron))
# wyliczone wartosci sterowania
self.ui.l_thrust_2.setText("Thrust: {:}".format(self.thrust_control))
self.ui.l_roll_2.setText("Roll: {:}".format(self.roll_control))
self.ui.l_pitch_2.setText("Pitch: {:}".format(self.pitch_control))
self.ui.l_yaw_2.setText("Yaw: {:}".format(self.yaw_control))
def update_log_table(self):
# funkcja to wysietlania logow w aplikacji - aktualnie nie wykorzystywana
self.ui.table_log.currentRow()
currentRowCount = self.ui.table_log.rowCount() #necessary even when there are no rows in the table
self.ui.table_log.insertRow(currentRowCount)
self.ui.table_log.setItem(currentRowCount,0,QtGui.QTableWidgetItem("{}".format(time.strftime("%H:%M:%S"))))
self.ui.table_log.setItem(currentRowCount,1,QtGui.QTableWidgetItem("{:.3f}, {:.3f}, {:.3f}, {:.3f}".format(
self.x_cam,self.y_cam,self.z_cam,self.yaw_cam)))
self.ui.table_log.setItem(currentRowCount,2,QtGui.QTableWidgetItem("{:.3f}, {:.3f}, {:.3f}, {:.3f}".format(
self.x_target-self.x_cam,self.y_target-self.y_cam,self.z_target-self.z_cam,self.yaw_target-self.yaw_cam)))
# self.ui.table_log.setItem(currentRowCount,3,QtGui.QTableWidgetItem("{:.3f}, {:.3f}, {:.3f}, {:.3f}".format(
# self.vx_actual,self.vy_actual,self.vz_actual,self.vyaw_actual)))
self.ui.table_log.setItem(currentRowCount,5,QtGui.QTableWidgetItem("{}, {:.3f}, {:.3f}, {:.3f}".format(
self.thrust_dron,self.roll_dron,self.pitch_dron,self.yaw_dron)))
self.ui.table_log.setItem(currentRowCount,4,QtGui.QTableWidgetItem("{}, {:.3f}, {:.3f}, {:.3f}".format(
self.thrust_control,self.roll_control,self.pitch_control,self.yaw_control)))
self.ui.table_log.setItem(currentRowCount,6,QtGui.QTableWidgetItem("{:.3f}, {:.3f}, {:.3f}, {:.3f}".format(
self.m1,self.m2,self.m3,self.m4)))
self.ui.table_log.scrollToBottom()
self.ui.table_log.resizeColumnsToContents()
def start_clicked(self):
# funckja wykonywana po wcisnieciu przycisku start
if not self.ctrl_isTrue:
self.ctrl_isTrue=True
self.ui.pb_start_2.setText("stop")
self.ui.pb_stop_2.setEnabled(True)
self.update_clicked()
self.update_control_thread.start(10)
else:
self.ctrl_isTrue=False
self.ui.pb_start_2.setText("start")
self.ui.pb_stop_2.setEnabled(True)
self.stop_clicked()
def stop_clicked(self):
# funckja wykonywana po wcisnieciu przycisku stop
self.ui.pb_start_2.setEnabled(True)
self.ui.pb_stop_2.setEnabled(False)
self.control_package={
"thrust":0.0,
"pitch":0.0,
"roll":0.0,
"yaw":0.0,
}
self.control_data_sig.emit(self.control_package)
self.update_control_thread.stop()
def update_clicked(self):
# funckja wykonywana po wcisnieciu przycisku update
self.x_target=float(self.ui.en_x_2.text())
self.y_target=float(self.ui.en_y_2.text())
self.z_target=float(self.ui.en_z_2.text())
self.yaw_target=float(self.ui.en_theta_2.text())
def update_control(self):
# watek liczacy sterowanie
vzcam=0
# zachowanie poprzednich wartosci sterowania jesli pojawi sie blad odczytu z kamer
if (self.isCorrect==0.0) :
self.thrust_control=self.thrust_control_previous
self.roll_control=self.roll_control_previous
self.pitch_control=self.pitch_control_previous
self.yaw_control=self.yaw_control_previous
self.timeout+=1
print self.timeout
if self.timeout>=15:
self.control_package={
"thrust":0,
"pitch":0,
"roll":0,
"yaw":0
}
self.control_data_sig.emit(self.control_package)
self.stop_clicked()
else:
# wyliczenie danych z filtru kalmna
self.stan=self.kalman.licz(self.x_cam,self.y_cam,self.z_cam,self.roll_dron,self.pitch_dron,self.yaw_cam) #
x=self.stan[0]
y=self.stan[1]
z=self.stan[2]
roll=self.stan[3]
pitch=self.stan[4]
alfa=self.stan[5]
vx=self.stan[6]
vy=self.stan[7]
vz=self.stan[8]
vroll=self.stan[9]
vpitch=self.stan[10]
valfa=self.stan[11]
self.x_dron=(self.x_target-x)*cos(alfa)+(self.y_target-y)*sin(alfa)
self.y_dron=(self.y_target-y)*cos(alfa)-(self.x_target-x)*sin(alfa)
self.z_dron=self.z_target-z
self.vx_dron=vx*cos(alfa)+vy*sin(alfa)
self.vy_dron=vy*cos(alfa)-vx*sin(alfa)
self.vz_dron=vz
self.alfa_dron=alfa
self.valfa_dron=valfa
# obliczenie sterowanie wykorzystujac sprzezenie od stanu
T,Y,R,P=sprzezenie.control(self.z_dron,self.vz_dron,alfa,valfa,self.x_dron,self.vx_dron,self.y_dron,self.vy_dron,roll,pitch,vroll,vpitch)
# obliczenie sterowanie wykorzystujac fuzzy logic
Tf=fz((self.z_target-z)*100,self.vz_dron)
Rf=-fd((self.y_dron),self.vy_dron)
Pf=fd((self.x_dron),self.vx_dron)
Yf=fyaw((self.yaw_target-alfa)*180/pi,valfa)
# wybrane wartosci sterowania
self.thrust_control=Tf
self.pitch_control=Pf-P
self.roll_control=Rf+R
self.yaw_control=Yf
self.timeout=0
# wyslanie sterowania do drona
self.control_package={
"thrust":self.thrust_control*600,
"pitch":self.pitch_control,
"roll":self.roll_control,
"yaw":self.yaw_control
}
if self.ui.cb_sendCtrl.isChecked():
self.control_data_sig.emit(self.control_package)
# logowanie danych
str1=",".join([str(self.x_cam),str(self.y_cam),str(self.z_cam),str(self.yaw_cam),str(self.vzcam)])
str2=",".join(str(i) for i in self.stan)
str3=",".join([str(self.pitch_control),str(self.roll_control),str(self.yaw_control),str(self.thrust_control)])
str4=",".join([str(self.x_dron),str(self.y_dron),str(self.z_dron)])
str5=",".join([str(self.roll_dron),str(self.pitch_dron),str(self.yaw_dron)])
str6=",".join([str(self.x_target),str(self.y_target),str(self.z_target),str(self.yaw_target)])
self.strlist.append(",".join([str1,str2,str3,str4,str5,str6]))
# przypisanie poprzedniego sterowania
self.thrust_control_previous=self.thrust_control
self.pitch_control_previous=self.pitch_control
self.roll_control_previous=self.roll_control
self.yaw_control_previous=self.yaw_control
pass
def stop_control_thread(self):
self.stop_clicked()
class dron(QtGui.QWidget):
control_data_sig = pyqtSignal(dict)
def __init__(self, id, uri, parent):
super(dron, self).__init__()
# ustawienie gui
self.ui = Ui_Dron()
self.ui.setupUi(self)
self.ui.tabWidget.setCurrentIndex(0)
#inicjalizacja ustawien drona
self.uri = uri
self.id = id
self.cf = cf(self)
self.ui.l_conn.setText("Choosen: {}".format(self.uri))
# odswiezanie interfejsu
self.update_gui_thread = QTimer()
self.update_gui_thread.timeout.connect(self.update_interface)
self.update_gui_thread.start(100)
# ustawienia watku wyliczjacego sterowanie
self.update_control_thread = QTimer()
self.update_control_thread.timeout.connect(self.update_control)
self.timeout = 0
self.set_signals() # ustawienie sygnalow PyQt
self.set_variables()
def set_variables(self):
# inicjalizacja filtru kalmana
self.kalman = kalman12()
self.stan = np.zeros((1, 12))
# zmienna logujaca dane
self.strlist = []
self.strlist.append(
"x,y,z,yaw,vzcam,xk,yk,zk,rollk,pitchk,yawk,vxk,vyk,vzk,vrollk,vpitchk,vyawk,pitch_con,roll_con,yaw_con,thrust_cam,xd,yd,zd,rolld,pitchd,yawd,xt,yt,zt,yawt"
)
# dane otryzmywane z kamer
self.isCorrect = "0"
self.z_cam_p = 0
self.vzcam = 0
self.x_cam = 0
self.y_cam = 0
self.z_cam = 0
self.yaw_cam = 0
self.dt = -1
# x,y,z w ukladzie drona
# pozycje
self.x_dron = 0
self.y_dron = 0
self.z_dron = 0
#predkosci
self.vx_dron = 0
self.vy_dron = 0
self.vz_dron = 0
self.alfa_dron = 0
self.valfa_dron = 0
# dane otrzymane z drona
self.thrust_dron = 0
self.roll_dron = 0
self.pitch_dron = 0
self.yaw_dron = 0
self.vyaw_dron = 0
# wartosci silnikow
self.m1 = 0.0
self.m2 = 0.0
self.m3 = 0.0
self.m4 = 0.0
# inicjalizacja zmiennych do sterowania
self.thrust_control = 0.0
self.pitch_control = 0.0
self.roll_control = 0.0
self.yaw_control = 0.0
self.thrust_control_previous = 0.0
self.pitch_control_previous = 0.0
self.roll_control_previous = 0.0
self.yaw_control_previous = 0.0
self.control_package = {
"thrust": self.thrust_control,
"pitch": self.pitch_control,
"roll": self.roll_control,
"yaw": self.yaw_control
}
# inicjalizacja celu
self.x_target = 0.0
self.y_target = 0.4
self.z_target = 0.0
self.yaw_target = 0.0
self.ctrl_isTrue = False
def set_signals(self):
# ustawienie interfejsu
self.ui.pb_conn_2.clicked.connect(self.connect_clicked)
self.ui.pb_disc_2.clicked.connect(self.disconnect_clicked)
self.ui.pb_start_2.clicked.connect(self.start_clicked)
self.ui.pb_stop_2.clicked.connect(self.stop_clicked)
self.ui.pb_update_2.clicked.connect(self.update_clicked)
self.ui.pb_save.clicked.connect(self.saveFile)
self.cf.cf_connected.connect(self.connected)
self.cf.cf_logs.connect(self.log_recv)
def saveFile(self):
# zapisywannie loga do .csv
txt = "\n".join(self.strlist)
f = open(name="log_{}_{}.csv".format(self.id, time.strftime("%H_%M")),
mode='w')
f.write(txt)
f.close()
del self.strlist[1:]
print self.strlist
print("saved")
def log_recv(self, data):
# zapisywanie danych otrzymanych od drona
self.thrust_dron = data["thrust"]
self.roll_dron = data["roll"]
self.pitch_dron = data["pitch"]
self.yaw_dron = data["yaw"]
self.m1 = data["m1"]
self.m2 = data["m2"]
self.m3 = data["m3"]
self.m4 = data["m4"]
def connected(self, status):
#funkcja wykonywana po polaczeniu z dronem
if status:
self.ui.l_conn.setText("Connected to {}".format(self.uri))
self.ui.pb_conn_2.setEnabled(False)
self.ui.pb_disc_2.setEnabled(True)
self.ui.gb_control_2.setEnabled(True)
self.ui.pb_start_2.setEnabled(True)
self.ui.pb_update_2.setEnabled(True)
else:
self.ui.l_conn.setText("Disconnected")
self.ui.pb_conn_2.setEnabled(True)
self.ui.pb_disc_2.setEnabled(False)
self.ui.gb_control_2.setEnabled(False)
def disconnect_clicked(self):
# wcisniecie przycisku Disconnect
self.cf.close()
self.stop_clicked()
def connect_clicked(self):
# wcisniecie przycisku Disconnect
self.ui.l_conn.setText("Connecting to {}".format(self.uri))
self.cf.connect(self.uri)
def update_interface(self):
# zmiana dynych w interfejsie
# pozycje z kamer
self.ui.l_x_2.setText("x: {:.3f}".format(self.x_cam))
self.ui.l_y_2.setText("y: {:.3f}".format(self.y_cam))
self.ui.l_z_2.setText("z: {:.3f}".format(self.z_cam))
self.ui.l_theta_2.setText("yaw: {:.3f}".format(self.yaw_cam))
# dane pobrane z drona
self.ui.l_vx_2.setText("thrust: {:.3f}".format(self.roll_dron))
self.ui.l_vy_2.setText("roll: {:.3f}".format(self.pitch_dron))
self.ui.l_vz_2.setText("pitch: {:.3f}".format(self.thrust_dron))
self.ui.l_vtheta_2.setText("yaw: {:.3f}".format(self.yaw_dron))
# Wyliczone bledy
self.ui.l_thrust_a_2.setText("Ez: {}".format(self.z_dron))
self.ui.l_roll_a_2.setText("Ex: {}".format(self.x_dron))
self.ui.l_pitch_a_2.setText("Ey: {}".format(self.y_dron))
self.ui.l_yaw_a_2.setText("Eyaw: {}".format(self.alfa_dron))
# wyliczone wartosci sterowania
self.ui.l_thrust_2.setText("Thrust: {:}".format(self.thrust_control))
self.ui.l_roll_2.setText("Roll: {:}".format(self.roll_control))
self.ui.l_pitch_2.setText("Pitch: {:}".format(self.pitch_control))
self.ui.l_yaw_2.setText("Yaw: {:}".format(self.yaw_control))
def update_log_table(self):
# funkcja to wysietlania logow w aplikacji - aktualnie nie wykorzystywana
self.ui.table_log.currentRow()
currentRowCount = self.ui.table_log.rowCount(
) #necessary even when there are no rows in the table
self.ui.table_log.insertRow(currentRowCount)
self.ui.table_log.setItem(
currentRowCount, 0,
QtGui.QTableWidgetItem("{}".format(time.strftime("%H:%M:%S"))))
self.ui.table_log.setItem(
currentRowCount, 1,
QtGui.QTableWidgetItem("{:.3f}, {:.3f}, {:.3f}, {:.3f}".format(
self.x_cam, self.y_cam, self.z_cam, self.yaw_cam)))
self.ui.table_log.setItem(
currentRowCount, 2,
QtGui.QTableWidgetItem("{:.3f}, {:.3f}, {:.3f}, {:.3f}".format(
self.x_target - self.x_cam, self.y_target - self.y_cam,
self.z_target - self.z_cam, self.yaw_target - self.yaw_cam)))
# self.ui.table_log.setItem(currentRowCount,3,QtGui.QTableWidgetItem("{:.3f}, {:.3f}, {:.3f}, {:.3f}".format(
# self.vx_actual,self.vy_actual,self.vz_actual,self.vyaw_actual)))
self.ui.table_log.setItem(
currentRowCount, 5,
QtGui.QTableWidgetItem("{}, {:.3f}, {:.3f}, {:.3f}".format(
self.thrust_dron, self.roll_dron, self.pitch_dron,
self.yaw_dron)))
self.ui.table_log.setItem(
currentRowCount, 4,
QtGui.QTableWidgetItem("{}, {:.3f}, {:.3f}, {:.3f}".format(
self.thrust_control, self.roll_control, self.pitch_control,
self.yaw_control)))
self.ui.table_log.setItem(
currentRowCount, 6,
QtGui.QTableWidgetItem("{:.3f}, {:.3f}, {:.3f}, {:.3f}".format(
self.m1, self.m2, self.m3, self.m4)))
self.ui.table_log.scrollToBottom()
self.ui.table_log.resizeColumnsToContents()
def start_clicked(self):
# funckja wykonywana po wcisnieciu przycisku start
if not self.ctrl_isTrue:
self.ctrl_isTrue = True
self.ui.pb_start_2.setText("stop")
self.ui.pb_stop_2.setEnabled(True)
self.update_clicked()
self.update_control_thread.start(10)
else:
self.ctrl_isTrue = False
self.ui.pb_start_2.setText("start")
self.ui.pb_stop_2.setEnabled(True)
self.stop_clicked()
def stop_clicked(self):
# funckja wykonywana po wcisnieciu przycisku stop
self.ui.pb_start_2.setEnabled(True)
self.ui.pb_stop_2.setEnabled(False)
self.control_package = {
"thrust": 0.0,
"pitch": 0.0,
"roll": 0.0,
"yaw": 0.0,
}
self.control_data_sig.emit(self.control_package)
self.update_control_thread.stop()
def update_clicked(self):
# funckja wykonywana po wcisnieciu przycisku update
self.x_target = float(self.ui.en_x_2.text())
self.y_target = float(self.ui.en_y_2.text())
self.z_target = float(self.ui.en_z_2.text())
self.yaw_target = float(self.ui.en_theta_2.text())
def update_control(self):
# watek liczacy sterowanie
vzcam = 0
# zachowanie poprzednich wartosci sterowania jesli pojawi sie blad odczytu z kamer
if (self.isCorrect == 0.0):
self.thrust_control = self.thrust_control_previous
self.roll_control = self.roll_control_previous
self.pitch_control = self.pitch_control_previous
self.yaw_control = self.yaw_control_previous
self.timeout += 1
print self.timeout
if self.timeout >= 15:
self.control_package = {
"thrust": 0,
"pitch": 0,
"roll": 0,
"yaw": 0
}
self.control_data_sig.emit(self.control_package)
self.stop_clicked()
else:
# wyliczenie danych z filtru kalmna
self.stan = self.kalman.licz(self.x_cam, self.y_cam, self.z_cam,
self.roll_dron, self.pitch_dron,
self.yaw_cam) #
x = self.stan[0]
y = self.stan[1]
z = self.stan[2]
roll = self.stan[3]
pitch = self.stan[4]
alfa = self.stan[5]
vx = self.stan[6]
vy = self.stan[7]
vz = self.stan[8]
vroll = self.stan[9]
vpitch = self.stan[10]
valfa = self.stan[11]
self.x_dron = (self.x_target - x) * cos(alfa) + (self.y_target -
y) * sin(alfa)
self.y_dron = (self.y_target - y) * cos(alfa) - (self.x_target -
x) * sin(alfa)
self.z_dron = self.z_target - z
self.vx_dron = vx * cos(alfa) + vy * sin(alfa)
self.vy_dron = vy * cos(alfa) - vx * sin(alfa)
self.vz_dron = vz
self.alfa_dron = alfa
self.valfa_dron = valfa
# obliczenie sterowanie wykorzystujac sprzezenie od stanu
T, Y, R, P = sprzezenie.control(self.z_dron, self.vz_dron, alfa,
valfa, self.x_dron, self.vx_dron,
self.y_dron, self.vy_dron, roll,
pitch, vroll, vpitch)
# obliczenie sterowanie wykorzystujac fuzzy logic
Tf = fz((self.z_target - z) * 100, self.vz_dron)
Rf = -fd((self.y_dron), self.vy_dron)
Pf = fd((self.x_dron), self.vx_dron)
Yf = fyaw((self.yaw_target - alfa) * 180 / pi, valfa)
# wybrane wartosci sterowania
self.thrust_control = Tf
self.pitch_control = Pf - P
self.roll_control = Rf + R
self.yaw_control = Yf
self.timeout = 0
# wyslanie sterowania do drona
self.control_package = {
"thrust": self.thrust_control * 600,
"pitch": self.pitch_control,
"roll": self.roll_control,
"yaw": self.yaw_control
}
if self.ui.cb_sendCtrl.isChecked():
self.control_data_sig.emit(self.control_package)
# logowanie danych
str1 = ",".join([
str(self.x_cam),
str(self.y_cam),
str(self.z_cam),
str(self.yaw_cam),
str(self.vzcam)
])
str2 = ",".join(str(i) for i in self.stan)
str3 = ",".join([
str(self.pitch_control),
str(self.roll_control),
str(self.yaw_control),
str(self.thrust_control)
])
str4 = ",".join([str(self.x_dron), str(self.y_dron), str(self.z_dron)])
str5 = ",".join(
[str(self.roll_dron),
str(self.pitch_dron),
str(self.yaw_dron)])
str6 = ",".join([
str(self.x_target),
str(self.y_target),
str(self.z_target),
str(self.yaw_target)
])
self.strlist.append(",".join([str1, str2, str3, str4, str5, str6]))
# przypisanie poprzedniego sterowania
self.thrust_control_previous = self.thrust_control
self.pitch_control_previous = self.pitch_control
self.roll_control_previous = self.roll_control
self.yaw_control_previous = self.yaw_control
pass
def stop_control_thread(self):
self.stop_clicked()
