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)
