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 )