def setupZipline(self):
if self.state == ZIPLINE_SETUP:
# enter GUIDED mode
self.vehicle.mode = VehicleMode("GUIDED")
# Take over RC
self.shotmgr.rcMgr.enableRemapping(True)
# default camera mode to FREE LOOK
self.camPointing = -1
self.initCam(FREE_LOOK)
self.state = ZIPLINE_RUN
# re-init Yaw
self.camYaw = camera.getYaw(self.vehicle)
# null pitch if we only want 2D ziplines
if self.is3D == 0:
self.camPitch = 0
# create a new pathHandler obejct with our new points
self.pathHandler = vectorPathHandler.VectorPathHandler(
self.vehicle, self.shotmgr, self.camYaw, self.camPitch)
self.pathHandler.setCruiseSpeed(self.cruiseSpeed)
# re-init Pitch
self.camPitch = camera.getPitch(self.vehicle)
# update the app
self.updateAppOptions()
self.updateAppStart()
def initFreeLookController(self):
'''Enter/exit free look'''
vectorOffset = location_helpers.getVectorFromPoints(
self.filteredROI, self.vehicle.location.global_relative_frame)
xOffset = vectorOffset.x
yOffset = vectorOffset.y
zOffset = self.followControllerAltOffset
heading = camera.getYaw(self.vehicle)
# Initialize the feed-forward orbit controller
self.pathController = FlyController(self.filteredROI, xOffset, yOffset,
zOffset, heading)
# set controller options
self.pathController.setOptions(maxClimbRate=self.maxClimbRate,
maxAlt=self.maxAlt)
# default camPitch and Yaw from vehicle
self.camPitch = camera.getPitch(self.vehicle)
self.camYaw = camera.getYaw(self.vehicle)
# only used for non-gimbaled copters
self.camDir = 1
def __init__(self, vehicle, shotmgr):
self.vehicle = vehicle
self.shotmgr = shotmgr
# Limit ziplines to a plane parallel with Earth surface
self.is3D = False
# default pathHandler to none
self.pathHandler = None
# track cruise speed between pathHandler instances
self.cruiseSpeed = 0
# tracks the camera control mode
self.camPointing = FREE_LOOK
# ROI will update when this is True
self.needsUpdate = True
# Camera control
self.camYaw = camera.getYaw(self.vehicle)
self.camPitch = camera.getPitch(self.vehicle)
self.camDir = 1
# initialize roi object to none
# roi used to store spot lock location object
self.roi = None
self.state = ZIPLINE_SETUP
def handlePathSettings(self, pathSettings):
'''pass in the value portion of the SOLO_SPLINE_PATH_SETTINGS packet'''
(cameraMode, desiredTime) = pathSettings
# don't run this function if we're not in Play Mode
if not self.cableCamPlaying:
logger.log(
"[multipoint]: Can't set cable settings. Not in play mode.")
return
# change pointing modes
if cameraMode != self.camInterpolation:
if cameraMode == 0:
self.yawPitchOffsetter.enableNudge()
logger.log("[multipoint]: Entering InterpolateCam.")
self.camInterpolation = 0
elif cameraMode == 1:
self.yawPitchOffsetter.disableNudge(
camera.getPitch(self.vehicle), camera.getYaw(self.vehicle))
logger.log("[multipoint]: Entering FreeCam.")
self.camInterpolation = 1
else:
logger.log("[multipoint]: Camera mode not recognized (%d)." %
self.camInterpolation)
self.yawPitchOffsetter.enableNudge()
# calculate cruise speed from desiredTime
self.setCruiseSpeed(speed=self.estimateCruiseSpeed(desiredTime))
def __init__(self, vehicle, shotmgr):
# assign the vehicle object
self.vehicle = vehicle
# assign the shotManager object
self.shotmgr = shotmgr
# Exit the shot and use RTL Mode
self.vehicle.mode = VehicleMode("RTL")
self.shotmgr.rcMgr.enableRemapping(false)
return
############################################################
# data manager for breadcrumbs
self.rewindManager = shotmgr.rewindManager
# defines how we exit
self.exitToRTL = True
# enable stick remappings
self.shotmgr.rcMgr.enableRemapping(True)
# enter GUIDED mode
logger.log("[Rewind] Try Guided")
self.setButtonMappings()
self.vehicle.mode = VehicleMode("GUIDED")
# grab a copy of home
self.homeLocation = self.shotmgr.getHomeLocation()
''' spline '''
# default targetP
self.targetP = 0.0
# initialize cable to None
self.cable = None
# last time that the controller was advanced
self.lastTime = None
self.splineOrigin = None
if not self.generateSplines():
logger.log("[Rewind]: Spline generation failed.")
if self.cable is not None:
# go to 1.0
self.cable.setTargetP(1.0)
# give cable controller our desired speed
self.cable.trackSpeed(REWIND_SPEED)
# Camera control
self.camYaw = camera.getYaw(self.vehicle)
self.camPitch = camera.getPitch(self.vehicle)
self.camDir = 1
self.manualGimbalTargeting()
def recordLocation(self):
'''record a cable control point'''
# don't run this function if we're already in the cable
if self.cableCamPlaying:
logger.log(
"[multipoint]: Cannot record a location when in PLAY mode.")
return
# record HOME absolute altitude reference if this is the first waypoint
if len(self.waypoints) == 0:
self.absAltRef = self.vehicle.location.global_frame.alt - self.vehicle.location.global_relative_frame.alt
logger.log(
"[multipoint]: HOME absolute altitude recorded: %f meters" %
self.absAltRef)
# capture current pitch and yaw state
pitch = camera.getPitch(self.vehicle)
degreesYaw = location_helpers.wrapTo360(camera.getYaw(self.vehicle))
# check if last recorded point is a duplicate with this new one
if self.duplicateCheck(self.vehicle.location.global_relative_frame,
len(self.waypoints)):
logger.log("[multipoint]: Overwriting last point.")
# overwrite last point
self.waypoints[
-1].loc = self.vehicle.location.global_relative_frame
self.waypoints[-1].pitch = pitch
self.waypoints[-1].yaw = degreesYaw
else:
# append a new point
self.waypoints.append(
Waypoint(self.vehicle.location.global_relative_frame, pitch,
degreesYaw))
# log this point
logger.log(
"[multipoint]: Successfully recorded waypoint #%d. Lat: %f, Lon: %f, Alt: %f, Pitch: %f, Yaw: %f"
% (len(self.waypoints) - 1,
self.vehicle.location.global_relative_frame.lat,
self.vehicle.location.global_relative_frame.lon,
self.vehicle.location.global_relative_frame.alt, pitch,
degreesYaw))
# send the spline point to the app
self.sendSoloSplinePoint(
self.splinePointVersion, self.absAltRef,
len(self.waypoints) - 1,
self.vehicle.location.global_relative_frame.lat,
self.vehicle.location.global_relative_frame.lon,
self.vehicle.location.global_relative_frame.alt, pitch, degreesYaw,
0, app_packet.SPLINE_ERROR_NONE)
logger.log("[multipoint]: Sent spline point #%d to app." %
(len(self.waypoints) - 1))
# update button mappings
self.setButtonMappings()
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
#state machine
self.state = PANO_SETUP
# Default panoType to None
self.panoType = PANO_CYLINDER
# steps to track incrementing in shot
self.step = 0
self.stepsTotal = 0
# Yaw rate for Video Pano shot
self.degSecondYaw = PANO_DEFAULT_VIDEO_YAW_RATE
# default FOV for Cylinder Pano shot
self.cylinder_fov = PANO_DEFAULT_FOV
self.lensFOV = CYLINDER_LENS_ANGLE
# list of angles in Cylinder Pano shot
self.cylinderAngles = None
self.sphereAngles = None
# default camPitch
self.camPitch = camera.getPitch(self.vehicle)
# default camYaw to current pointing
self.camYaw = camera.getYaw(self.vehicle)
# only used for non-gimbaled copters
self.camDir = 1
self.counter = 0
# 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()
# give the app our defaults
self.updateAppOptions()
# take over RC
self.shotmgr.rcMgr.enableRemapping(True)
def spotLock(self):
'''take the angle of the copter and lock onto a ground level target'''
logger.log("[Orbit] spotLock")
# don't use a shallow angle resulting in massively distant ROIs
pitch = min(camera.getPitch(self.vehicle), SHALLOW_ANGLE_THRESHOLD)
# Get ROI for the vehicle to look at
spotLock = location_helpers.getSpotLock(
self.vehicle.location.global_relative_frame, pitch,
camera.getYaw(self.vehicle))
self.addLocation(spotLock)
def manualGimbalTargeting(self):
'''set gimbal targeting mode to manual'''
msg = self.vehicle.message_factory.mount_configure_encode(
0,
1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, # mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
self.vehicle.send_mavlink(msg)
# init the open loop gimbal pointing vars
self.camYaw = camera.getYaw(self.vehicle)
self.camPitch = camera.getPitch(self.vehicle)
def spotLock(self):
'''take the angle of the copter and lock onto a ground level target'''
if self.camPointing == SPOT_LOCK:
return
self.needsUpdate = True
# don't use a shallow angle resulting in massively distant ROIs
pitch = min(camera.getPitch(self.vehicle), SHALLOW_ANGLE_THRESHOLD)
# Get ROI for the vehicle to look at
spotLock = location_helpers.getSpotLock(
self.vehicle.location.global_relative_frame, pitch,
camera.getYaw(self.vehicle))
self.addLocation(spotLock)
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 __init__(self, vehicle, shotmgr):
# assign the vehicle object
self.vehicle = vehicle
# assign the shotManager object
self.shotmgr = shotmgr
# Exit the shot and use RTL Mode
self.vehicle.mode = VehicleMode("RTL")
self.shotmgr.rcMgr.enableRemapping(false)
return
##########################################################
# grab a copy of home
self.homeLocation = self.shotmgr.getHomeLocation()
# enable stick remappings
self.shotmgr.rcMgr.enableRemapping(True)
# enter GUIDED mode
logger.log("[RTL] Try Guided")
self.setButtonMappings()
self.vehicle.mode = VehicleMode("GUIDED")
# sets desired climb altitude
self.rtlAltParam = shotmgr.getParam("RTL_ALT", RTL_ALT) / 100.0
self.returnAlt = max(
(self.vehicle.location.global_relative_frame.alt + RTL_CLIMB_MIN),
self.rtlAltParam)
self.returnAlt = self.coneOfAwesomeness(self.returnAlt)
# open loop alt target
self.targetAlt = self.vehicle.location.global_relative_frame.alt
# manages acceleration of alt target
self.desiredClimbRate = 0
self.currentClimbRate = 0
# init state machine
self.state = RISING
# Camera control
self.camYaw = camera.getYaw(self.vehicle)
self.camPitch = camera.getPitch(self.vehicle)
self.camDir = 1
self.manualGimbalTargeting()
def setupTargeting(self):
# set gimbal targeting mode
msg = self.vehicle.message_factory.mount_configure_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, #mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
self.shotmgr.rcMgr.enableRemapping( True )
logger.log("[cable cam]: setting gimbal to mavlink mode")
if self.camInterpolation > 0:
logger.log("[cable cam]: turning on camera interpolation")
self.yawPitchOffsetter.enableNudge()
else:
logger.log("[cable cam]: turning off camera interpolation")
self.yawPitchOffsetter.disableNudge( camera.getPitch(self.vehicle), camera.getYaw(self.vehicle))
self.vehicle.send_mavlink(msg)
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 testCameraGetPitchNoGimbal(self):
""" retrieve pitch from camera class (no gimbal) """
v = Vehicle()
self.assertEqual( camera.getPitch(v), 0.0 )
def testCameraGetPitch45(self):
""" retrieve 45 pitch from camera class """
v = Vehicle( pitch = 45.0 )
self.assertEqual( camera.getPitch(v), 45.0 )
def testCameraGetPitch70(self):
""" retrieve 70 pitch from camera class """
v = Vehicle( pitch = 70.0 )
self.assertEqual( camera.getPitch(v), 70.0 )
