from dji_asdk_to_python.products.aircraft import Aircraft from dji_asdk_to_python.flight_controller.flight_controller_state import ( FlightControllerState, ) APP_IP = "192.168.0.110" drone = Aircraft(APP_IP) fc = drone.getFlightController() flight_controller_state = fc.getState() print("areMotorsOn %s " % flight_controller_state.areMotorsOn()) print("isFlying %s " % flight_controller_state.isFlying()) print("velocityX %s " % flight_controller_state.getVelocityX()) print("velocityY %s " % flight_controller_state.getVelocityY()) print("velocityZ %s " % flight_controller_state.getVelocityZ()) aircraft_location = flight_controller_state.getAircraftLocation() print("getAltitude %s " % aircraft_location.getAltitude()) print("getLatitude %s " % aircraft_location.getLatitude()) print("getLongitude %s " % aircraft_location.getLongitude()) aircraft_attitude = flight_controller_state.getAttitude() print("pitch %s " % aircraft_attitude.pitch) print("roll %s " % aircraft_attitude.roll) print("yaw %s " % aircraft_attitude.yaw) print("GoHomeExecutionState %s" % flight_controller_state.getGoHomeExecutionState()) print("getFlightMode %s" % flight_controller_state.getFlightMode())
class ArucoLanding: """ Inits the Stage2 class Parameters: drone_ip (str) -> The IP of the drone camera_matrix (ndarray) -> The camera matrix of the drone's camera camera_distortion (ndarray) -> The camera distortion of the drone's camera marker_id (int) -> The ID of the aruco marker to be detected on the landing stage marker_size_cm (int) -> The size in CM of the aruco marker to be detected in the stage """ def __init__(self, drone_ip, camera_matrix, camera_distortion, marker_id, marker_size_cm): self.aircraft = Aircraft(drone_ip) self.marker_id = marker_id self.marker_size_cm = marker_size_cm self.ast = ArucoSingleTracker(camera_distortion=camera_distortion, camera_matrix=camera_matrix) self.rtp_manager = self.aircraft.getLiveStreamManager().getRTPManager() self.p = 0.004 self.i = 0.000005 self.d = 0.0005 self.pidx = PID(P=self.p, I=self.i, D=self.d) self.pidy = PID(P=self.p, I=self.i, D=self.d) self.pidz = PID(P=self.p, I=self.i, D=self.d) self.pidx.SetPoint = 0.0 self.pidy.SetPoint = 10.0 self.pidz.SetPoint = 100.0 self.pidx.setSampleTime(0.1) self.pidy.setSampleTime(0.1) self.pidz.setSampleTime(0.1) self.yaw_margin = 15 def start(self): self.rtp_manager.setWidth(1280) self.rtp_manager.setHeigth(720) self.rtp_manager.startStream() result = self.rtp_manager.startStream() print("result startStream %s" % result) if isinstance(result, CustomError): raise Exception("%s" % result) print( "--------------------------------------- STAGE 1 --------------------------------------------------------" ) gimbal = self.aircraft.getGimbal() gimbal.rotate(-90, 0, 0) print("Gimbal set to -90 degrees") fc = self.aircraft.getFlightController() fc.setVirtualStickModeEnabled(True) fcd = FlightControlData() while True: fcd.setPitch(0) fcd.setYaw(0) fcd.setRoll(0) fcd.setVerticalThrottle(0) flight_controller_state = fc.getState() if isinstance(flight_controller_state, FlightControllerState): flying = flight_controller_state.isFlying() if not flying: break frame = self.rtp_manager.getFrame() if frame is None: continue ( marker_found, x_marker, y_marker, z_marker, x_camera, y_camera, z_camera, roll_marker, yaw_marker, pitch_marker, roll_marker, roll_camera, yaw_camera, pitch_camera, ) = self.ast.track(frame, self.marker_id, self.marker_size_cm) if marker_found: print("x_marker %s y_marker %s z_marker %s" % (x_marker, y_marker, z_marker)) print("yaw_camera %s" % yaw_camera) if abs(yaw_camera) > self.yaw_margin: print("CORRECTING YAW") if yaw_camera < 0: fcd.setYaw(10) else: fcd.setYaw(-10) self.pidx.update(x_marker) self.pidy.update(y_marker) self.pidz.update(z_marker) xoutput = self.pidx.output youtput = self.pidy.output zoutput = self.pidz.output print("X output:%s" % xoutput) print("Y output:%s" % youtput) print("Z output:%s" % zoutput) fcd.setPitch(youtput) fcd.setRoll(xoutput * -1) if z_marker > 100 and abs(yaw_camera) < 20: print("DESCENDING") if abs(zoutput) > 4: fcd.setVerticalThrottle(-2) else: fcd.setVerticalThrottle(zoutput) else: fcd.setVerticalThrottle(0) if z_marker < 120 and abs(yaw_camera) < 15 and abs( x_camera) < 8 and abs(y_camera - 10) < 8: fc.startLanding() print("LANDING") self.rtp_manager.stopStream() print("pitch %s, roll %s, yaw %s, throttle %s" % (fcd.getPitch(), fcd.getRoll(), fcd.getYaw(), fcd.getVerticalThrottle())) fc.sendVirtualStickFlightControlData(fcd) fcd.setPitch(0) fcd.setYaw(0) fcd.setRoll(0) fcd.setVerticalThrottle(0) cv2.destroyAllWindows() def stop_streaming(self): self.rtp_manager.stopStream()
# Setting global variables APP_IP = "192.168.20.37" FREQ_SEND = 5 # hz REQUEST_DELAY = 3 # sec MIN_ELAPSED_TIME = 300 # seconds IS_FLYING = False # Initializing aircraft object to handle drone aircraft = Aircraft(APP_IP) print(" ############################## Starting Telemetry process in DJI") time_0 = time.time() flight_controller = aircraft.getFlightController() start = time_0 elapsed = 0 flying = IS_FLYING count = 0 # Loop to do actions while time be lesser than min allowed or drone is flying} #i = 0 print("############################## Starting Telemetry loop before flying") import datetime while elapsed < MIN_ELAPSED_TIME or flying: #while i != 2: #i += 1 # Updating time variables if time.time() >= time_0 + 1.0 / FREQ_SEND: now = datetime.datetime.now()
from dji_asdk_to_python.products.aircraft import Aircraft APP_IP = "192.168.100.206" aircraft = Aircraft(APP_IP) fc = aircraft.getFlightController() res = fc.setCollisionAvoidanceEnabled(True) print(res)
class ArucoLanding: """ Inits ArucoLanding class Parameters: drone_ip (str) -> The IP of the drone camera_matrix (ndarray) -> The camera matrix of the drone's camera camera_distortion (ndarray) -> The camera distortion of the drone's camera marker_id (int) -> The ID of the aruco marker to be detected on the landing stage marker_size_cm (int) -> The size in CM of the aruco marker to be detected in the stage """ def __init__(self, drone_ip, camera_matrix, camera_distortion, marker_id, marker_size_cm): self.aircraft = Aircraft(drone_ip) self.marker_id = marker_id self.marker_size_cm = marker_size_cm self.ast = ArucoSingleTracker(camera_distortion=camera_distortion, camera_matrix=camera_matrix) self.rtp_manager = self.aircraft.getLiveStreamManager().getRTPManager() self.p = 0.004 self.i = 0.000005 self.d = 0.0005 self.pidx = PID(P=self.p, I=self.i, D=self.d) self.pidy = PID(P=self.p, I=self.i, D=self.d) self.pidz = PID(P=self.p, I=self.i, D=self.d) self.pidx.SetPoint = 0.0 self.pidy.SetPoint = 20.0 self.pidz.SetPoint = 0.0 self.pidx.setSampleTime(0.03) self.pidy.setSampleTime(0.03) self.pidz.setSampleTime(0.03) self.yaw_margin = 15 def start(self): self.rtp_manager.setWidth(1280) self.rtp_manager.setHeigth(720) self.rtp_manager.startStream() result = self.rtp_manager.startStream() print("result startStream %s" % result) if isinstance(result, CustomError): raise Exception("%s" % result) gimbal = self.aircraft.getGimbal() gimbal.rotate(-90, 0, 0) print("Gimbal set to -90 degrees") # send_set_iso(self.pk_drone, "ISO_100", "SHUTTER_SPEED_1_8000") fc = self.aircraft.getFlightController() fc.setVirtualStickModeEnabled(True) fcd = FlightControlData() # get_down = False while True: fcd.setPitch(0) fcd.setYaw(0) fcd.setRoll(0) fcd.setVerticalThrottle(0) frame = self.rtp_manager.getFrame() if isinstance(frame, None): continue frame = cv2.resize(frame, (1280, 720), interpolation=cv2.INTER_AREA) ( marker_found, x_marker, y_marker, z_marker, x_camera, y_camera, z_camera, roll_marker, yaw_marker, pitch_marker, roll_marker, roll_camera, yaw_camera, pitch_camera, ) = self.ast.track(frame, self.marker_id, self.marker_size_cm) if marker_found: print("x_marker %s y_marker %s z_marker %s" % (x_marker, y_marker, z_marker)) print("yaw_camera %s" % yaw_camera) if abs(yaw_camera) > self.yaw_margin: print("CORRECTING YAW") if yaw_camera < 0: fcd.setYaw(5) else: fcd.setYaw(-5) self.pidx.update(x_marker) self.pidy.update(y_marker) xoutput = self.pidx.output youtput = self.pidy.output print("X output:%s" % xoutput) print("Y output:%s" % youtput) fcd.setPitch(youtput) fcd.setRoll(xoutput * -1) if z_marker > 180 and abs(yaw_camera) < 20: self.pidz.update(z_marker) zoutput = self.pidz.output fcd.setVerticalThrottle(-abs(zoutput) / 5) print("Z output:%s" % zoutput) if z_marker < 200 and abs(yaw_camera) < 30 and math.sqrt( math.pow(x_marker, 2) + math.pow(y_marker, 2)) < 15: fc.startLanding() print("LANDING") time.sleep(10) # esperar a que haga landing flight_controller_state = fc.getState() if not isinstance(flight_controller_state, FlightControllerState): continue flying = flight_controller_state.isFlying() if flying is not None and flying: fc.move_distance(pitch_distance=0.1, roll_distance=0, throttle_distance=2, meters_per_second=0.3, order=["THROTTLE", "PITCH", "ROLL"]) # send_move_distance(self.pk_drone, 0, 0, 200, 70, "PITCH") gimbal.rotate(-90, 0, 0) fc.setVirtualStickModeEnabled(True) else: # send_set_iso_auto(self.pk_drone) # send_set_iso(self.pk_drone,"ISO_200","SHUTTER_SPEED_1_320") break fc.sendVirtualStickFlightControlData(fcd) fcd.setPitch(0) fcd.setYaw(0) fcd.setRoll(0) fcd.setVerticalThrottle(0)