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 ##############################