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:
