def setUp(self):
self.drone = ARDrone(connect=False)
self.drone.get_raw_config = mock.Mock(
spec=self.drone.get_raw_config,
return_value=config_file_example,
)
self.drone.send = mock.Mock(spec=self.drone.send)
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()
def camera_input(current_image: np.ndarray) -> None:
"""Reads data from the drone camera and stores it in the shared buffer.
Args:
current_image (np.ndarray): Cross-thread image data.
"""
from pyardrone.video import VideoMixin
from pyardrone import ARDrone
drone = ARDrone()
print(drone)
current_image = drone.frame
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)
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()
#検出・計算・動作の一連の流れ
#allからemergency追加・png出力無し
import cv2
import numpy as np
import sys
from pyardrone import ARDrone
import logging
logging.basicConfig(level=logging.DEBUG)
aruco = cv2.aruco #arucoライブラリ
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
# ドローンカメラ
client = ARDrone()
client.video_ready.wait()
parameters = aruco.DetectorParameters_create()
# CORNER_REFINE_NONE, no refinement. CORNER_REFINE_SUBPIX, do subpixel refinement. CORNER_REFINE_CONTOUR use contour-Points
parameters.cornerRefinementMethod = aruco.CORNER_REFINE_CONTOUR
####################################################################
def drone_chage_state(forward,land,hover):
print(forward,back,land,hover)
if land:
drone.land()
elif hover:
drone.hover()
from pyardrone import ARDrone
from contextlib import suppress
drone = ARDrone()
drone.navdata_ready.wait()
with suppress(KeyboardInterrupt):
while True:
print(drone.state)
drone.close()
def __init__(self):
self.d = ARDrone()
self.d.navdata_ready.wait()
def __init__(self):
self.logger = Logger()
self.drone = ARDrone(connect=True)
self.drone.send(at.CONFIG('general:navdata_demo', True))
from libs import communications from pyardrone import ARDrone c = communications.CommunicationManager() d = ARDrone() d.emergency() #c.close_serial_port()
class FreeMovement:
communication = communications.CommunicationManager()
filtering = filterings.FilteringManager()
report = datalog.DatalogManager()
drone = ARDrone()
drone.emergency()
drone.trim()
drone.navdata_ready.wait() # wait until NavData is ready
drone.set_navdata_available()
navdata = drone.get_navdata()
initial_yaw = navdata.psi
bat = navdata.vbat_flying_percentage
print('Battery = ' + str(bat) + '%')
mixer.init()
alert1 = mixer.Sound('beep-07.wav')
alert2 = mixer.Sound('beep-08.wav')
communication.open_serial_port()
max_samples = 14
watch_samples_counter = -1
handside = 'left'
x_axis_acceleration = []
y_axis_acceleration = []
z_axis_acceleration = []
acceleration = []
clap = 0
sample_time = 0.05
time_limit = 120
# Datalog time
report.create_file('detection.txt')
report.record_data('detection.txt', 'direction', 'x', 'y', 'z')
mode = [0]
line = []
threading._start_new_thread(input_thread, (line, ))
x_direction = ' '
old_x_direction = ' '
y_direction = ' '
old_y_direction = ' '
take_off = False
time_initial = time.time()
last_activity = time_initial
last_down_movement = 0
while time.time() - time_initial <= time_limit:
if line:
break
if drone.state.vbat_low == 1:
print('low bat')
break
bytes_to_read = communication.send_data_request()
inbyte = communication.read_data(bytes_to_read)
enter = filtering.data_availability(bytes_to_read, inbyte)
if (bytes_to_read >= 7 and inbyte[3] == 1
and enter is True) or (bytes_to_read == 14 and inbyte[10] == 1
and enter is True):
watch_samples_counter += 1
if watch_samples_counter == 0:
alert1.play()
x_axis_acceleration.append(inbyte[bytes_to_read - 3])
y_axis_acceleration.append(inbyte[bytes_to_read - 2])
z_axis_acceleration.append(inbyte[bytes_to_read - 1])
filtering.filter_acceleration(x_axis_acceleration,
watch_samples_counter)
filtering.filter_acceleration(y_axis_acceleration,
watch_samples_counter)
filtering.filter_acceleration(z_axis_acceleration,
watch_samples_counter)
x_axis_acceleration[watch_samples_counter], y_axis_acceleration[
watch_samples_counter], z_axis_acceleration[
watch_samples_counter] = filtering.handside_mode_3_axis(
x_axis_acceleration[watch_samples_counter],
y_axis_acceleration[watch_samples_counter],
z_axis_acceleration[watch_samples_counter], handside)
acceleration = [
x_axis_acceleration[watch_samples_counter],
y_axis_acceleration[watch_samples_counter],
z_axis_acceleration[watch_samples_counter]
]
if np.linalg.norm(acceleration) >= 130 and time.time(
) - last_activity <= 3 and take_off is False:
print('CLAP!!!')
time.sleep(1)
alert2.play()
if clap == 1:
take_off = True
while not drone.state.fly_mask:
drone.takeoff()
drone.hover()
time.sleep(10)
alert1.play()
x_axis_acceleration[watch_samples_counter] = 15
y_axis_acceleration[watch_samples_counter] = 15
sample_time = 0.5
time.sleep(1)
clap += 1
mode.append(0)
if take_off is True and x_axis_acceleration[watch_samples_counter] >= -85 \
and x_axis_acceleration[watch_samples_counter] <= -55:
mode[watch_samples_counter] = 1
print('mode = 1 (left...)')
elif take_off is True and z_axis_acceleration[watch_samples_counter] >= 55 \
and z_axis_acceleration[watch_samples_counter] <= 85:
mode[watch_samples_counter] = 2
print('mode = 2 (up...)')
if mode[watch_samples_counter - 1] != mode[watch_samples_counter]:
print('MODE CHANGE')
if mode[watch_samples_counter-2] == 1 and take_off is True \
and x_axis_acceleration[watch_samples_counter] <= -20 \
and x_axis_acceleration[watch_samples_counter] >= -55 and abs(
y_axis_acceleration[watch_samples_counter]) < abs(
x_axis_acceleration[watch_samples_counter]) and old_x_direction == ' '\
and z_axis_acceleration[watch_samples_counter] < -20:
x_direction = 'left '
alert1.play()
print(x_direction)
initial_flight_time = time.time()
while time.time() - initial_flight_time <= 2:
drone.move(left=0.1)
last_activity = time.time()
elif mode[watch_samples_counter-2] == 1 and take_off is True \
and x_axis_acceleration[watch_samples_counter] <= -20 \
and x_axis_acceleration[watch_samples_counter] >= -55 and abs(
y_axis_acceleration[watch_samples_counter]) < abs(
x_axis_acceleration[watch_samples_counter]) and old_x_direction == ' ' \
and z_axis_acceleration[watch_samples_counter] > 20:
x_direction = 'right '
alert1.play()
print(x_direction)
initial_flight_time = time.time()
while time.time() - initial_flight_time <= 2:
drone.move(right=0.1)
last_activity = time.time()
elif mode[watch_samples_counter-2] == 1 and take_off is True \
and y_axis_acceleration[watch_samples_counter] >= 30 and abs(
x_axis_acceleration[watch_samples_counter]) < abs(
y_axis_acceleration[watch_samples_counter]) and old_y_direction == ' ':
y_direction = 'backward'
alert1.play()
print(y_direction)
initial_flight_time = time.time()
while time.time() - initial_flight_time <= 2:
drone.move(backward=0.1)
last_activity = time.time()
elif mode[watch_samples_counter-2] == 1 and take_off is True \
and y_axis_acceleration[watch_samples_counter] <= -20 and abs(
x_axis_acceleration[watch_samples_counter]) < abs(
y_axis_acceleration[watch_samples_counter]) and old_y_direction == ' ':
y_direction = 'forward'
alert1.play()
print(y_direction)
initial_flight_time = time.time()
while time.time() - initial_flight_time <= 2:
drone.move(forward=0.1)
last_activity = time.time()
elif mode[watch_samples_counter-2] == 2 and take_off is True \
and y_axis_acceleration[watch_samples_counter] >= 30 and abs(
x_axis_acceleration[watch_samples_counter]) < abs(
y_axis_acceleration[watch_samples_counter]) and old_y_direction == ' ':
y_direction = 'down'
alert1.play()
print(y_direction)
if time.time() - last_down_movement <= 5:
time_initial = 0
initial_flight_time = time.time()
while time.time() - initial_flight_time <= 2:
drone.move(down=0.3)
last_activity = time.time()
last_down_movement = time.time()
elif mode[watch_samples_counter-2] == 2 and take_off is True \
and y_axis_acceleration[watch_samples_counter] <= -20 and abs(
x_axis_acceleration[watch_samples_counter]) < abs(
y_axis_acceleration[watch_samples_counter]) and old_y_direction == ' ':
y_direction = 'up'
alert1.play()
print(y_direction)
initial_flight_time = time.time()
while time.time() - initial_flight_time <= 2:
drone.move(up=0.3)
last_activity = time.time()
if time.time() - last_activity >= 1:
last_activity = time.time()
print('1 sec without movement')
report.record_data(
'detection.txt',
str(mode[watch_samples_counter]) + x_direction + y_direction,
x_axis_acceleration[watch_samples_counter],
y_axis_acceleration[watch_samples_counter],
z_axis_acceleration[watch_samples_counter])
old_x_direction = x_direction
x_direction = ' '
old_y_direction = y_direction
y_direction = ' '
time.sleep(sample_time)
alert2.play()
time.sleep(1)
navdata = drone.get_navdata()
bat = navdata.vbat_flying_percentage
print('Battery = ' + str(bat) + '%')
while drone.state.fly_mask:
drone.land()
if not drone.state.fly_mask:
print('CIAO!')
