def takeoff(z=1.5,
safe_takeoff=False,
frame_id='map',
timeout=5.0,
use_leds=True,
interrupter=interrupt_event):
if use_leds:
led.set_effect(effect='wipe', r=255, g=0, b=0)
result = flight.takeoff(height=z,
timeout_takeoff=timeout,
frame_id=frame_id,
emergency_land=safe_takeoff,
interrupter=interrupter)
if result == 'not armed':
raise Exception(
'STOP'
) # Raise exception to clear task_manager if copter can't arm
if use_leds:
led.set_effect(effect='blink_fast', r=0, g=255, b=0)
def execute_frame(frame, config, interrupter=interrupt_event):
auto_arm = False
use_leds = config.led_use
frame_id = config.flight_frame_id
if frame.action == 'takeoff':
use_leds = use_leds & config.led_takeoff_indication
takeoff(z=config.flight_takeoff_height,
frame_id=frame_id,
timeout=config.flight_takeoff_time,
use_leds=use_leds,
interrupter=interrupter)
return
if frame.action == 'land':
use_leds = use_leds & config.led_land_indication
land(frame_id=frame_id,
timeout=config.flight_land_timeout,
use_leds=use_leds,
interrupter=interrupter)
return
if frame.action in ('fly', 'arm'):
if frame.action == 'arm':
auto_arm = True
if frame.pose_is_valid():
flight.navto(x=frame.x,
y=frame.y,
z=frame.z,
yaw=frame.yaw,
frame_id=frame_id,
auto_arm=auto_arm,
interrupter=interrupter)
else:
logger.error("Frame pose is not valid for flying")
if use_leds:
try:
red, green, blue = frame.get_color()
except ValueError:
logger.error("Can't get frame color!")
else:
led.set_effect(r=red, g=green, b=blue)
def land(z=1.5,
descend=False,
timeout=5.0,
frame_id='map',
use_leds=True,
interrupter=interrupt_event):
led.set_effect(r=0, g=0, b=0)
if use_leds:
led.set_effect(effect='blink_fast', r=255, g=0, b=0)
flight.land(z=z,
descend=descend,
timeout_land=timeout,
frame_id_land=frame_id,
interrupter=interrupter)
if use_leds:
while (flight.get_telemetry_locked().armed):
if interrupter.is_set():
break
time.sleep(0.5)
led.set_effect(r=0, g=0, b=0)
def turn_off_led(interrupter=interrupt_event):
led.set_effect(r=0, g=0, b=0)
def _command_led_fill(*args, **kwargs):
red = kwargs.get("red", 0)
green = kwargs.get("green", 0)
blue = kwargs.get("blue", 0)
led.set_effect(r=red, g=green, b=blue)
def _command_led_test(*args, **kwargs):
led.set_effect(effect='flash', r=255, g=255, b=255)