class Drone(object):
def __init__(self):
self.d = ARDrone()
self.d.navdata_ready.wait()
def takeoff(self):
"""Start flying the drone."""
self.d.takeoff()
def land(self):
"""Land the drone."""
self.d.land()
def move(self, direction):
"""
Move the drone forward or backward or rotate the drone for 't' seconds.
The 'direction' argument can be:
- 'forward': move forward
- 'backward': move backward
- 'rotate_right': rotate 90 degrees to the right
- 'rotate_left': rotate 90 degrees to the left
"""
if direction == 'forward':
self.forward(t=1)
elif direction == 'backward':
self.backward(t=1)
elif direction == 'rotate_right':
self.cw(t=0.4)
elif direction == 'rotate_left':
self.ccw(t=0.4)
else:
raise ValueError(
'Given direction {} not supported!'.format(direction))
self.d.hover()
def forward(self, t=0.3, s=0.1):
t_end = time() + t
while time() < t_end:
self.d.move(forward=s)
def backward(self, t=0.3, s=0.1):
t_end = time() + t
while time() < t_end:
self.d.move(backward=s)
def cw(self, t=0.08, s=0.8):
t_end = time() + t
while time() < t_end:
self.d.move(cw=s)
def ccw(self, t=0.08, s=0.8):
t_end = time() + t
while time() < t_end:
self.d.move(ccw=s)
def drone_output(current_controls: Controls) -> None:
"""Connects to the Parrot drone using its API and sends controls.
Args:
current_controls (Controls): Cross-thread controls data.
"""
drone = ARDrone()
drone.navdata_ready.wait()
directions = {
action: 1 if control.state else 0
for (action, control) in current_controls.items()
if action not in ['takeoff', 'land']
}
drone.move(**directions)
if current_controls['land'].state:
drone.land()
if current_controls['takeoff'].state:
drone.takeoff()
# threading.Thread(target=recordVideo, args=(drone,)).start()
# navThread = threading.Thread(target=printNavdata, args=(drone,)).start()
out = cv2.VideoWriter('test-flight-with-yolo-4.mp4', 0x7634706d, 2.0, (640,360))
with suppress(KeyboardInterrupt):
land_time = time.time() + 15
while True:
# image = cv2.resize(image,(608,608), interpolation = cv2.INTER_AREA)
if not drone.state.fly_mask:
drone.takeoff()
image = drone.frame
fps, last_frame_shown = fps_calc(time.time(), last_frame_shown)
if time.time() > land_time:
while drone.state.fly_mask:
drone.land()
break
bbox = tiny_yolo.yolo_update.get_bounding_box(image, net, background)
if bbox:
x, y, w, h = bbox
image = tiny_yolo.yolo_update.add_bounding_box(image, bbox)
image = tiny_yolo.yolo_update.add_size(image, w)
## Control drone
if drone.state.fly_mask:
if w < 170:
go_back=False
if w >= 170:
drone.move(backward=0.9)
def main():
try:
while True:
# 受信
msg, address = s.recvfrom(8192)
msg = msg.decode('utf-8')
print("message: " + msg + "\nfrom: " + address)
s.sendto(msg.encode(), (ADDRESS, PORT))
if msg == "takeoff": #離陸
client.takeoff()
elif msg == "tracking": #追尾
tracking()
elif msg == "land": #着陸
client.land()
else:
pass
finally:
s.close()
client.close()
if __name__ == '__main__':
try:
main()
except KeyboardInterrupt:
print("Ctrl+Cで停止しました")
finally:
client.land()
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_)
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
