def start(self):
if not self.vehicle.armed:
print "Arming..."
self.vehicle.armed = True
self.vehicle.flush()
time.sleep(2)
print "Changing to GUIDED mode"
self.vehicle.mode = VehicleMode("GUIDED")
self.vehicle.flush()
time.sleep(2)
TAKEOFF_HEIGHT = refLLH[2] - self.altitudeOffset
print "Taking off to %s meters" % TAKEOFF_HEIGHT
self.vehicle.commands.takeoff(TAKEOFF_HEIGHT)
self.vehicle.flush()
while self.vehicle.location.alt < TAKEOFF_HEIGHT - 1:
time.sleep(1)
print "Changing to GUIDED mode"
self.vehicle.mode = VehicleMode("GUIDED")
self.vehicle.flush()
time.sleep(2)
def start(drone):
if not drone.vehicle.armed:
print "Arming..."
drone.vehicle.armed = True
drone.vehicle.flush()
time.sleep(2)
print "Changing to GUIDED mode"
drone.vehicle.mode = VehicleMode("GUIDED")
drone.vehicle.flush()
time.sleep(2)
TAKEOFF_HEIGHT = drone.refLLH[2] - drone.altitudeOffset
print "Taking off to %s meters" % TAKEOFF_HEIGHT
drone.vehicle.commands.takeoff(TAKEOFF_HEIGHT)
drone.vehicle.flush()
while drone.vehicle.location.alt < TAKEOFF_HEIGHT - 1:
time.sleep(1)
print "Changing to GUIDED mode"
drone.vehicle.mode = VehicleMode("GUIDED")
drone.vehicle.flush()
time.sleep(2)
_stateTransition(drone, 'flyToStart')
def complete(self):
# debug
if self.debug:
print "Complete!"
# stop the vehicle and give up control
if self.controlling_vehicle:
self.guided_target_vel = (0, 0, 0)
self.send_nav_velocity(self.guided_target_vel[0],
self.guided_target_vel[1],
self.guided_target_vel[2])
self.guided_last_update = time.time()
# if in GUIDED mode switch back to LOITER
if self.vehicle.mode.name == "GUIDED":
self.vehicle.mode = VehicleMode("LOITER")
self.vehicle.flush()
# if in AUTO move to next command
if self.vehicle.mode.name == "AUTO":
self.advance_current_cmd()
# flag we are not in control of the vehicle
self.controlling_vehicle = False
return
def arm_and_takeoff(aTargetAltitude):
"""
Arms vehicle and fly to aTargetAltitude.
"""
print "Basic pre-arm checks"
# Don't let the user try to fly autopilot is booting
if vehicle.mode.name == "INITIALISING":
print "Waiting for vehicle to initialise"
time.sleep(1)
while vehicle.gps_0.fix_type < 2:
print "Waiting for GPS...:", vehicle.gps_0.fix_type
time.sleep(1)
print "Arming motors"
# Copter should arm in GUIDED mode
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
vehicle.flush()
while not vehicle.armed:
print " Waiting for arming..."
time.sleep(1)
print "Taking off!"
#vehicle.commands.takeoff(aTargetAltitude) # Take off to target altitude
vehicle.simple_takeoff(aTargetAltitude)
vehicle.flush()
def set_mode(self, mode):
self._log('Command MODE {} received'.format(mode))
modes = {'GUIDED', 'LOITER', 'LAND', 'AUTO'}
if mode not in modes:
raise ValueError('Mode {} not supported'.format(mode))
self.vehicle.mode = VehicleMode(mode)
self.vehicle.flush()
def Mission():
# Change MODE --> AUTO
v.mode = VehicleMode("STABILIZE")
#ARME HEXA
print "Arming..."
v.armed = True
v.flush()
# Change MODE --> AUTO
v.mode = VehicleMode("AUTO")
while not v.armed and not api.exit:
print "Waiting for arming..."
time.sleep(1)
def set_mode(self, mode):
if self.controlling_vehicle():
self.vehicle.mode = VehicleMode(mode)
self.vehicle.flush()
return True
return False
def set_mode(self, mode): """ set the mode of the vehicle mode: the name of the mode """ self.uav.mode = VehicleMode(mode) self.uav.flush()
def arm_and_takeoff(aTargetAltitude):
"""
Arms vehicle and fly to aTargetAltitude.
"""
print "Basic pre-arm checks"
# Don't let the user try to fly autopilot is booting
if vehicle.mode.name == "INITIALISING":
print "Waiting for vehicle to initialise"
time.sleep(1)
while vehicle.gps_0.fix_type < 2:
print "Waiting for GPS...:", vehicle.gps_0.fix_type
time.sleep(1)
print "Arming motors"
# Copter should arm in GUIDED mode
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
vehicle.flush()
while not vehicle.armed and not api.exit:
print " Waiting for arming..."
time.sleep(1)
print "Taking off!"
vehicle.commands.takeoff(aTargetAltitude) # Take off to target altitude
vehicle.flush()
while not api.exit:
print " Altitude: ", vehicle.location.alt
if vehicle.location.alt >= aTargetAltitude * 0.95: #Just below target, in case of undershoot.
print "Reached target altitude"
break
time.sleep(1)
def land_here(self):
"""
make the uav land at the current location
USE WITH CAUTION
"""
self.uav.mode = VehicleMode("LAND")
self.uav.flush()
def arm_and_takeoff(aTargetAltitude):
print "Basic pre-arm checks"
if vehicle.mode.name == "INITIALISING":
print "Waiting for vehicle to initialise"
time.sleep(1)
while vehicle.gps_0.fix_type < 2:
print "Waiting for GPS........", vehicle.gps_0.fix_type
time.sleep(1)
print "Arming motors"
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
vehicle.flush()
while not vehicle.armed and not api.exit:
print "Waiting for arming..."
time.sleep(1)
print "Take off!"
vehicle.commands.takeoff(aTargetAltitude)
vehicle.flush()
#Wait until the vehicle reaches a safe height
while not api.exit:
print "Altitude: ", vehicle.location.alt
if vehicle.location.alt >= aTargetAltitude * 0.95:
print "Reached target altitude"
break
time.sleep(1)
def testVel(self, vehicle):
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
time.sleep(1)
channel_override = {"3": 2000}
time.sleep(3)
channel_override = {"3": 1500}
return
def SetMode(mode): global v """ Sets a new mode for the vehicle (e.g. MANUAL, AUTO, RTL). Function returns nothing.""" v.mode = VehicleMode(mode) # Assumed that v.flush is run SetParam() #v.flush() pass
def takeoff(self, vehicle):
vehicle.mode = VehicleMode("LOITER")
time.sleep(2)
vehicle.armed = True
print "overriding"
vehicle.channel_override = {'3': 2000}
print "done"
time.sleep(2)
vehicle.channel_override = {"3": 1500}
return
def goto(lat, lon, alt=30):
print 'GOING TO', (lat, lon)
for i in range(0, 5):
time.sleep(.1)
# Set mode to guided - this is optional as the goto method will change the mode if needed.
vehicle.mode = VehicleMode("GUIDED")
# Set the target location and then call flush()
a_location = Location(lat, lon, alt, is_relative=True)
vehicle.commands.goto(a_location)
vehicle.flush()
def arm_and_takeoff(aTargetAltitude):
"""
Arms vehicle and fly to aTargetAltitude.
"""
# print "Basic pre-arm checks"
# Don't let the user try to fly autopilot is booting
if vehicle.mode.name == "INITIALISING":
# print "Waiting for vehicle to initialise"
time.sleep(1)
while vehicle.gps_0.fix_type < 2:
# print "Waiting for GPS...:", vehicle.gps_0.fix_type
time.sleep(1)
# print "Arming motors"
# Copter should arm in GUIDED mode
vehicle.mode = VehicleMode("GUIDED")
vehicle.flush()
i = 60
while not api.exit and vehicle.mode.name != 'GUIDED' and i > 0:
# print " Waiting for guided %s seconds..." % (i,)
time.sleep(1)
i = i - 1
vehicle.armed = True
vehicle.flush()
i = 60
while not api.exit and not vehicle.armed and vehicle.mode.name == 'GUIDED' and i > 0:
# print " Waiting for arming %s seconds..." % (i,)
time.sleep(1)
i = i - 1
# Failure will result in arming but immediately landing
assert vehicle.armed
assert_equals(vehicle.mode.name, 'GUIDED')
# print "Taking off!"
vehicle.commands.takeoff(
aTargetAltitude) # Take off to target altitude
vehicle.flush()
# Wait until the vehicle reaches a safe height before
# processing the goto (otherwise the command after
# Vehicle.commands.takeoff will execute immediately).
while not api.exit:
# print " Altitude: ", vehicle.location.alt
# Test for altitude just below target, in case of undershoot.
if vehicle.location.alt >= aTargetAltitude * 0.95:
# print "Reached target altitude"
break
assert_equals(vehicle.mode.name, 'GUIDED')
time.sleep(1)
def arm_and_takeoff():
global api
global vehicle
"""Dangerous: Arm and takeoff vehicle - use only in simulation"""
# NEVER DO THIS WITH A REAL VEHICLE - it is turning off all flight safety checks
# but fine for experimenting in the simulator.
print "Waiting for GPS..."
while vehicle.gps_0.fix_type < 3:
# gps_0.fix_type:
# 0-1: no fix
# 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK
# check https://pixhawk.ethz.ch/mavlink/#GPS_RAW_INT
time.sleep(1)
print "Waiting for location..."
while vehicle.location.alt == 0.0:
time.sleep(1)
print "Arming..."
vehicle.mode = VehicleMode("STABILIZE")
#Securite non inibe
#vehicle.parameters["ARMING_CHECK"] = 0
vehicle.armed = True
vehicle.flush()
print "Waiting for arming cycle completes..."
while not vehicle.armed and not api.exit:
time.sleep(1)
print("Setting AUTO mode...")
vehicle.mode = VehicleMode("AUTO")
vehicle.flush()
time.sleep(1)
print "Taking off!"
#Takeoff mode auto
#vehicle.commands.takeoff(5) # Take off to 5m height
vehicle.flush()
time.sleep(10)
def test_110(local_connect):
api = local_connect()
v = api.get_vehicles()[0]
# Change the vehicle into STABILIZE mode
v.mode = VehicleMode("STABILIZE")
# NOTE wait crudely for ACK on mode update
time.sleep(3)
# Define example callback for mode
def armed_callback(attribute):
armed_callback.called += 1
armed_callback.called = 0
# When the same (event, callback) pair is passed to add_attribute_observer,
# only one instance of the observer callback should be added.
v.add_attribute_observer('armed', armed_callback)
v.add_attribute_observer('armed', armed_callback)
v.add_attribute_observer('armed', armed_callback)
v.add_attribute_observer('armed', armed_callback)
v.add_attribute_observer('armed', armed_callback)
# Disarm and see update.
v.armed = False
v.flush()
# Wait for ACK.
time.sleep(3)
# Ensure the callback was called.
assert armed_callback.called > 0, "Callback should have been called."
# Rmove all observers. The first call should remove all listeners
# we've added; the second call should be ignored and not throw.
# NOTE: We test if armed_callback were treating adding each additional callback
# and remove_attribute_observer were removing them one at a time; in this
# case, there would be three callbacks still attached.
v.remove_attribute_observer('armed', armed_callback)
v.remove_attribute_observer('armed', armed_callback)
callcount = armed_callback.called
# Re-arm and see update.
v.armed = True
v.flush()
# Wait for ack
time.sleep(3)
# Ensure the callback was called zero times.
assert_equals(armed_callback.called, callcount,
"Callback should not have been called once removed.")
def test(self):
api = self.api
v = api.get_vehicles()[0]
print "Mode: %s" % v.mode
print "Location: %s" % v.location
print "Attitude: %s" % v.attitude
print "GPS: %s" % v.gps_0
print "Param: %s" % v.parameters['THR_MAX']
cmds = v.commands
cmds.download()
cmds.wait_valid()
print "Home WP: %s" % cmds[0]
v.mode = VehicleMode("AUTO")
v.flush()
def land_mode():
print("Setting LAND mode...")
drone.mode = VehicleMode("LAND")
time.sleep(30) # LANDING TIME DON'T TOUCH THIS
drone.flush()
print " Waiting for landing..."
while not drone.mode.name == 'LAND' and not api.exit:
print " Waiting for landing..."
print " Altitude: ", drone.location.alt
if drone.location.alt <= 0.3: #
print "Landed at: %s" % (drone.location.alt)
break
time.sleep(1)
def __init__(self, vehicle, shotmgr):
# assign the vehicle object
self.vehicle = vehicle
# assign the shotManager object
self.shotmgr = shotmgr
# ticks to track timing in shot
self.ticks = 0
# steps to track incrementing in shot
self.step = 0
# Default panoMode to None
self.panoMode = PANO_NONE
# Yaw rate for Video Pano shot
self.degSecondYaw = 10
# default FOV for Linear Pano shot
self.fov = 180
# index that tracks angles in Linear Pano shot
self.linearIndex = 0
# list of angles in Linear Pano shot
self.linearAngles = []
# default camYaw to current pointing
self.camYaw = camera.getYaw(self.vehicle)
# default camPitch to PITCH_1
self.camPitch = camera.getPitch(self.vehicle)
# default origYaw to current pointing
self.origYaw = self.camYaw
# default to not paused
self.paused = False
# set button mappings on Artoo
self.setButtonMappings()
# switch vehicle into GUIDED mode
self.vehicle.mode = VehicleMode("GUIDED")
# switch gimbal into MAVLINK TARGETING mode
self.setupTargeting()
def takeoff(self, alt = DEFAULT_ALTITUDE):
"""
Arms self.vehicle and fly to aTargetAltitude.
"""
print "Basic pre-arm checks"
# Don't let the user try to fly autopilot is booting
if self.vehicle.mode.name == "INITIALISING":
print "Waiting for self.vehicle to initialise"
time.sleep(1)
while self.vehicle.gps_0.fix_type < 2:
print "Waiting for GPS...:", self.vehicle.gps_0.fix_type
time.sleep(1)
print "Arming motors"
# Copter should arm in GUIDED mode
self.vehicle.mode = VehicleMode("GUIDED")
self.vehicle.armed = True
self.vehicle.flush()
self.socketIO.emit('notificationFromPi', 'Arming...')
while not self.vehicle.armed and not self.api.exit:
print " Waiting for arming..."
time.sleep(1)
print "Taking off!"
self.vehicle.commands.takeoff(alt) # Take off to target altitude
self.vehicle.flush()
self.inFlight = True
# Wait until the self.vehicle reaches a safe height before processing the goto (otherwise the command
# after self.vehicle.commands.takeoff will execute immediately).
while not self.api.exit:
print " Altitude: ", self.vehicle.location.alt
notification = "Altitude: " + str(self.vehicle.location.alt) + ' m'
self.socketIO.emit('notificationFromPi', notification)
self.socketIO.emit('trackFromPi',json.dumps({'lat': self.vehicle.location.lat,'lng': self.vehicle.location.lon, 'alt': self.vehicle.location.alt, 'yaw': self.vehicle.attitude.yaw }))
if self.vehicle.location.alt>=alt*0.95: #Just below target, in case of undershoot.
notification = "Reached target altitude of " + str(alt) + ' m'
self.socketIO.emit('notificationFromPi', notification)
self.stableFlight = True
break;
time.sleep(1)
def goto_balloon(self):
dest = self.balloon_loc
if dest is None:
print "No balloon found"
mode = self.vehicle.mode.name
if mode == "AUTO" and not self.debug:
VehicleMode("GUIDED")
elif mode != "GUIDED":
print "Not driving to %s" % dest
return
print "Going to: %s" % dest
# A better implementation would only send new waypoints if the position had changed significantly
vehicle.commands.goto(dest)
vehicle.flush()
def mission():
global QueueVideo
init_api()
#Takeoff
arm_and_takeoff()
#Attente de detection objet
boolVideoDet = False
while not boolVideoDet:
while not QueueVideo.empty():
boolVideoDet = (QueueVideo.get()[0] == 'DETECT')
print "Target detected"
#INIT TRACKING
vehicle.mode = VehicleMode("GUIDED")
#BEGIN TRACKING
while 1:
tracking()
time.sleep(0.1)
def condition_yaw(heading, relative=False):
for i in range(0, 5):
time.sleep(0.1)
vehicle.mode = VehicleMode("GUIDED")
# create the CONDITION_YAW command using command_long_encode()
msg = vehicle.message_factory.command_long_encode(
0,
0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
0, #confirmation
heading, # param 1, yaw in degrees
0, # param 2, yaw speed deg/s
1, # param 3, direction -1 ccw, 1 cw
1
if relative else 0, # param 4, relative offset 1, absolute angle 0
0,
0,
0) # param 5 ~ 7 not used
# send command to vehicle
vehicle.send_mavlink(msg)
vehicle.flush()
def arm_and_takeoff(self, targetAltitude):
"""
Arms vehicle and fly to targetAltitude.
"""
print "Basic pre-arm checks"
# Don't let the user try to fly autopilot is booting
while self.vehicle.mode.name == "INITIALISING":
print "Waiting for vehicle to initialise..."
time.sleep(1)
while self.vehicle.gps_0.fix_type < 2:
print "Waiting for GPS...:", self.vehicle.gps_0.fix_type
time.sleep(1)
print "Arming motors"
# Copter should arm in GUIDED mode
self.vehicle.mode = VehicleMode("GUIDED")
self.vehicle.armed = True
self.vehicle.flush()
while not self.vehicle.armed and not self.api.exit:
print " Waiting for arming..."
time.sleep(1)
# Take off to target altitude
print "Taking off!"
self.vehicle.commands.takeoff(targetAltitude)
self.vehicle.flush()
# Wait until the vehicle reaches a safe height before processing the
# goto (otherwise the command after Vehicle.commands.takeoff will
# execute immediately).
while not self.api.exit:
# TODO: Check sensors here already?
print " Altitude: ", self.vehicle.location.alt
# Just below target, in case of undershoot.
if self.vehicle.location.alt >= targetAltitude * 0.95:
print "Reached target altitude"
break
time.sleep(1)
def test_115(local_connect):
api = local_connect()
v = api.get_vehicles()[0]
# Dummy callback
def mavlink_callback(*args):
mavlink_callback.count += 1
mavlink_callback.count = 0
# Set the callback.
v.set_mavlink_callback(mavlink_callback)
# Change the vehicle into STABILIZE mode
v.mode = VehicleMode("STABILIZE")
# NOTE wait crudely for ACK on mode update
time.sleep(3)
# Expect the callback to have been called
assert mavlink_callback.count > 0
# Unset the callback.
v.unset_mavlink_callback()
savecount = mavlink_callback.count
# Disarm. A callback of None should not throw errors
v.armed = False
v.flush()
# NOTE wait crudely for ACK on mode update
time.sleep(3)
# Expect the callback to have been called
assert_equals(savecount, mavlink_callback.count)
# Re-arm should not throw errors.
v.armed = True
v.flush()
# NOTE wait crudely for ACK on mode update
time.sleep(3)
def arm_and_takeoff(aTargetAltitude):
"""
Arms vehicle and fly to aTargetAltitude(meters).
"""
print "Basic pre-arm checks"
# Don't let the user try to fly autopilot is booting
if drone.mode.name == "INITIALISING":
print "Waiting for vehicle to initialise"
time.sleep(1)
while drone.gps_0.fix_type < 2:
print "Waiting for GPS...:", vehicle.gps_0.fix_type
time.sleep(1)
print "Arming motors"
# Copter should arm in GUIDED mode
drone.mode = VehicleMode("GUIDED")
drone.armed = True
drone.flush()
while not drone.armed and not api.exit:
print " Waiting for arming..."
time.sleep(1)
print "Taking off!"
drone.commands.takeoff(aTargetAltitude) # Take off to target altitude
drone.flush()
# Wait until the vehicle reaches a safe height before processing the goto (otherwise the command
# after Vehicle.commands.takeoff will execute immediately).
while not api.exit:
print " Altitude: ", drone.location.alt
# print " Atiltude que eu quero: ", aTargetAltitude
if drone.location.alt >= aTargetAltitude * 0.95: #Just below target, in case of undershoot.
print "Reached target altitude: %s" % (drone.location.alt)
break
time.sleep(1)
def take_off(aTargetAltitude):
print "Arming motors"
# Copter should arm in GUIDED mode
drone.mode = VehicleMode("GUIDED")
drone.armed = True
drone.flush()
while not drone.armed and not api.exit:
print " Waiting for arming..."
time.sleep(1)
print "Taking off!"
drone.commands.takeoff(aTargetAltitude) # Take off to target altitude
drone.flush()
# Wait until the vehicle reaches a safe height before processing the goto (otherwise the command
# after Vehicle.commands.takeoff will execute immediately).
while not api.exit:
print " Altitude: ", drone.location.alt
if drone.location.alt >= aTargetAltitude * 0.95: #Just below target, in case of undershoot.
print "Reached target altitude: %s" % (drone.location.alt)
break
time.sleep(1)
def arm_and_takeoff():
"""Dangerous: Arm and takeoff vehicle - use only in simulation"""
# NEVER DO THIS WITH A REAL VEHICLE - it is turning off all flight safety checks
# but fine for experimenting in the simulator.
print "Arming and taking off"
vehicle.mode = VehicleMode("STABILIZE")
vehicle.parameters["ARMING_CHECK"] = 0
vehicle.armed = True
vehicle.flush()
while not vehicle.armed and not api.exit:
print "Waiting for arming..."
time.sleep(1)
print "Taking off!"
vehicle.commands.takeoff(20) # Take off to 20m height
# Pretend we have a RC transmitter connected
rc_channels = vehicle.channel_override
rc_channels[3] = 1500 # throttle
vehicle.channel_override = rc_channels
vehicle.flush()
time.sleep(10)