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 testCameraGetYaw90WithGimbal(self):
""" retrieve yaw 90 from camera class (gimbal) """
v = Vehicle( yaw = math.radians(90.0), pitch = 78.3 )
v.attitude.yaw = math.radians(13.0)
#self.assertEqual( camera.getYaw(v), 90.0 )
# for now, we are using vehicle yaw in all cases
self.assertEqual( camera.getYaw(v), 13.0 )
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 initCylinder(self):
'''Initialize the cylinder pano shot'''
# limit the inputed FOV from App
self.cylinder_fov = max(self.cylinder_fov, 91.0)
self.cylinder_fov = min(self.cylinder_fov, 360.0)
self.yaw_total = self.cylinder_fov - (self.lensFOV / 4)
steps = math.ceil(self.cylinder_fov / (self.lensFOV / 4))
num_photos = math.ceil(self.cylinder_fov / (self.lensFOV / 4))
yawDelta = self.yaw_total / (num_photos - 1)
self.origYaw = camera.getYaw(self.vehicle)
yawStart = self.origYaw - (self.yaw_total / 2.0)
self.cylinderAngles = []
for x in range(0, int(steps)):
tmp = yawStart + (x * yawDelta)
if tmp < 0:
tmp += 360
elif tmp > 360:
tmp -= 360
self.cylinderAngles.append(int(tmp))
self.stepsTotal = len(self.cylinderAngles)
# go to first angle
self.camYaw = self.cylinderAngles.pop()
# give first move an extra second to get there
self.ticks = TICKS_TO_BEGIN
self.updatePanoStatus(0, self.stepsTotal)
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 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 __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 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 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 __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 __init__(self, vehicle, shotmgr):
# assign the vehicle object
self.vehicle = vehicle
# assign the shotManager object
self.shotmgr = shotmgr
# default pathHandler to none
self.pathHandler = None
# default gimbal into mavlink targeting mode
self.setupTargeting()
# initialize roi object to none
self.roi = None
# default camYaw to current pointing
self.camYaw = camera.getYaw(self.vehicle)
# enter GUIDED mode
self.vehicle.mode = VehicleMode("GUIDED")
#disable stick deadzones for throttle and yaw
self.shotmgr.rcMgr.enableRemapping( True )
#state machine
self.state = SETUP
#state machine
self.lastShot = -1
# distance traveled
self.dist = 0
# distance between photos in M
self.triggerDist = 10
# length of cable
self.transectLength = MIN_TRANSECT_LENGTH
self.start_loc = None
# used for timing
self.ticks = 0
# set Camera to Photo
self.enterPhotoMode()
def initLinear(self):
'''Initialize the linear pano shot'''
self.fov = max(self.fov, 91.0)
self.fov = min(self.fov, 360.0)
steps = math.ceil(self.fov / LINEAR_INCREMENT_ANGLE)
# we could do center+- or left>right
self.origYaw = camera.getYaw(self.vehicle)
self.linearAngles = []
self.linearAngles.append(self.origYaw)
for x in xrange(1, int(steps)):
tmp = (self.origYaw - (self.fov / 2)) + (self.fov / steps) * x
if tmp < 0:
tmp += 360
elif tmp > 360:
tmp -= 360
self.linearAngles.append(tmp)
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 handleRCs( self, channels ):
if len(self.waypoints) < 2:
# if we've already recorded one waypoint, start
# trying to remember the intended rotation direction here
currentYaw = camera.getYaw(self.vehicle)
yawDiff = currentYaw - self.lastYaw
if yawDiff > 180.0:
yawDiff -= 360.0
elif yawDiff < -180.0:
yawDiff += 360.0
self.aggregateYaw += yawDiff
self.lastYaw = currentYaw
return
self.desiredSpeed, goingForwards = self.pathHandler.MoveTowardsEndpt(channels)
# the moment we enter guided for the first time, setup our targeting
# For some as yet unknown reason, this still doesn't work unless we call
# InterpolateCamera a few times, which is the reason for the
# not self.haveSetupTargeting check below
if not self.haveSetupTargeting and self.vehicle.mode.name == "GUIDED" and self.pathHandler.curTarget:
self.setupTargeting()
self.haveSetupTargeting = True
logger.log("[cable cam]: did initial setup of targeting")
self.yawPitchOffsetter.Update(channels)
if self.camInterpolation > 0 or not self.haveSetupTargeting:
# because we flipped cable cam around, we actually need to flip goingForwards
self.InterpolateCamera(not goingForwards)
else:
self.handleFreePitchYaw()
if self.pathHandler.isNearTarget():
self.pathHandler.currentSpeed = 0.0
self.pathHandler.pause()
self.updateAppOptions()
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 testCameraGetYawZeroNoGimbal(self):
""" retrieve yaw 0 from camera class (no gimbal) """
v = Vehicle( yaw = 0.0 )
self.assertEqual( camera.getYaw(v), 0.0 )
def testCameraGetYaw90NoGimbal(self):
""" retrieve yaw 90 from camera class (no gimbal) """
v = Vehicle( yaw = math.radians(90.0) )
self.assertEqual( camera.getYaw(v), 90.0 )
