def enterFailsafe(self):
''' called when we loose RC link or have Batt FS event '''
# dont enter failsafe on the ground
if not self.vehicle.armed or self.vehicle.system_status != 'ACTIVE':
return
# dont enter failsafe if we are already rewinding home
if self.currentShot == shots.APP_SHOT_REWIND:
self.curController.exitToRTL = True
return
if self.currentShot == shots.APP_SHOT_RTL:
return
if self.last_ekf_ok is False:
# no GPS - force an emergency land
self.vehicle.mode = VehicleMode("LAND")
return
# ignore FS while in Auto mode
if self.vehicle.mode.name == 'AUTO' and self.rewindManager.fs_thr == 2:
return
if self.rewindManager.enabled:
#self.enterShot(shots.APP_SHOT_REWIND) #Disabled by Matt
#self.curController.exitToRTL = True
self.vehicle.mode = VehicleMode("RTL")
else:
self.vehicle.mode = VehicleMode("RTL")
def armed_callback(self, vehicle, name, armed):
try:
if armed != self.last_armed:
self.last_armed = armed
logger.log("[callback]: armed status changed to %d" % (armed))
self.buttonManager.setButtonMappings()
# clear Rewind manager cache
self.rewindManager.resetSpline()
if not armed and self.currentShot not in shots.CAN_START_BEFORE_ARMING:
self.enterShot(shots.APP_SHOT_NONE)
self.vehicle.mode = VehicleMode("LOITER")
# stop recording upon disarm (landing, hopefully)
if not armed:
self.goproManager.handleRecordCommand(
self.goproManager.captureMode, RECORD_COMMAND_STOP)
# read home loc from vehicle
if armed:
self.rewindManager.loadHomeLocation()
except Exception as e:
logger.log('[callback]: armed callback error, %s' % e)
def Start(self, vehicle):
logger.log("+-+-+-+-+-+-+ Starting up %s +-+-+-+-+-+-+" % VERSION)
### initialize dronekit vehicle ###
self.vehicle = vehicle
### switch vehicle to loiter mode ###
self.vehicle.mode = VehicleMode("LOITER")
### initialize rc manager ###
self.rcMgr = rcManager.rcManager(self)
### initialize app manager ###
self.appMgr = appManager.appManager(self)
### initialize button manager ###
self.buttonManager = buttonManager.buttonManager(self)
### initialize extended functions ###
self.extFunctions = extFunctions.extFunctions(self.vehicle, self)
### initialize gopro manager ###
self.goproManager = GoProManager.GoProManager(self)
### Initialize GeoFence manager ###
self.geoFenceManager = GeoFenceManager.GeoFenceManager(self)
# instantiate rewindManager
self.rewindManager = rewindManager.RewindManager(self.vehicle, self)
### init APM stream rates ###
self.initStreamRates()
### register callbacks ###
self.registerCallbacks()
# Try to maintain a constant tick rate
self.timeOfLastTick = monotonic.monotonic()
# how many ticks have we performed since an RC update?
# register all connections (gopro manager communicates via appMgr's socket)
self.inputs = [self.rcMgr.server, self.appMgr.server]
self.outputs = []
#check if gimbal is present
if self.vehicle.gimbal.yaw is not None:
logger.log("[shot]: Gimbal detected.")
# Initialize gimbal to RC targeting
self.vehicle.gimbal.release()
else:
logger.log("[shot]: No gimbal detected.")
# mark first tick time
self.timeOfLastTick = monotonic.monotonic()
# check for In-Air start from Shotmanager crash
if self.vehicle.system_status == 'ACTIVE':
logger.log("[shot]: Restart in air.")
# load vehicle home
self.rewindManager.loadHomeLocation()
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
while not vehicle.armed:
print " Waiting for arming..."
time.sleep(1)
print "Taking off!"
vehicle.commands.takeoff(aTargetAltitude) # Take off to target altitude
# Wait until the vehicle reaches a safe height before processing the goto (otherwise the command
# after Vehicle.commands.takeoff will execute immediately).
while True:
print " Altitude: ", vehicle.location.global_frame.alt
if vehicle.location.global_frame.alt >= aTargetAltitude * 0.95: #Trigger just below target alt.
print "Reached target altitude"
break
time.sleep(1)
def test_110(connpath):
v = connect(connpath, await_params=True)
# NOTE these are *very inappropriate settings*
# to make on a real vehicle. They are leveraged
# exclusively for simulation. Take heed!!!
v.parameters['ARMING_CHECK'] = 0
v.parameters['FS_THR_ENABLE'] = 0
v.parameters['FS_GCS_ENABLE'] = 0
v.parameters['EKF_CHECK_THRESH'] = 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 checkForObstacle(self):
# check if beam will hit the ground at altitudes lower than 3 meters then disable scan
self.pitch_angle = abs(self.vehicle.attitude.pitch)
self.check_altitude = self.vehicle.location.global_relative_frame.alt
#logger.log("pitch: %s" % self.pitch_angle)
if (self.vehicle.attitude.pitch < 0 and self.check_altitude < 3):
self.beamreach = (self.check_altitude / math.sin(self.pitch_angle))
else:
self.beamreach = 1000
if (self.beamreach > SCAN_BEAM_DISTANCE):
if (self.led_beam_angle_state == 1):
self.led_beam_angle_state = 0
if (self.arduinoBoard.digital_read(COLL_CENTER) == 1):
if (self.led_center_state == 0):
logger.log("[objavoid]: Obstacle in center")
# when we are not in a shot, nor in RTL or LAND and flying higher than 1 meter
if (self.currentShot == shots.APP_SHOT_NONE
and (self.vehicle.mode.name != 'LAND'
or self.vehicle.mode.name != 'RTL')):
if (self.check_altitude > 1):
self.vehicle.mode = VehicleMode("BRAKE")
else:
logger.log(
"[objavoid]: Must be 1M above the ground")
self.led_center_state = 1
else:
logger.log("[objavoid]: no obstacle in sight reset LED")
self.LEDrgb(2, 2, 0, 255, 0)
self.LEDrgb(3, 2, 255, 0, 0)
#self.led_right_state = 0
#self.led_left_state = 0
self.led_center_state = 0
#self.led_beam_angle_state = 0
else:
if (self.led_beam_angle_state == 0):
logger.log(
"[objavoid]: pitch/altitude too low - obstacle detection disabled"
)
self.LEDrgb(2, 4, 255, 100, 0)
self.LEDrgb(3, 4, 255, 100, 0)
self.led_beam_angle_state = 1
self.led_right_state = 0
self.led_left_state = 0
self.led_center_state = 0
# send status to app
if self.appMgr.isAppConnected():
exceptStr = "Solo altitude too low for scan"
packet = struct.pack(
'<II%ds' % (len(exceptStr)),
app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR,
len(exceptStr), exceptStr)
self.appMgr.client.send(packet)
# sleep to make sure the packet goes out
time.sleep(0.1)
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 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 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 True:
# 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 start_vehicle(self, altitude):
if self.vehicle.mode.name == "INITIALISING":
time.sleep(1)
while self.vehicle.gps_0.fix_type < 2:
time.sleep(1)
self.vehicle.armed = True
self.vehicle.flush()
while not self.vehicle.armed:
time.sleep(1)
self.vehicle.mode = VehicleMode("GUIDED")
self.vehicle.commands.takeoff(altitude)
self.vehicle.flush()
def test_115(connpath):
v = connect(connpath, await_params=True)
# 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 Run(self):
while True:
try:
#print "in shotManager server loop"
# handle TCP/RC packets
# we set a timeout of UPDATE_TIME, so we roughly do this every UPDATE_TIME
rl, wl, el = select.select( self.inputs, self.outputs, [], UPDATE_TIME )
# handle reads
for s in rl:
if s is self.appMgr.server: # if read is a connection attempt
self.appMgr.connectClient()
elif s is self.appMgr.client: # if read is from app
self.appMgr.parse()
elif s is self.rcMgr.server: # if read is an RC packet
self.rcMgr.parse()
elif s is self.buttonManager.client: # if read is from buttons
self.buttonManager.parse()
# now handle writes (sololink.btn_msg handles all button writes)
for s in wl:
if s is self.appMgr.client: # if write is for app
self.appMgr.write()
# exceptions
for s in el:
if s is self.appMgr.client: # if its the app socket throwing an exception
self.appMgr.exception()
else:
# otherwise remove whichever socket is excepting
if s in self.inputs:
self.inputs.remove(s)
if s in self.outputs:
self.outputs.remove(s)
s.close()
self.buttonManager.checkButtonConnection()
# Check for obstacles when the lidar is active
if (self.buttonManager.scanactive == 1):
self.checkForObstacle()
# Check if copter is outside fence or will be
self.geoFenceManager.activateGeoFenceIfNecessary()
# call main control/planning loop at UPDATE_RATE
if time.time() - self.timeOfLastTick > UPDATE_TIME:
self.Tick()
except Exception as ex:
# reset any RC timeouts and stop any stick remapping
self.rcMgr.detach()
# try to put vehicle into LOITER
self.vehicle.mode = VehicleMode("LOITER")
exceptStr = traceback.format_exc()
print exceptStr
strlist = exceptStr.split('\n')
for i in strlist:
logger.log(i)
if self.appMgr.isAppConnected():
# send error to app
packet = struct.pack('<II%ds' % (len(exceptStr)), app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR, len(exceptStr), exceptStr)
self.appMgr.client.send(packet)
# sleep to make sure the packet goes out (for some reason
# setting client.setblocking(1) doesn't work)
time.sleep(0.4)
# cleanup
for socket in self.inputs:
socket.close()
os._exit(1)
def setMode(mode):
# Now change the vehicle into auto mode
vehicle.mode = VehicleMode(mode)
def change_mode(self, mode):
self._log("Mode: {0}".format(mode))
self.vehicle.mode = VehicleMode(mode)
def test_goto(connpath):
vehicle = connect(connpath, await_params=True)
# NOTE these are *very inappropriate settings*
# to make on a real vehicle. They are leveraged
# exclusively for simulation. Take heed!!!
vehicle.parameters['ARMING_CHECK'] = 0
vehicle.parameters['FS_THR_ENABLE'] = 0
vehicle.parameters['FS_GCS_ENABLE'] = 0
vehicle.parameters['EKF_CHECK_THRESH'] = 0
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 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 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 True:
# 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)
arm_and_takeoff(10)
# print "Going to first point..."
point1 = Location(-35.361354, 149.165218, 20, is_relative=True)
vehicle.commands.goto(point1)
vehicle.flush()
# sleep so we can see the change in map
time.sleep(3)
# print "Going to second point..."
point2 = Location(-35.363244, 149.168801, 20, is_relative=True)
vehicle.commands.goto(point2)
vehicle.flush()
# sleep so we can see the change in map
time.sleep(3)
# print "Returning to Launch"
vehicle.mode = VehicleMode("RTL")
vehicle.flush()
while True:
print " Altitude: ", vehicle.location.global_frame.alt
if vehicle.location.global_frame.alt >= aTargetAltitude * 0.95: #Trigger just below target alt.
print "Reached target altitude"
break
time.sleep(1)
arm_and_takeoff(20)
print "Going to first point..."
point1 = LocationGlobal(-35.361354, 149.165218, 20, is_relative=True)
vehicle.commands.goto(point1)
# sleep so we can see the change in map
time.sleep(30)
print "Going to second point..."
point2 = LocationGlobal(-35.363244, 149.168801, 20, is_relative=True)
vehicle.commands.goto(point2)
# sleep so we can see the change in map
time.sleep(30)
print "Returning to Launch"
vehicle.mode = VehicleMode("RTL")
#Close vehicle object before exiting script
print "Close vehicle object"
vehicle.close()
def recordLocation(self):
degreesYaw = camera.getYaw(self.vehicle)
pitch = camera.getPitch(self.vehicle)
# don't allow two waypoints near each other
if len(self.waypoints) == 1:
if location_helpers.getDistanceFromPoints3d(self.waypoints[0].loc, self.vehicle.location.global_relative_frame) < WAYPOINT_NEARNESS_THRESHOLD:
logger.log("[cable cam]: attempted to record a point too near the first point")
# update our waypoint in case yaw, pitch has changed
self.waypoints[0].yaw = degreesYaw
self.waypoints[0].pitch = pitch
# force the app to exit and restart cable cam
packet = struct.pack('<IIi', app_packet.SOLO_MESSAGE_GET_CURRENT_SHOT, 4, shots.APP_SHOT_NONE)
self.shotmgr.appMgr.sendPacket(packet)
packet = struct.pack('<IIi', app_packet.SOLO_MESSAGE_GET_CURRENT_SHOT, 4, shots.APP_SHOT_CABLECAM)
self.shotmgr.appMgr.sendPacket(packet)
packet = struct.pack('<IIddfff', app_packet.SOLO_CABLE_CAM_WAYPOINT,
28, self.vehicle.location.global_relative_frame.lat, self.vehicle.location.global_relative_frame.lon,
self.vehicle.location.global_relative_frame.alt, degreesYaw, pitch)
self.shotmgr.appMgr.sendPacket(packet)
return
# create a waypoint and add it to our list
waypt = Waypoint(self.vehicle.location.global_relative_frame, degreesYaw, pitch)
logger.log("[cable cam]: Recorded location %f, %f, %f. Yaw = %f" %
( self.vehicle.location.global_relative_frame.lat, self.vehicle.location.global_relative_frame.lon,
self.vehicle.location.global_relative_frame.alt, degreesYaw))
logger.log("[cable cam]: gimbal pitch is " + str(pitch))
self.waypoints.append(waypt)
self.setButtonMappings()
# send this waypoint to the app
packet = struct.pack('<IIddfff', app_packet.SOLO_CABLE_CAM_WAYPOINT,
28, self.vehicle.location.global_relative_frame.lat, self.vehicle.location.global_relative_frame.lon,
self.vehicle.location.global_relative_frame.alt, degreesYaw, pitch)
self.shotmgr.appMgr.sendPacket(packet)
#start monitoring heading changes
if len(self.waypoints) == 1:
self.aggregateYaw = 0.0
self.lastYaw = degreesYaw
elif len(self.waypoints) == 2:
# Now change the vehicle into guided mode
self.vehicle.mode = VehicleMode("GUIDED")
logger.log("[cable cam]: Got second cable point. Should be in guided %s" % (str(self.vehicle.mode)))
self.totalDistance = location_helpers.getDistanceFromPoints3d(
self.waypoints[0].loc, self.waypoints[1].loc)
# handle the 0-360 border
if self.waypoints[1].yaw - self.waypoints[0].yaw > 180.0:
self.waypoints[0].yaw += 360.0
elif self.waypoints[1].yaw - self.waypoints[0].yaw < -180.0:
self.waypoints[1].yaw += 360.0
#disregard aggregate yaw if it's less than 180
if abs(self.aggregateYaw) < 180.0:
self.aggregateYaw = self.waypoints[1].yaw - self.waypoints[0].yaw
self.yawDirection = 1 if self.aggregateYaw > 0.0 else 0
logger.log("[cable cam]: Aggregate yaw = %f. Yaw direction saved as %s" % (self.aggregateYaw, "CCW" if self.yawDirection == 1 else "CW"))
# send this yawDir to the app
self.updateAppOptions()
self.pathHandler = pathHandler.TwoPointPathHandler( self.waypoints[1].loc, self.waypoints[0].loc, self.vehicle, self.shotmgr )
def demo(local_connect):
# This example shows how to use DroneKit-Python to get and set vehicle state, parameter and channel-override information.
# It also demonstrates how to observe vehicle attribute (state) changes.
#
# Usage:
# * mavproxy.py
# * module load api
# * api start vehicle-state.py
#
from dronekit.lib import VehicleMode
from pymavlink import mavutil
import time
# First get an instance of the API endpoint
api = local_connect()
# Get the connected vehicle (currently only one vehicle can be returned).
v = api.get_vehicles()[0]
# Get all vehicle attributes (state)
print "\nGet all vehicle attribute values:"
print " Location: %s" % v.location
print " Attitude: %s" % v.attitude
print " Velocity: %s" % v.velocity
print " GPS: %s" % v.gps_0
print " Groundspeed: %s" % v.groundspeed
print " Airspeed: %s" % v.airspeed
print " Mount status: %s" % v.mount_status
print " Battery: %s" % v.battery
print " Mode: %s" % v.mode.name # settable
print " Armed: %s" % v.armed # settable
# Set vehicle mode and armed attributes (the only settable attributes)
print "Set Vehicle.mode=GUIDED (currently: %s)" % v.mode.name
v.mode = VehicleMode("GUIDED")
v.flush() # Flush to guarantee that previous writes to the vehicle have taken place
while not v.mode.name=='GUIDED' and not api.exit: #Wait until mode has changed
print " Waiting for mode change ..."
time.sleep(1)
print "Set Vehicle.armed=True (currently: %s)" % v.armed
v.armed = True
v.flush()
while not v.armed and not api.exit:
print " Waiting for arming..."
time.sleep(1)
# Show how to add and remove and attribute observer callbacks (using mode as example)
def mode_callback(attribute):
print " CALLBACK: Mode changed to: ", v.mode.name
print "\nAdd mode attribute observer for Vehicle.mode"
v.add_attribute_observer('mode', mode_callback)
print " Set mode=STABILIZE (currently: %s)" % v.mode.name
v.mode = VehicleMode("STABILIZE")
v.flush()
print " Wait 2s so callback invoked before observer removed"
time.sleep(2)
# Remove observer - specifying the attribute and previously registered callback function
v.remove_attribute_observer('mode', mode_callback)
# # Get Vehicle Home location ((0 index in Vehicle.commands)
# print "\nGet home location"
# cmds = v.commands
# cmds.download()
# cmds.wait_valid()
# print " Home WP: %s" % cmds[0]
# Get/Set Vehicle Parameters
print "\nRead vehicle param 'THR_MIN': %s" % v.parameters['THR_MIN']
print "Write vehicle param 'THR_MIN' : 10"
v.parameters['THR_MIN']=10
v.flush()
print "Read new value of param 'THR_MIN': %s" % v.parameters['THR_MIN']
# # Overriding an RC channel
# # NOTE: CHANNEL OVERRIDES may be useful for simulating user input and when implementing certain types of joystick control.
# #DO NOT use unless there is no other choice (there almost always is!)
# print "\nOverriding RC channels for roll and yaw"
# v.channel_override = { "1" : 900, "4" : 1000 }
# v.flush()
# print " Current overrides are:", v.channel_override
# print " Channel default values:", v.channel_readback # All channel values before override
# # Cancel override by setting channels to 0
# print " Cancelling override"
# v.channel_override = { "1" : 0, "4" : 0 }
# v.flush()
## Reset variables to sensible values.
print "\nReset vehicle atributes/parameters and exit"
v.mode = VehicleMode("STABILIZE")
v.armed = False
v.parameters['THR_MIN']=130
v.flush()
print " Velocity: %s" % vehicle.velocity
print " GPS: %s" % vehicle.gps_0
print " Groundspeed: %s" % vehicle.groundspeed
print " Airspeed: %s" % vehicle.airspeed
print " Mount status: %s" % vehicle.mount_status
print " Battery: %s" % vehicle.battery
print " Rangefinder: %s" % vehicle.rangefinder
print " Rangefinder distance: %s" % vehicle.rangefinder.distance
print " Rangefinder voltage: %s" % vehicle.rangefinder.voltage
print " Mode: %s" % vehicle.mode.name # settable
print " Armed: %s" % vehicle.armed # settable
# Set vehicle mode and armed attributes (the only settable attributes)
print "\nSet Vehicle.mode=GUIDED (currently: %s)" % vehicle.mode.name
vehicle.mode = VehicleMode("GUIDED")
while not vehicle.mode.name=='GUIDED': #Wait until mode has changed
print " Waiting for mode change ..."
time.sleep(1)
# Check we have a good gps fix (required to arm)
while vehicle.gps_0.fix_type < 2:
print "Waiting for GPS fix=3 (needed to arm):", vehicle.gps_0.fix_type
time.sleep(1)
print "\nSet Vehicle.armed=True (currently: %s)" % vehicle.armed
vehicle.armed = True
while not vehicle.armed:
print " Waiting for arming..."
def checkForObstacle(self):
# check if beam will hit the ground at altitudes lower than 3 meters then disable scan
self.pitch_angle = abs(self.vehicle.attitude.pitch)
self.check_altitude = self.vehicle.location.global_relative_frame.alt
#logger.log("pitch: %s" % self.pitch_angle)
if (self.vehicle.attitude.pitch < 0 and self.check_altitude < 3):
self.beamreach = (self.check_altitude / math.sin(self.pitch_angle))
else:
self.beamreach = 1000
if (self.beamreach > SCAN_BEAM_DISTANCE):
if (self.led_beam_angle_state == 1):
self.led_beam_angle_state = 0
self.LEDrgb(2, 2, 0, 255, 0)
self.LEDrgb(3, 2, 255, 0, 0)
if (self.arduinoBoard.digital_read(COLL_RIGHT) == 1):
if (self.led_right_state == 0):
logger.log("[objavoid]: Obstacle on the right")
# send info to app
if self.appMgr.isAppConnected():
exceptStr = "Obstacle to the right"
packet = struct.pack(
'<II%ds' % (len(exceptStr)),
app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR,
len(exceptStr), exceptStr)
self.appMgr.client.send(packet)
# sleep to make sure the packet goes out
time.sleep(0.1)
# LED right_front set to strobe magenta
self.LEDrgb(3, 2, 255, 0, 0)
self.LEDrgb(2, 4, 255, 0, 255)
self.led_right_state = 1
self.led_left_state = 0
self.led_center_state = 0
self.led_beam_angle_state = 0
elif (self.arduinoBoard.digital_read(COLL_CENTER) == 1):
if (self.led_center_state == 0):
logger.log("[objavoid]: Obstacle in center")
# when we are not in a shot, nor in RTL or LAND and flying higher than 1 meter
if (self.currentShot == shots.APP_SHOT_NONE
and (self.vehicle.mode.name != 'LAND'
or self.vehicle.mode.name != 'RTL')):
if (self.check_altitude > 1):
self.vehicle.mode = VehicleMode("BRAKE")
# all other shots and modes trigger an audio and visual warning only to not interfere with shots
# send status to app
if self.appMgr.isAppConnected():
exceptStr = "Obstacle ahead in %.1f" % self.coll_distance(
) + " meters"
packet = struct.pack(
'<II%ds' % (len(exceptStr)),
app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR,
len(exceptStr), exceptStr)
self.appMgr.client.send(packet)
# sleep to make sure the packet goes out
time.sleep(0.2)
# both LED front set to strobe magenta
self.LEDrgb(2, 4, 255, 0, 255)
self.LEDrgb(3, 4, 255, 0, 255)
self.led_right_state = 0
self.led_left_state = 0
self.led_center_state = 1
self.led_beam_angle_state = 0
elif (self.arduinoBoard.digital_read(COLL_LEFT) == 1):
if (self.led_left_state == 0):
logger.log("[objavoid]: Obstacle on the left")
# send status to app
if self.appMgr.isAppConnected():
exceptStr = "Obstacle to the left"
packet = struct.pack(
'<II%ds' % (len(exceptStr)),
app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR,
len(exceptStr), exceptStr)
self.appMgr.client.send(packet)
# sleep to make sure the packet goes out
time.sleep(0.1)
# LED left_front set to strobe magenta
self.LEDrgb(2, 2, 0, 255, 0)
self.LEDrgb(3, 4, 255, 0, 255)
self.led_right_state = 0
self.led_left_state = 1
self.led_center_state = 0
self.led_beam_angle_state = 0
else:
# no obstacle in sight reset everything
if (self.led_left_state == 1 or self.led_right_state == 1
or self.led_center_state == 1):
logger.log("[objavoid]: no obstacle in sight reset LED")
self.LEDrgb(2, 2, 0, 255, 0)
self.LEDrgb(3, 2, 255, 0, 0)
self.led_right_state = 0
self.led_left_state = 0
self.led_center_state = 0
self.led_beam_angle_state = 0
else:
if (self.led_beam_angle_state == 0):
logger.log(
"[objavoid]: pitch/altitude too low - obstacle detection disabled"
)
self.LEDrgb(2, 4, 255, 100, 0)
self.LEDrgb(3, 4, 255, 100, 0)
self.led_beam_angle_state = 1
self.led_right_state = 0
self.led_left_state = 0
self.led_center_state = 0
# send status to app
if self.appMgr.isAppConnected():
exceptStr = "Solo altitude too low for scan"
packet = struct.pack(
'<II%ds' % (len(exceptStr)),
app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR,
len(exceptStr), exceptStr)
self.appMgr.client.send(packet)
# sleep to make sure the packet goes out
time.sleep(0.1)
def test_vehicle_mode_eq():
assert_equals(VehicleMode('GUIDED'), VehicleMode('GUIDED'))
def setMode(mode):
# Now change the vehicle into auto mode
v.mode = VehicleMode(mode)
# Always call flush to guarantee that previous writes to the vehicle have taken place
v.flush()
print 'Clear the current mission'
clear_mission()
print 'Create a new mission'
adds_square_mission(vehicle.location, 50)
time.sleep(
2) # This is here so that mission being sent is displayed on console
# From Copter 3.3 you will be able to take off using a mission item. Plane must take off using a mission item (currently).
arm_and_takeoff(10)
print "Starting mission"
# Set mode to AUTO to start mission
vehicle.mode = VehicleMode("AUTO")
vehicle.flush()
# Monitor mission.
# Demonstrates getting and setting the command number
# Uses distance_to_current_waypoint(), a convenience function for finding the
# distance to the next waypoint.
while True:
nextwaypoint = vehicle.commands.next
if nextwaypoint > 1:
print 'Distance to waypoint (%s): %s' % (
nextwaypoint, distance_to_current_waypoint())
if nextwaypoint == 3: #Skip to next waypoint
print 'Skipping to Waypoint 4 when reach waypoint 3'
vehicle.commands.next = 4
print("Set new Home location to current location")
set_home(vehicle.location)
print "Get new home location" #This reloads the home location in GCSs
cmds = vehicle.commands
cmds.download()
cmds.wait_valid()
print " Home WP: %s" % cmds[0]
print("Velocity South and West")
send_global_velocity(NORTH, EAST, 0)
print("Yaw 90 relative (to previous yaw heading)")
condition_yaw(90, relative=True)
time.sleep(DURATION)
send_global_velocity(0, 0, 0)
print("Velocity North and West")
send_global_velocity(SOUTH, EAST, 0)
print("Yaw 90 relative (to previous yaw heading)")
condition_yaw(90, relative=True)
time.sleep(DURATION)
send_global_velocity(0, 0, 0)
"""
The example is completing. LAND at current location.
"""
print("Setting LAND mode...")
vehicle.mode = VehicleMode("LAND")
vehicle.flush()
print("Completed")
def enterShot(self, shot):
if shot not in shots.SHOT_NAMES:
logger.log("[shot]: Shot not recognized. (%d)" % shot)
return
if shot == shots.APP_SHOT_NONE:
pass
# check our EKF - if it's bad and that we can't init the shot prior to EKF being OK, reject shot entry attempt
elif self.last_ekf_ok is False and shot not in shots.CAN_START_BEFORE_EKF:
logger.log('[shot]: Vehicle EKF quality is poor, shot entry into %s disallowed.' % shots.SHOT_NAMES[shot])
# set shot to APP_SHOT_NONE
shot = shots.APP_SHOT_NONE
# notify the app of shot entry failure
packet = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_BAD_EKF)
self.appMgr.sendPacket(packet)
# check vehicle system status - if it's CRITICAL or EMERGENCY, reject shot entry attempt
elif self.vehicle.system_status in ['CRITICAL', 'EMERGENCY']:
logger.log('[shot]: Vehicle in %s, shot entry into %s disallowed.' % (self.vehicle.system_status, shots.SHOT_NAMES[shot]))
# set shot to APP_SHOT_NONE
shot = shots.APP_SHOT_NONE
# notify the app of shot entry failure
packet = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_RTL)
self.appMgr.sendPacket(packet)
# check if vehicle is not armed or in STANDBY and that we can't init the shot prior to arm, reject shot entry attempt
elif (self.vehicle.armed is False or self.vehicle.system_status == 'STANDBY') and shot not in shots.CAN_START_BEFORE_ARMING:
logger.log('[shot]: Vehicle is unarmed, shot entry into %s disallowed.' % shots.SHOT_NAMES[shot])
self.vehicle.mode = VehicleMode("LOITER")
# set shot to APP_SHOT_NONE
shot = shots.APP_SHOT_NONE
# notify the app of shot entry failure
packet = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_UNARMED)
self.appMgr.sendPacket(packet)
# OK fine, you get to start the shot.
if self.currentShot != shot:
logger.log('[shot]: Entering shot %s.' % shots.SHOT_NAMES[shot])
if self.currentShot == shots.APP_SHOT_REWIND:
# we are exiting Rewind
self.rewindManager.resetSpline()
# APP_SHOT_NONE
if shot == shots.APP_SHOT_NONE:
# mark curController for garbage collection
del self.curController
# set curController to None (should also mark for garbage collection)
self.curController = None
# re-enable manual gimbal controls (RC Targeting mode)
self.vehicle.gimbal.release()
# disable the stick re-mapper
self.rcMgr.enableRemapping( False )
# disable scanning
self.arduinoBoard.digital_write(6, 0)
# if the Rewind shot put us into RTL, lets stay there
if self.vehicle.mode.name == 'RTL':
logger.log("[shot]: Leaving vehicle in mode RTL")
# if vehicle mode is in another mode such as GUIDED or AUTO, then switch to LOITER
elif self.vehicle.mode.name in shots.SHOT_MODES:
logger.log("[shot]: Changing vehicle mode to LOITER.")
self.vehicle.mode = VehicleMode("LOITER")
else:
self.curController = ShotFactory.get_shot_obj(shot, self.vehicle, self)
# update currentShot
self.currentShot = shot
logger.log("[shot]: Successfully entered %s." % shots.SHOT_NAMES[shot])
# already in that shot
else:
logger.log('[shot]: Already in shot %s.' % shots.SHOT_NAMES[shot])
# let the world know
if self.appMgr.isAppConnected():
self.appMgr.broadcastShotToApp(shot)
# let Artoo know too
self.buttonManager.setArtooShot(shot)
# set new button mappings appropriately
self.buttonManager.setButtonMappings()
def Start(self, vehicle):
logger.log("+-+-+-+-+-+-+ Starting up %s +-+-+-+-+-+-+" % VERSION)
### initialize dronekit vehicle ###
self.vehicle = vehicle
### switch vehicle to loiter mode ###
self.vehicle.mode = VehicleMode("LOITER")
### initialize rc manager ###
self.rcMgr = rcManager.rcManager(self)
### initialize app manager ###
self.appMgr = appManager.appManager(self)
### initialize button manager ###
self.buttonManager = buttonManager.buttonManager(self)
### initialize gopro manager ###
self.goproManager = GoProManager.GoProManager(self)
### Initialize GeoFence manager ###
self.geoFenceManager = GeoFenceManager.GeoFenceManager(self)
# instantiate rewindManager
self.rewindManager = rewindManager.RewindManager(self.vehicle, self)
# Create a PyMata instance and initialize the object avoidance toggles
self.led_left_state = 0
self.led_right_state = 0
self.led_center_state = 0
self.led_beam_angle_state = 0
self.center_collision_state = 0
try:
self.arduinoBoard = PyMata("/dev/ttyACM0", verbose=True)
self.arduinoBoard.set_pin_mode(COLL_RIGHT, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL)
self.arduinoBoard.set_pin_mode(COLL_CENTER, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL)
self.arduinoBoard.set_pin_mode(COLL_LEFT, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL)
self.arduinoBoard.set_pin_mode(SCAN_BIT1, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL)
self.arduinoBoard.set_pin_mode(SCAN_BIT2, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL)
self.arduinoBoard.set_pin_mode(SCAN_BIT3, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL)
self.arduinoBoard.set_pin_mode(SCAN_BIT4, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL)
except:
logger.log("[shotmanager]: Error in communication to Arduino")
### init APM stream rates ###
self.initStreamRates()
### register callbacks ###
self.registerCallbacks()
# Try to maintain a constant tick rate
self.timeOfLastTick = monotonic.monotonic()
# how many ticks have we performed since an RC update?
# register all connections (gopro manager communicates via appMgr's socket)
self.inputs = [self.rcMgr.server, self.appMgr.server]
self.outputs = []
#check if gimbal is present
if self.vehicle.gimbal.yaw is not None:
logger.log("[shot]: Gimbal detected.")
# Initialize gimbal to RC targeting
self.vehicle.gimbal.release()
else:
logger.log("[shot]: No gimbal detected.")
# mark first tick time
self.timeOfLastTick = monotonic.monotonic()
# check for In-Air start from Shotmanager crash
if self.vehicle.system_status == 'ACTIVE':
logger.log("[shot]: Restart in air.")
# load vehicle home
self.rewindManager.loadHomeLocation()
def __get_mode(self):
"""Private method to read current vehicle mode without polling"""
return VehicleMode(self.__module.status.flightmode)
