def __init__(self, vehicle, shotmgr):
# initialize vehicle object
self.vehicle = vehicle
# initialize shotmanager object
self.shotmgr = shotmgr
# initialize pathController to None
self.pathController = None
# used to manage cruise speed and pause control for orbiting
self.pathHandler = pathHandler.PathHandler(self.vehicle, self.shotmgr)
''' Shot State '''
self.followState = FOLLOW_WAIT
self.followPreference = FOLLOW_PREF_HOLD_ANGLE
# init camera mount style
self.updateMountStatus()
''' ROI control '''
# initialize raw & filtered rois to None
self.filteredROI = None
self.rawROI = None
# initialize raw and filtered roi queues
self.rawROIQueue = collections.deque(maxlen=MIN_RAW_ROI_QUEUE_LENGTH)
self.filteredROIQueue = collections.deque(
maxlen=MIN_FILT_ROI_QUEUE_LENGTH)
# initialize roiVelocity to None
self.roiVelocity = None
# for limiting follow acceleration could lead to some bad lag
self.translateVel = Vector3()
''' Altitude Limit'''
# get maxClimbRate and maxAltitude from APM params
self.maxClimbRate = shotmgr.getParam(
"PILOT_VELZ_MAX",
DEFAULT_PILOT_VELZ_MAX_VALUE) / 100.0 * ZVEL_FACTOR
self.maxAlt = self.shotmgr.getParam("FENCE_ALT_MAX",
DEFAULT_FENCE_ALT_MAX)
logger.log("[follow]: Maximum altitude stored: %f" % self.maxAlt)
# check APM params to see if Altitude Limit should apply
if self.shotmgr.getParam("FENCE_ENABLE", DEFAULT_FENCE_ENABLE) == 0:
self.maxAlt = None
logger.log("[Follow Me]: Altitude Limit disabled.")
# the open loop altitude of the follow controllers, relative to the ROI
self.followControllerAltOffset = 0
# used to adjust frame of ROI vertically (Just affects camera pointing, not copter position)
self.ROIAltitudeOffset = 0
''' Communications '''
# configure follow socket
self.setupSocket()
# take away user control of vehicle
self.vehicle.mode = VehicleMode("GUIDED")
self.shotmgr.rcMgr.enableRemapping(True)
def initOrbitShot(self):
'''Initialize the orbit autonomous controller - called once'''
# create pathHandler object
self.pathHandler = pathHandler.PathHandler(self.vehicle, self.shotmgr)
# set button mappings
self.setButtonMappings()