from djitellopy import Tello
import sys, time
if len(sys.argv) > 1:
tello = Tello(sys.argv[1])
else:
tello = Tello()
tello.connect()
speed = tello.query_speed()
snr = tello.query_wifi_signal_noise_ratio()
sdk = tello.query_sdk_version()
serial = tello.query_serial_number()
print("Speed = ", speed)
print("Battery = ", tello.get_battery())
print("Duration = ", tello.get_flight_time())
print("Height = ", tello.get_height())
print("Distance = ", tello.get_distance_tof())
print("Barometer = ", tello.get_barometer())
print("Attitude = ", tello.get_pitch(), tello.get_roll(), tello.get_yaw())
print("WiFi SNR = ", snr)
print("SDK Version = ", sdk)
print("Serial Number = ", serial)
print(tello.get_current_state())
me.for_back_velocity = 0
me.left_right_velocity = 0
me.up_down_velocity = 0
me.yaw_velocity = 0
me.speed = 0
print("Done Connecting:", time.process_time())
print("Battery:", me.get_battery())
d = 0
dh = 130
while True:
print("Beginning Takeoff:", time.process_time())
me.takeoff()
print("Done Takeoff:", time.process_time())
me.move_up(50)
curr_height = me.get_height()
while curr_height > (dh + 5) or curr_height < (dh - 5):
if curr_height > (dh + 5):
me.move_down(20)
curr_height = me.get_height()
if curr_height < (dh - 5):
me.move_up(20)
curr_height = me.get_height()
print("Height", curr_height)
'''
print("Beginning Move:", time.process_time())
# me.__send_stick_command()
me.move_forward(100)
print("Done Move:", time.process_time())
'''
# mission pad detection
class Drone(object):
def __init__(self):
# Initialze Pygame
pygame.init()
# Create pygame window
pygame.display.set_caption("Tello-Game video stream")
self.screen = pygame.display.set_mode([960, 720])
# Create tello object to interact with called tello
self.tello = Tello()
# Master Movement Velocities
self.ForwardsBackwards = 0
self.movRightLeft = 0
self.UpDown = 0
self.rotateYaw = 0
self.send_rc_control = True
# create update timer
pygame.time.set_timer(USEREVENT + 1, 50)
def get_altitude(self):
try:
alt = self.tello.get_height() # Gives Long String
alt = re.findall(r'\d+',
alt) # Parse just altitude number from string
alt = alt[0]
alt = (int(alt) / 3.048) # Conversion from Decimeters to Feet
return (format(alt, '0.2f') + ' FT')
except IndexError:
print('***Altitude Read Error***') # console log error message
return 'Error' # Message for display
def check_battery(self):
try:
battery = self.tello.get_battery() # Gives Long String
battery = re.findall(
r'\d+', battery) # Parse just the battery number from string
battery = battery[0]
battery = int(battery)
if battery > 10:
return (str(battery) + ' % Bat')
elif battery == 15:
return 'LOW BATTERY WARNING 15%'
elif battery == 10:
return 'LOW BATTERY LAND NOW'
elif battery < 10: # Fail Safe
time.sleep(5)
self.tello.land()
return 'LOW BATTERY EMERGENCY LANDING'
except TypeError:
print(
'******Battery Read Error******') # Console log error message
return 'Error' # Error message for display
except IndexError:
return 'Error'
def run(self):
# Check For Controller and get Name
for i in range(0, pygame.joystick.get_count()):
try:
JOYSTICKS.append(pygame.joystick.Joystick(i))
JOYSTICKS[-1].init()
print("Detected joystick '%s'" % JOYSTICKS[-1].get_name())
except:
print("Controller Not Found")
break
if not self.tello.connect():
print("Tello not connected")
return
# Turns off video stream in case of crash
if not self.tello.streamoff():
print("Could not stop video stream")
return
if not self.tello.streamon():
print("Could not start video stream")
return
frame_read = self.tello.get_frame_read()
should_stop = False
while not should_stop:
font = pygame.font.SysFont('comicsansms', 20)
battery = self.check_battery()
altitude = self.get_altitude(
) # Check battery and Altitude every event
for event in pygame.event.get(
): # checks the event type and calls the corresponding method
if event.type == USEREVENT + 1:
self.update()
elif event.type == QUIT:
should_stop = True
elif self.movRightLeft or self.ForwardsBackwards > 0:
self.movRightLeft = 0
self.ForwardsBackwards = 0
elif event.type == JOYAXISMOTION:
self.StickMov(event.value, AXIS_NAMES[event.axis])
elif event.type == JOYBUTTONDOWN:
button = BUTTON_NAMES[event.button]
self.ButtonPushed(button)
elif event.type == JOYBUTTONUP:
button = BUTTON_NAMES[event.button]
if button != 'B' or 'Y':
self.ButtonReleased(button)
if frame_read.stopped:
frame_read.stop()
break
self.screen.fill([0, 0, 0])
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
frame = np.rot90(frame)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
batteryDisp = font.render(
battery, True,
(255, 0, 0)) # Add the battery display to the screen
self.screen.blit(batteryDisp, (50, 10))
altDisp = font.render(
altitude, True, (255, 0, 0)) # Add Altitude display to screen
self.screen.blit(altDisp, (50, 40))
pygame.display.update()
time.sleep(1 / FPS)
self.tello.end()
def ButtonPushed(
self,
button): # tracks which buttons on the controller are pressed
if button == 'A':
self.UpDown = S
elif button == 'X':
self.UpDown = -S
elif button == "B":
self.tello.land()
elif button == "Y":
self.tello.takeoff()
elif button == 'L_BUMPER':
self.rotateYaw = -YAWSPEED
elif button == 'R_BUMPER':
self.rotateYaw = YAWSPEED
def ButtonReleased(self, button):
# Stop moving
if button == 'A':
self.UpDown = 0
elif button == 'X':
self.UpDown = 0
elif button == 'L_BUMPER' or 'R_BUMPER':
self.rotateYaw = 0
def StickMov(self, movement, axis): # Tracks joystick movement
if axis == 'LEFT_Y':
if movement < 0.25: # controller inputs start at 0 and are sensitive helps prevent minor touches affecting flight
self.ForwardsBackwards = int(
S * movement
) # Governs the speed based on the input from the controller.
print(
'Move Back'
) # Made into int since dji tello package only accepts ints to its velocity function
elif movement > 0 and movement > -0.25:
self.ForwardsBackwards = int(S * movement)
print('Move Forwards')
elif axis == 'RIGHT_Y':
if movement > 0.25:
self.movRightLeft = int(S * movement)
print('Move Right')
elif movement < 0 and movement < -0.25:
self.movRightLeft = int(S * movement)
print('Move Left')
def update(self): # send the controls to the drone
if self.send_rc_control:
self.tello.send_rc_control(self.movRightLeft,
self.ForwardsBackwards, self.UpDown,
self.rotateYaw)
tello.connect()
print("Test 1 - Take off - Rotate - Land")
tello.takeoff()
tello.rotate_clockwise(360)
tello.land()
print("Test 2 - Take off - forward - back - left - right - Land")
tello.takeoff()
tello.move_forward(30)
tello.move_back(30)
tello.move_left(30)
tello.move_right(30)
tello.land()
print("Test 3 - Take off - Ascend - Descend - Land")
tello.takeoff()
tello.move_up(50)
tello.move_down(50)
tello.land()
print("Test 4 - Telemetry")
tello.takeoff()
tello.get_battery()
tello.get_current_state()
tello.get_height()
tello.get_pitch()
tello.get_roll()
tello.get_yaw()
tello.land
detector = MarkerDetector(aruco_dict, mtx, dist)
# loop start
# drone.takeoff()
while True:
# ret, frame = video_capture.read()
frame = drone.get_frame_read().frame
image, horizontal_error, vertical_error, frontal_error = detector.detect_and_compute_error_values(frame,
SET_POINT_X,
SET_POINT_Y,
SET_POINT_Z_cm,
target)
if target == 33 and drone.get_height():
action_x, action_y, action_z = 0, 30, 0
elif target == 42:
action_x, action_y, action_z = 0, -30, 0
else:
action_x, action_y, action_z = 0, 0, 0
if horizontal_error is not None:
drawer.draw_errors(image, horizontal_error, vertical_error, frontal_error, frontal_error+SET_POINT_Z_cm)
# action_y = int(pidY.compute_action(vertical_error))
# action_z = int(pidZ.compute_action(frontal_error))
action_x = cx.compute_action(horizontal_error)
# action_y = cy.compute_action(-vertical_error)
action_z = cz.compute_action(-frontal_error)
# roll in degree drone.get_yaw # yaw in degree drone.get_speed_x() drone.get_speed_y() drone.get_speed_z() # get speed in int drone.get_acceleration_x() drone.get_acceleration_y() drone.get_acceleration_z() # get acceleration in int drone.get_height() # get height in cm drone.get_distance_tof() # current distance from tof in cm drone.get_barometer() # barometer measurement in cm drone.get_flight_time() # flight time in s drone.get_battery() # get battery percentage ##############################
