t = threading.Thread(name= 'updateFrame',target = updateFrame) t.start() fig = plt.figure() ax1 = fig.add_subplot(1,1,1) ys = [0] * 500 xs = np.arange(0,500) drone = ARDrone() drone.navdata_ready.wait() # wait until NavData is ready #while(drone.state.emergency_mask): # print("emergency") # drone.emergency() drone.send(at.CONFIG('general:navdata_demo', True)) time.sleep(3) positie = Positiebepaling() time.sleep(10) def animate(i): y = drone.navdata.demo.altitude ys.pop(0) ys.append(y) ax1.clear() ax1.plot(xs, ys) ax1.plot(xs,[0] * 500) class PIDcontroller: #construction of the object PIDcontroller: def __init__(self,Kp,Ki,Kd,dT,e_prev,e_int): self.Kp = Kp # proportional gain
class Ui_Form(object): def __init__(self): self.logger = Logger() self.drone = ARDrone(connect=True) self.drone.send(at.CONFIG('general:navdata_demo', True)) # self.drone.navdata_ready.wait() # self.drone.state.video_thread_on = 0 def setupUi(self, Form): Form.setObjectName("GameOfDrones GUI") Form.resize(500, 300) Form.setMouseTracking(False) self.gridLayout_5 = QtWidgets.QGridLayout(Form) self.gridLayout_5.setObjectName("gridLayout_5") self.gridLayout = QtWidgets.QGridLayout() self.gridLayout.setObjectName("gridLayout") self.resetButton = QtWidgets.QPushButton(Form) # self.resetButton.setEnabled(False) self.resetButton.setObjectName("resetButton") self.gridLayout.addWidget(self.resetButton, 2, 0, 1, 1) self.takeoffButton = QtWidgets.QPushButton(Form) self.takeoffButton.setEnabled(False) self.takeoffButton.setObjectName("takeoffButton") self.gridLayout.addWidget(self.takeoffButton, 0, 0, 1, 1) self.landButton = QtWidgets.QPushButton(Form) self.landButton.setEnabled(True) self.landButton.setObjectName("landButton") self.gridLayout.addWidget(self.landButton, 1, 0, 1, 1) self.gridLayout_5.addLayout(self.gridLayout, 2, 5, 1, 1) self.gridLayout_6 = QtWidgets.QGridLayout() self.gridLayout_6.setObjectName("gridLayout_6") self.label = QtWidgets.QLabel(Form) self.label.setLayoutDirection(QtCore.Qt.LeftToRight) self.label.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignTrailing | QtCore.Qt.AlignVCenter) self.label.setObjectName("label") self.gridLayout_6.addWidget(self.label, 1, 0, 1, 1) self.xVelLabel = QtWidgets.QLabel(Form) self.xVelLabel.setObjectName("xVelLabel") self.gridLayout_6.addWidget(self.xVelLabel, 1, 1, 1, 1) self.yVelLabel = QtWidgets.QLabel(Form) self.yVelLabel.setObjectName("yVelLabel") self.gridLayout_6.addWidget(self.yVelLabel, 2, 1, 1, 1) self.label_3 = QtWidgets.QLabel(Form) self.label_3.setLayoutDirection(QtCore.Qt.LeftToRight) self.label_3.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignTrailing | QtCore.Qt.AlignVCenter) self.label_3.setObjectName("label_3") self.gridLayout_6.addWidget(self.label_3, 2, 0, 1, 1) self.zVelLabel = QtWidgets.QLabel(Form) self.zVelLabel.setObjectName("zVelLabel") self.gridLayout_6.addWidget(self.zVelLabel, 3, 1, 1, 1) self.label_5 = QtWidgets.QLabel(Form) self.label_5.setLayoutDirection(QtCore.Qt.LeftToRight) self.label_5.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignTrailing | QtCore.Qt.AlignVCenter) self.label_5.setObjectName("label_5") self.gridLayout_6.addWidget(self.label_5, 3, 0, 1, 1) spacerItem = QtWidgets.QSpacerItem(20, 40, QtWidgets.QSizePolicy.Minimum, QtWidgets.QSizePolicy.Expanding) self.gridLayout_6.addItem(spacerItem, 4, 1, 1, 1) spacerItem1 = QtWidgets.QSpacerItem(20, 40, QtWidgets.QSizePolicy.Minimum, QtWidgets.QSizePolicy.Expanding) self.gridLayout_6.addItem(spacerItem1, 0, 1, 1, 1) self.gridLayout_5.addLayout(self.gridLayout_6, 0, 1, 1, 1) self.gridLayout_2 = QtWidgets.QGridLayout() self.gridLayout_2.setObjectName("gridLayout_2") self.cwButton = QtWidgets.QPushButton(Form) self.cwButton.setObjectName("cwButton") self.gridLayout_2.addWidget(self.cwButton, 0, 2, 1, 1) self.leftButton = QtWidgets.QPushButton(Form) self.leftButton.setObjectName("leftButton") self.gridLayout_2.addWidget(self.leftButton, 2, 0, 1, 1) self.rightButton = QtWidgets.QPushButton(Form) self.rightButton.setObjectName("rightButton") self.gridLayout_2.addWidget(self.rightButton, 2, 2, 1, 1) self.backwardButton = QtWidgets.QPushButton(Form) self.backwardButton.setObjectName("backwardButton") self.gridLayout_2.addWidget(self.backwardButton, 3, 1, 1, 1) self.ccwButton = QtWidgets.QPushButton(Form) self.ccwButton.setObjectName("ccwButton") self.gridLayout_2.addWidget(self.ccwButton, 0, 0, 1, 1) self.forwardButton = QtWidgets.QPushButton(Form) self.forwardButton.setObjectName("forwardButton") self.gridLayout_2.addWidget(self.forwardButton, 1, 1, 1, 1) self.gridLayout_5.addLayout(self.gridLayout_2, 2, 1, 1, 1) self.gridLayout_3 = QtWidgets.QGridLayout() self.gridLayout_3.setObjectName("gridLayout_3") self.decreaseAltButton = QtWidgets.QPushButton(Form) self.decreaseAltButton.setObjectName("decreaseAltButton") self.gridLayout_3.addWidget(self.decreaseAltButton, 2, 0, 1, 1) self.increaseAltButton = QtWidgets.QPushButton(Form) self.increaseAltButton.setObjectName("increaseAltButton") self.gridLayout_3.addWidget(self.increaseAltButton, 1, 0, 1, 1) self.gridLayout_5.addLayout(self.gridLayout_3, 2, 3, 1, 1) spacerItem2 = QtWidgets.QSpacerItem(20, 40, QtWidgets.QSizePolicy.Minimum, QtWidgets.QSizePolicy.Expanding) self.gridLayout_5.addItem(spacerItem2, 0, 5, 1, 1) spacerItem3 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) self.gridLayout_5.addItem(spacerItem3, 2, 2, 1, 1) spacerItem4 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) self.gridLayout_5.addItem(spacerItem4, 2, 0, 1, 1) spacerItem5 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) self.gridLayout_5.addItem(spacerItem5, 2, 8, 1, 1) spacerItem6 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) self.gridLayout_5.addItem(spacerItem6, 2, 4, 1, 1) self.saveButton = QtWidgets.QPushButton(Form) self.saveButton.setObjectName("saveButton") self.gridLayout_5.addWidget(self.saveButton, 2, 7, 1, 1) self.saveButton.setEnabled(False) self.batteryLabel = QtWidgets.QLabel(Form) self.batteryLabel.setObjectName("batteryLabel") self.gridLayout_5.addWidget(self.batteryLabel, 0, 8, 1, 1) self.label_2 = QtWidgets.QLabel(Form) self.label_2.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignTrailing | QtCore.Qt.AlignVCenter) self.label_2.setObjectName("label_2") self.gridLayout_5.addWidget(self.label_2, 0, 7, 1, 1) spacerItem7 = QtWidgets.QSpacerItem(20, 40, QtWidgets.QSizePolicy.Minimum, QtWidgets.QSizePolicy.Expanding) self.gridLayout_5.addItem(spacerItem7, 1, 7, 1, 1) self.retranslateUi(Form) self.ccwButton.clicked.connect(self.ccw) self.takeoffButton.clicked.connect(self.takeoff) self.saveButton.clicked.connect(self.save) self.resetButton.clicked.connect(self.reset) self.rightButton.clicked.connect(self.right) self.leftButton.clicked.connect(self.move_left) self.landButton.clicked.connect(self.land) self.increaseAltButton.clicked.connect(self.increase_alt) self.forwardButton.clicked.connect(self.forward) self.decreaseAltButton.clicked.connect(self.decrease_alt) self.cwButton.clicked.connect(self.cw) self.backwardButton.clicked.connect(self.backward) Form.destroyed.connect(self.quit) QtCore.QMetaObject.connectSlotsByName(Form) self.toggleButtonEnabled() def forward(self): self.drone.move(forward=1) print("moving drone forward") def backward(self): self.drone.move(backward=1) print("moving drone backward") def right(self): self.drone.move(right=1) print("moving drone right") def move_left(self): self.drone.move(left=1) print("moving drone left") def cw(self): self.drone.move(cw=1) print("rotating clockwise") def ccw(self): self.drone.move(ccw=1) print("rotating counter-clockwise") def increase_alt(self): self.drone.move(up=1) print("increasing altitude") def decrease_alt(self): self.drone.move(down=1) print("decreasing altitude") def takeoff(self): try: print("takeoff") self.toggleButtonEnabled() self.drone.takeoff() except: print('Not connected to a drone') return if self.drone.navdata: self.toggleButtonEnabled() self.logger = Logger() self.begin_log() print("taking off") def land(self): self.toggleButtonEnabled() while self.drone.state.fly_mask: self.drone.land() self.logger.currently_logging = False print("landing") def save(self): self.logger.saveFile() def at_ref(self, emergency=False): p = 0b10001010101000000000000000000 if emergency: p += 0b0100000000 at("REF", self, [p]) def reset(self): self.at(self.at_ref, False, True) self.at(self.at_ref, False, False) print("drone reset") def begin_log(self): "Starts the thread and runs log_data below" self.logger.currently_logging = True log_thread = Thread(target=self.log_data) log_thread.start() def log_data(self): "Writes data to the CSV every .25 seconds" while self.logger.currently_logging: self.logger.writer([ self.logger.time, self.drone.navdata.demo.vx, self.drone.navdata.demo.vy, self.drone.navdata.demo.vz, self.drone.navdata.demo.theta, self.drone.navdata.demo.phi, self.drone.navdata.demo.psi, self.drone.navdata.demo.altitude ]) sleep(.25) self.logger.time += .25 def retranslateUi(self, Form): _translate = QtCore.QCoreApplication.translate Form.setWindowTitle(_translate("GameOfDrones GUI", "GameOfDrones GUI")) self.resetButton.setToolTip( _translate("Form", "<html><head/><body><p>Reset (P)</p></body></html>")) self.resetButton.setText(_translate("Form", "Reset")) self.resetButton.setShortcut(_translate("Form", "P")) self.takeoffButton.setToolTip( _translate("Form", "<html><head/><body><p>Take off (T)</p></body></html>")) self.takeoffButton.setText(_translate("Form", "Takeoff")) self.takeoffButton.setShortcut(_translate("Form", "T")) self.landButton.setToolTip( _translate("Form", "<html><head/><body><p>Land (L)</p></body></html>")) self.landButton.setText(_translate("Form", "Land")) self.landButton.setShortcut(_translate("Form", "L")) # self.label.setText(_translate("Form", "X Velocity:")) # self.xVelLabel.setText(_translate("Form", "TextLabel")) # self.yVelLabel.setText(_translate("Form", "TextLabel")) # self.label_3.setText(_translate("Form", "Y Velocity:")) # self.zVelLabel.setText(_translate("Form", "TextLabel")) # self.label_5.setText(_translate("Form", "Z Velocity:")) self.cwButton.setToolTip( _translate( "Form", "<html><head/><body><p>Rotate the drone clockwise (E)</p></body></html>" )) self.cwButton.setText(_translate("Form", "Clockwise")) self.cwButton.setShortcut(_translate("Form", "E")) self.leftButton.setToolTip( _translate( "Form", "<html><head/><body><p>Move the drone left (A)</p></body></html>" )) self.leftButton.setText(_translate("Form", "Left")) self.leftButton.setShortcut(_translate("Form", "A")) self.rightButton.setToolTip( _translate( "Form", "<html><head/><body><p>Move the drone right (D)</p></body></html>" )) self.rightButton.setText(_translate("Form", "Right")) self.rightButton.setShortcut(_translate("Form", "D")) self.backwardButton.setToolTip( _translate( "Form", "<html><head/><body><p>Move the drone backward (S)</p></body></html>" )) self.backwardButton.setText(_translate("Form", "Backward")) self.backwardButton.setShortcut(_translate("Form", "S")) self.ccwButton.setToolTip( _translate( "Form", "<html><head/><body><p>Rotate the drone counter clockwise (Q)</p></body></html>" )) self.ccwButton.setText(_translate("Form", "AntiClockwise")) self.ccwButton.setShortcut(_translate("Form", "Q")) self.forwardButton.setToolTip( _translate( "Form", "<html><head/><body><p>Move the drone forward (W)</p></body></html>" )) self.forwardButton.setText(_translate("Form", "Forward")) self.forwardButton.setShortcut(_translate("Form", "W")) self.decreaseAltButton.setToolTip( _translate( "Form", "<html><head/><body><p>Decrease the drone\'s altitude (C)</p></body></html>" )) self.decreaseAltButton.setText(_translate("Form", "Decrease Altitude")) self.decreaseAltButton.setShortcut(_translate("Form", "C")) self.increaseAltButton.setToolTip( _translate( "Form", "<html><head/><body><p>Increase the drone\'s altitude (space)</p></body></html>" )) self.increaseAltButton.setText(_translate("Form", "Increase Altitude")) self.increaseAltButton.setShortcut(_translate("Form", "Space")) self.saveButton.setText(_translate("Form", "Save")) # self.batteryLabel.setText(_translate("Form", "TextLabel")) # self.label_2.setText(_translate("Form", "Battery:")) def quit(self): self.drone.close() self.logger.currently_logging = False # sys.exit(app.exec_()); def toggleButtonEnabled(self): self.ccwButton.setEnabled(not self.ccwButton.isEnabled()) self.cwButton.setEnabled(not self.cwButton.isEnabled()) self.forwardButton.setEnabled(not self.forwardButton.isEnabled()) self.rightButton.setEnabled(not self.rightButton.isEnabled()) self.leftButton.setEnabled(not self.leftButton.isEnabled()) self.backwardButton.setEnabled(not self.backwardButton.isEnabled()) self.increaseAltButton.setEnabled( not self.increaseAltButton.isEnabled()) self.decreaseAltButton.setEnabled( not self.decreaseAltButton.isEnabled()) self.landButton.setEnabled(not self.landButton.isEnabled()) self.takeoffButton.setEnabled(not self.takeoffButton.isEnabled()) self.saveButton.setEnabled(not self.saveButton.isEnabled()) def update_labels(self): self.xVelLabel.setText(self.drone.navdata.demo.vx) self.yVelLabel.setText(self.drone.navdata.demo.vy) self.zVelLabel.setText(self.drone.navdata.demo.vz) self.batteryLabel.setText( self.drone.navdata.demo.vbat_flying_percentage)
class App(QWidget): def __init__(self): super().__init__() self.title = 'GameOfDrones Test GUI' self.left = 100 self.top = 100 self.width = 1024 self.height = 768 self.initUI() self.logger = Logger() self.drone = ARDrone(connect=True) self.drone.send(at.CONFIG('general:navdata_demo', True)) font = QFont() font.setBold(True) font.setPixelSize(18) self.setFont(font) # self.drone.navdata_ready.wait() def initUI(self): self.setWindowTitle(self.title) self.setGeometry(self.left, self.top, self.width, self.height) quit = QAction("Quit", self) quit.triggered.connect(self.closer) forwards_btn = QPushButton('\u25b2', self) forwards_btn.setToolTip('Moves the drone forward') forwards_btn.move(100, 630) forwards_btn.clicked.connect(self.forward) backwards_btn = QPushButton('\u25bc', self) backwards_btn.setToolTip('Moves the drone backward') backwards_btn.move(100, 690) backwards_btn.clicked.connect(self.backward) right_btn = QPushButton('\u25b6', self) right_btn.setToolTip('Moves the drone right') right_btn.move(160, 660) right_btn.clicked.connect(self.right) left_btn = QPushButton('\u25c0', self) left_btn.setToolTip('Moves the drone move_left') left_btn.move(40, 660) left_btn.clicked.connect(self.move_left) cw_btn = QPushButton('\u27f3', self) cw_btn.setToolTip('Rotates the drone cw') cw_btn.move(160, 590) cw_btn.clicked.connect(self.cw) ccw_btn = QPushButton('\u27f2', self) ccw_btn.setToolTip('Rotates the drone ccw') ccw_btn.move(40, 590) ccw_btn.clicked.connect(self.ccw) increase_alt_btn = QPushButton('\u21a5', self) increase_alt_btn.setToolTip('Increase Altitude') increase_alt_btn.move(280, 550) increase_alt_btn.clicked.connect(self.increase_alt) decrease_alt_btn = QPushButton('\u21a7', self) decrease_alt_btn.setToolTip('Decrease Altitude') decrease_alt_btn.move(280, 580) decrease_alt_btn.clicked.connect(self.decrease_alt) takeoff_btn = QPushButton('\u21eb', self) takeoff_btn.setToolTip('Takeoff') takeoff_btn.move(280, 650) takeoff_btn.clicked.connect(self.takeoff) land_btn = QPushButton('\u2913', self) land_btn.setToolTip('Land') land_btn.move(280, 680) land_btn.clicked.connect(self.land) reset_btn = QPushButton('\u238C', self) reset_btn.setToolTip('Reset') reset_btn.move(280, 710) reset_btn.clicked.connect(self.reset) # close_btn = QPushButton('&X Close (use this not the window)', self) # close_btn.setToolTip('Reset') # close_btn.move(280, 10) # close_btn.clicked.connect(self.quit) self.show() @pyqtSlot(name="forward") def forward(self): self.drone.move(forward=1) print("moving drone forward") @pyqtSlot(name="backward") def backward(self): self.drone.move(backward=1) print("moving drone backward") @pyqtSlot(name="right") def right(self): self.drone.move(right=1) print("moving drone right") @pyqtSlot(name="move_left") def move_left(self): self.drone.move(left=1) print("moving drone left") @pyqtSlot(name="cw") def cw(self): self.drone.move(cw=1) print("rotating clockwise") @pyqtSlot(name="ccw") def ccw(self): self.drone.move(ccw=1) print("rotating counter-clockwise") @pyqtSlot(name="IncreaseAlt") def increase_alt(self): self.drone.move(up=1) print("increasing altitude") @pyqtSlot(name="DecreaseAlt") def decrease_alt(self): self.drone.move(down=1) print("decreasing altitude") @pyqtSlot(name="takeoff") def takeoff(self): self.logger = Logger() self.begin_log() while not self.drone.state.fly_mask: self.drone.takeoff() print("taking off") @pyqtSlot(name="land") def land(self): while self.drone.state.fly_mask: self.drone.land() self.logger.currently_logging = False print("landing") @pyqtSlot(name="Reset") def reset(self): if not self.drone.state.fly_mask: self.drone.state.emergency_mask = False print("drone reset") def begin_log(self): "Starts the thread and runs log_data below" self.logger.currently_logging = True log_thread = Thread(target=self.log_data) log_thread.start() def log_data(self): "Writes data to the CSV every .25 seconds" while self.logger.currently_logging: self.logger.writer([self.logger.time, self.drone.navdata.demo.vx, self.drone.navdata.demo.vy, self.drone.navdata.demo.vz, self.drone.navdata.demo.theta, self.drone.navdata.demo.phi, self.drone.navdata.demo.psi, self.drone.navdata.demo.altitude]) sleep(.25) self.logger.time += .25 def closer(self): self.close() sys.exit(self.exec_)
def updateFrame(): global framein global running cam = cv2.VideoCapture('tcp://192.168.1.1:5555') while True: running, framein = cam.read() t = threading.Thread(name= 'updateFrame',target = updateFrame) t.start() drone = ARDrone() drone.navdata_ready.wait() state = 1 drone.send(at.CONFIG('video:video_channel',1)) while True: if running: #cv2.imwrite("frame.png", framein) frame = cv2.cvtColor(framein, cv2.COLOR_BGR2GRAY) framecon = framein * 1 frame = frame * 1.5 #cv2.imwrite("boost.png", frame) frame = cv2.inRange(frame,0,70) #cv2.imwrite("thres.png", frame) frame = cv2.medianBlur(frame,5) kernel = np.ones((3,3),np.uint8) erosion = cv2.erode(frame,kernel,iterations = 1) frame = frame - erosion #cv2.imwrite("edges.png", frame)
class Drone: def __init__(self): logging.basicConfig(level=logging.INFO) self.droneLog = logging.getLogger("Drone") self.appLog = logging.getLogger("App") # drone protocol variables self.status = 0 # offline, check, ready, flying, land, error self.velocity = 0.5 self.battery = 100 self.altitude = 0 self.errorCode = 0 self.flyMode = 3 #setup video stream self.video = VideoClient('192.168.1.1', 5555) self.video.connect() self.video.video_ready.wait() self.droneLog.info("Video ready") #drone setup self.drone = ARDrone() self.drone.navdata_ready.wait() self.droneLog.info("Nav ready") self.drone.send(pyardrone.at.CONFIG("video:video_channel","0")) time.sleep(5) self.drone.send(pyardrone.at.CONFIG('general:navdata_demo',True)) time.sleep(5) self.demo = self.drone.navdata.demo # drone camera and flying variable self.camera = None self.frontCamera = True self.flying = False #self.droneLog.info(self.camera.get(cv2.CAP_PROP_FPS)) # drone coords self.x = 0 self.y = 0 self.z = 0 self.orientation = 0 #One time check of camera status # ok, frame = self.camera.read() # if not ok: # self.droneLog.info('Error: Camera not working') # self.status = 5 # self.errorCode = 6 self.checkDrone() #launched when drone connects def initDrone(self): self.checkDrone() # get latest info from app def sendAppData(self, update): # handle new flight mode if self.flyMode != update[1]: self.flyMode = update[1] self.updateFlightMode() #self.appLog.info(str(update)) # handle button pressed self.handleButton(update[0]) # send latest drone info to app def getDroneData(self): # update info self.demo = self.drone.navdata.demo self.battery = self.getBattery() self.altitude = self.getAltitude() return self.updateInfo() # Handle button logic def handleButton(self, button): #land if drone is flying and battery is 20 or less if self.flying and self.battery <= 20: self.land() if self.status != 5: # launch/land if button == 1: # launch if not self.flying: self.launch() # land else: # land and check drone self.land() # emergency land elif button == 2 and self.flying: self.emergencyLand() # move drone according to button elif button >= 3 and button <= 10 and self.flying: self.moveDrone(button) # switch cameras elif button == 11: self.switchCamera() elif self.flying and button ==12: self.landAtBase() # Move drone logic def moveDrone(self, move): # code to move drone goes here if move == 3: self.droneLog.info(f"Moving {self.velocity} up.") # self.altitude += self.velocity self.y += self.velocity self.drone.move(up=self.velocity) elif move == 4 and not self.y <= 0: self.droneLog.info(f"Moving {self.velocity} down.") # self.altitude -= self.velocity self.y -= self.velocity self.drone.move(down=self.velocity) elif move == 5: self.droneLog.info(f"Moving {self.velocity} left.") self.x += self.velocity self.drone.move(left=self.velocity) elif move == 6: self.droneLog.info(f"Moving {self.velocity} right.") self.x -= self.velocity self.drone.move(right=self.velocity) elif move == 7: self.droneLog.info(f"Moving {self.velocity} foward.") self.z += self.velocity self.drone.move(forward=self.velocity) elif move == 8: self.droneLog.info(f"Moving {self.velocity} backwards.") self.z -= self.velocity self.drone.move(backward=self.velocity) elif move == 9: self.droneLog.info(f"Rotating {self.velocity} left.") self.orientation += self.velocity self.drone.move(cw=self.velocity) elif move == 10: self.droneLog.info(f"Rotating {self.velocity} right.") self.orientation -= self.velocity self.drone.move(ccw=self.velocity) #self.battery -= 1 # get latest coords and print self.droneLog.info(str(self.getCoords())) #return True # get frame from camera def getFrame(self): if self.video.video_ready: return self.video.frame # change resolution, only supports native resolutions of camera def changeCameraResolution(self, res): #self.droneLog.info("Camera resolution changed to" + str(res)) #self.camera.set(cv2.CAP_PROP_FRAME_WIDTH, res[0]) #self.camera.set(cv2.CAP_PROP_FRAME_HEIGHT, res[1]) return True # launch drone def launch(self): self.droneLog.info("Launching!") # code to launch goes here while not self.drone.state.fly_mask: self.drone.takeoff() time.sleep(5) self.status = 3 self.flying = True #self.battery -= 5 self.droneLog.info("Launched with fly mode: " + str(self.flyMode)) # land drone def land(self): # code to land goes here while self.drone.state.fly_mask: self.drone.land() time.sleep(5) self.flying = False self.droneLog.info("Landed!") self.checkDrone() # Emergency land drone def emergencyLand(self): self.droneLog.info("Emergency landing!") # code to emergency land goes here while self.drone.state.fly_mask: self.drone.emergency() time.sleep(5) self.flying = False self.status = 1 self.checkDrone() self.droneLog.info("Emergency landed.") # attempt to return drone to base def landAtBase(self): self.droneLog.info("Landing back at base.") # code goes here self.flying = False self.status = 2 self.droneLog.info("Landed back at base.") # check if drone can fly def checkDrone(self): self.droneLog.info("Checking!") self.status = 1 # code to check hardware goes here self.battery = self.getBattery() self.altitude = self.getAltitude() self.orientation = self.getOrientation() #low battery check if self.battery <= 20 : self.status = 5 self.errorCode = 1 self.droneLog.info("Error: Low Battery") else: self.status = 2 self.droneLog.info("Check passed.") # update flight mode def updateFlightMode(self): self.droneLog.info("Switched flying mode: "+ str(self.flyMode)) # toggle cameras def switchCamera(self): self.droneLog.info("Switching camera.") # code to switch between cameras goes here if self.frontCamera: self.frontCamera = False self.drone.send(pyardrone.at.CONFIG("video:video_channel","1")) self.droneLog.info("Switched to bottom camera.") else: self.frontCamera = True self.drone.send(pyardrone.at.CONFIG("video:video_channel","0")) self.droneLog.info("Switched to front camera.") time.sleep(5) # get drone's battery def getBattery(self): # code to get hardware battery goes here # self.log.info("Battery: " + str(self.battery)) return int(self.demo.vbat_flying_percentage) # get drone's altitude def getAltitude(self): # code to get altitude goes here # self.log.info("Drone altitude: " + str( self.y)) return int(self.demo.altitude / 0.0328084) # update drone info list def updateInfo(self): return self.status, self.battery, int(self.velocity), self.altitude, self.errorCode # stop whatever the drone is doing def stopEverything(self): # code goes here self.droneLog.info("Drone stopped.") def getCoords(self): return self.x, self.y, self.z, self.orientation def resetDrone(self): if self.flying: self.land() # drone coords self.x = 0 self.y = 0 self.z = 0 self.orientation = self.getOrientation() # get drone's orientation def getOrientation(self): # code goes here return 90 # drones default at a 90 degree angle facing foward
roll_ref=math.atan(acc_y/9.8)/1.3 elif (quad==3): pitch_ref=-math.atan(acc_x/9.8)/1.3 roll_ref=-math.atan(acc_y/9.8)/1.3 elif (quad==4): pitch_ref=math.atan(acc_x/9.8)/1.3 roll_ref=-math.atan(acc_y/9.8)/1.3 else: pitch_ref=0 roll_ref=0 pitch_ref= round(pitch_ref,6) roll_ref= round(roll_ref,6) #calculate at command reference adn direction print( pitch_ref, roll_ref,u[0]) drone.send(at.PCMD(at.PCMD.flag.progressive, roll_ref, pitch_ref, 0, 0)) #record results in a text file file.write( str(drone.navdata.demo.theta)+" "+str(drone.navdata.demo.phi)+" "+str(u[0])+" "+str(e[0])+"\n") #data=""+str(x_angle)+" "+str(y_angle)+" "+str(x_off)+" "+str(y_off)+"\n" #file.write(data) time.sleep(0.05) count=count+1 file.close() drone.land() sys.exit()
import time import cv2 import numpy as np from pyardrone import ARDrone from pyardrone import at drone = ARDrone() drone.send(at.CONFIG("custom:application_id", "2902050D")) drone.send(at.CONFIG("custom:session_id", "2902150D")) drone.send(at.CTRL(5, 0)) drone.send(at.CONFIG_IDS("2902150D", "00000000", "2902050D")) drone.send(at.CONFIG("custom:application_desc", "andet")) drone.send(at.CTRL(5, 0)) drone.send(at.CTRL(4, 0)) drone.send(at.CTRL(5, 0)) drone.send(at.CONFIG_IDS("2902150D", "00000000", "2902050D")) drone.send(at.CONFIG("video:video_codec", "131")) drone.send(at.CTRL(5, 0)) drone.send(at.CTRL(4, 0)) drone.send(at.CTRL(5, 0)) drone.send(at.CONFIG_IDS("2902150D", "00000000", "2902050D")) drone.send(at.CONFIG("video:max_bitrate", "4000")) drone.navdata_ready.wait() first = 1 try: while True: