''' follow.py shotmanager The Follow Me smart shot controller. Runs as a DroneKit-Python script. Created by Will Silva on 12/14/2015. Altitude and Leash follow created by Jason Short 2/25/2016 Copyright (c) 2016 3D Robotics. Licensed under the Apache License, Version 2.0 (the "License"); You may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ''' import os import math import struct import monotonic import collections import app_packet import camera import location_helpers import pathHandler import shotLogger import shots import socket from dronekit import Vehicle, LocationGlobalRelative, VehicleMode from pymavlink import mavutil from shotManagerConstants import * from orbitController import OrbitController from lookAtController import LookAtController from flyController import FlyController from leashController import LeashController from os import sys, path from vector3 import Vector3 from sololink import btn_msg sys.path.append(os.path.realpath('')) FOLLOW_PORT = 14558 SOCKET_TIMEOUT = 0.01 DEFAULT_PILOT_VELZ_MAX_VALUE = 133.0 ZVEL_FACTOR = 0.95 # in degrees per second for FREE CAM YAW_SPEED = 120.0 MIN_RAW_ROI_QUEUE_LENGTH = 5 MIN_FILT_ROI_QUEUE_LENGTH = 4 #Path accel/decel constants PATH_ACCEL = 2.5 ACCEL_PER_TICK = PATH_ACCEL * UPDATE_TIME FOLLOW_ALT_NUDGE_SPEED = 6 # m/s FOLLOW_ALT_ROI_OFFSET = 100 FOLLOW_ALT_PADDLE_DEADZONE = 0.02 FOLLOW_YAW_SPEED = 60.0 # deg/s FOLLOW_PITCH_SPEED = 60.0 # deg/s ROI_ALT_FILTER_GAIN = 0.65 # Relative contribution of the previous and new data in the phone altitude filter. Higher is slower and smoother. logger = shotLogger.logger ''' Define the different followStates: Follow Wait Initial state before everything is ready. This state exits when the first ROI is sent from the app. (in filterROI()) The shot will probably only be in this state for a few ticks. Look At Me Copter does not move unless commanded by stick actions, but the camera rotates to keep the ROI in frame Follow Orbit: Default behaviour. Copter translates to keep the user in frame, maintining a offset in a particular direction (i.e. North) of the user. Once the orbit is started (cruise via the app or with sticks) then the copter orbits around the ROI, keeping the subject in frame. Follow Leash Copter tries to stay behind the user's direction of motion. If the copter is within the leash length from the user, the leash is "slack" so the copter rotates but does not move. Once the copter gets further from the user. It swings around to get behind the user and keep up. User can adjust altitude and radius with sticks but cannot strafe manually. Follow Free Look Copter maintains an offset from the ROI and can get dragged around as the ROI moves. User can move the copter with the roll/pitch sticks like FLY mode. User can adjust the altitude with the controller paddle. User can freely control the camera pan/tilt with the left stick. Camera maintains constant yaw/pitch unless the user permutes it. ''' FOLLOW_WAIT = 0 FOLLOW_LOOKAT = 1 FOLLOW_ORBIT = 2 FOLLOW_LEASH = 3 FOLLOW_FREELOOK = 4 # used to manage user preferences for different follow styles FOLLOW_PREF_HOLD_ANGLE = 0 FOLLOW_PREF_FREELOOK = 1 FOLLOW_PREF_LEASH = 2 class FollowShot(): 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 ) # channels are expected to be floating point values in the (-1.0, 1.0) range def handleRCs( self, channels ): # check socket for a new ROI from the app self.checkSocket() # if we have never received an ROI if not self.rawROI: return # smooth ROI and calculate translateVel for follow self.filterROI() # if we are not warmed up, dont do anything if self.followState == FOLLOW_WAIT: return ''' Position Control Note: All follow controllers return position and velocity of the drone in "absolute" space (as opposed to relative to the ROI) Pos: Lat, lon, alt. Alt is relative to home location. NEU frame.. Vel: Speed in the x,y, and z directions. NEU frame. vel.z needs to be inverted before passing to the autopilot. ''' # Look At Me Mode (Vehicle stays put) if self.followState == FOLLOW_LOOKAT: pos, vel = self.pathController.move(channels) # Free Look Follow Mode (Vehicle controls are similar to FLY) elif self.followState == FOLLOW_FREELOOK: pos, vel = self.pathController.move(channels, newHeading = self.camYaw, newOrigin = self.filteredROI, roiVel = self.translateVel) # Follow Me Mode (Vehicle controls are similar to ORBIT) elif self.followState == FOLLOW_ORBIT: pos, vel = self.pathController.move(channels, cruiseSpeed = self.pathHandler.cruiseSpeed, newroi = self.filteredROI, roiVel = self.translateVel) elif self.followState == FOLLOW_LEASH: pos, vel = self.pathController.move(channels, newroi = self.filteredROI, roiVel = self.translateVel) # learn any changes to controller alt offset to apply it to other controllers when instantiated (to avoid jerks) self.followControllerAltOffset = pos.alt - self.filteredROI.alt # mavlink expects 10^7 integer for accuracy latInt = int(pos.lat * 10000000) lonInt = int(pos.lon * 10000000) # Convert from NEU to NED to send to autopilot vel.z *= -1 # create the SET_POSITION_TARGET_GLOBAL_INT command msg = self.vehicle.message_factory.set_position_target_global_int_encode( 0, # time_boot_ms (not used) 0, 1, # target system, target component mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame 0b0000110111000000, # Pos Vel type_mask latInt, lonInt, pos.alt, # x, y, z positions vel.x, vel.y, vel.z, # x, y, z velocity in m/s 0, 0, 0, # x, y, z acceleration (not used) 0, 0) # yaw, yaw_rate (not used) # send command to vehicle self.vehicle.send_mavlink(msg) ''' Pointing Control''' # If followState mandates that the user controls the pointing if self.followState == FOLLOW_FREELOOK: self.manualPitch(channels[THROTTLE]) self.manualYaw(channels) self.handleFreeLookPointing() # If followState mandates that the copter should point at the ROI else: # Adjust the height of the ROI using the paddle # we pass in both the filtered gimbal paddle value as well as the raw one self.updateROIAltOffset(channels[RAW_PADDLE]) # we make a copy of the ROI to allow us to add in an altitude offset # this means the ROI doesn't get polluted with the alt nudging tempROI = LocationGlobalRelative(self.filteredROI.lat, self.filteredROI.lon, self.filteredROI.alt) self.handleLookAtPointing(tempROI) # moves an offset of the ROI altitude up or down def updateROIAltOffset(self, rawPaddleValue): # no gimbal, no reason to change ROI if self.vehicle.mount_status[0] == None: return if abs(rawPaddleValue) > FOLLOW_ALT_PADDLE_DEADZONE: self.ROIAltitudeOffset += (rawPaddleValue * FOLLOW_ALT_NUDGE_SPEED * UPDATE_TIME) # if we can handle the button we do def handleButton(self, button, event): if button == btn_msg.ButtonA and event == btn_msg.ClickRelease: # Allow the user to exit Look At Me mode (into the previous follow mode) with the A button if self.followPreference == FOLLOW_PREF_LEASH: self.initState(FOLLOW_LEASH) elif self.followPreference == FOLLOW_PREF_FREELOOK: self.initState(FOLLOW_FREELOOK) else: self.initState(FOLLOW_ORBIT) # Change Follow Mode to Look At Me on Pause button press if button == btn_msg.ButtonLoiter and event == btn_msg.ClickRelease: self.initState(FOLLOW_LOOKAT) self.shotmgr.notifyPause(True) self.updateAppOptions() self.setButtonMappings() def setButtonMappings(self): buttonMgr = self.shotmgr.buttonManager # Allow the user to exit Look At Me mode (into the previous follow mode) with the A button if self.followState == FOLLOW_LOOKAT: buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_FOLLOW, btn_msg.ARTOO_BITMASK_ENABLED, "Resume\0") else: buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_FOLLOW, 0, "\0") buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_FOLLOW, 0, "\0") # pass in the value portion of the SOLO_FOLLOW_OPTIONS packet def handleOptions(self, options, version): if version == 1: logger.log("[Follow Me]: Received Follow Me Options v1 packet.") (cruiseSpeed, _lookat) = struct.unpack('Cruise speed: %f" % cruiseSpeed) logger.log( "[Follow Me]: -->Look At: %i" % _lookat) logger.log( "[Follow Me]: -->Follow Pref: %i" % self.followPreference) self.pathHandler.setCruiseSpeed(cruiseSpeed) # if user presses cruise arrows, force user into Orbit mode if cruiseSpeed != 0: # if we press cruise, force user into Orbit mode # only work on state changes # force follow Pref into Orbit self.followPreference = FOLLOW_PREF_HOLD_ANGLE newState = FOLLOW_ORBIT elif _lookat == 1: # Lookat overrides the follow preferences newState = FOLLOW_LOOKAT else: # we may be exiting lookat into Orbit, Freelook or Leash if self.followPreference == FOLLOW_PREF_FREELOOK: newState = FOLLOW_FREELOOK elif self.followPreference == FOLLOW_PREF_LEASH: newState = FOLLOW_LEASH else: # enter default state newState = FOLLOW_ORBIT self.initState(newState) def initState(self, newState): '''Manages state changes''' # Don't change state until we've received at least one ROI from phone # Don't re-init previous state if not self.rawROI or self.followState == newState: return self.followState = newState if self.followState == FOLLOW_LOOKAT: logger.log("[Follow Me]: enter Lookat") self.initLookAtMeController() elif self.followState == FOLLOW_ORBIT: logger.log("[Follow Me]: enter Orbit") self.initOrbitController() elif self.followState == FOLLOW_LEASH: logger.log("[Follow Me]: enter Leash") self.initLeashController() elif self.followState == FOLLOW_FREELOOK: logger.log("[Follow Me]: enter Freelook") self.initFreeLookController() self.updateMountStatus() # update UI self.updateAppOptions() self.setButtonMappings() # send our current set of options to the app def updateAppOptions(self): _lookAtMe = self.followState == FOLLOW_LOOKAT packetV1 = struct.pack(' 0: (id, length, lat, lon, alt) = struct.unpack(' 1 and self.roiDeltaTime: #calculate vector from previous to new roi in North,East,Up vec = location_helpers.getVectorFromPoints(self.filteredROIQueue[-2] , self.filteredROIQueue[-1]) # calculate velocity based on time difference # roiVelocity in NEU frame self.roiVelocity = vec/self.roiDeltaTime # calculate desiredYaw and desiredPitch from new ROI self.desiredYaw, self.desiredPitch = location_helpers.calcYawPitchFromLocations(self.vehicle.location.global_relative_frame, self.filteredROI) #x component accel limit if self.roiVelocity.x > self.translateVel.x: self.translateVel.x += ACCEL_PER_TICK self.translateVel.x = min(self.translateVel.x, self.roiVelocity.x) elif self.roiVelocity.x < self.translateVel.x: self.translateVel.x -= ACCEL_PER_TICK self.translateVel.x = max(self.translateVel.x, self.roiVelocity.x) #y component accel limit if self.roiVelocity.y > self.translateVel.y: self.translateVel.y += ACCEL_PER_TICK self.translateVel.y = min(self.translateVel.y, self.roiVelocity.y) elif self.roiVelocity.y < self.translateVel.y: self.translateVel.y -= ACCEL_PER_TICK self.translateVel.y = max(self.translateVel.y, self.roiVelocity.y) #z component accel limit if self.roiVelocity.z > self.translateVel.z: self.translateVel.z += ACCEL_PER_TICK self.translateVel.z = min(self.translateVel.z, self.roiVelocity.z) elif self.roiVelocity.z < self.translateVel.z: self.translateVel.z -= ACCEL_PER_TICK self.translateVel.z = max(self.translateVel.z, self.roiVelocity.z) def initOrbitController(self): '''Resets the controller''' # reset Orbit resetRadius = location_helpers.getDistanceFromPoints(self.filteredROI, self.vehicle.location.global_relative_frame) resetAz = location_helpers.calcAzimuthFromPoints(self.filteredROI, self.vehicle.location.global_relative_frame) # Initialize the feed-forward orbit controller self.pathController = OrbitController(self.filteredROI, resetRadius, resetAz, self.followControllerAltOffset) # set controller options self.pathController.setOptions(maxClimbRate = self.maxClimbRate, maxAlt = self.maxAlt) def initLeashController(self): '''Resets the controller''' # reset leash resetRadius = location_helpers.getDistanceFromPoints(self.filteredROI, self.vehicle.location.global_relative_frame) resetAz = location_helpers.calcAzimuthFromPoints(self.filteredROI, self.vehicle.location.global_relative_frame) # Initialize the feed-forward orbit controller self.pathController = LeashController(self.filteredROI, resetRadius, resetAz, self.followControllerAltOffset) # set controller options self.pathController.setOptions(maxClimbRate = self.maxClimbRate, maxAlt = self.maxAlt) 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 initLookAtMeController(self): '''Enter lookat mode''' # zero out any cruise speed self.pathHandler.pause() # Initialize the feed-forward orbit controller. Look at me is unique in that we pass total altitude, not just the offset self.pathController = LookAtController(self.vehicle, self.followControllerAltOffset + self.filteredROI.alt) # set controller options self.pathController.setOptions(maxClimbRate = self.maxClimbRate, maxAlt = self.maxAlt) def updateMountStatus(self): if self.followState == FOLLOW_FREELOOK: 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 ) else: # set gimbal targeting mode msg = self.vehicle.message_factory.mount_configure_encode( 0, 1, # target system, target component mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, #mount_mode 1, # stabilize roll 1, # stabilize pitch 1, # stabilize yaw ) self.vehicle.send_mavlink(msg) def manualPitch(self, stick): self.camPitch += stick * FOLLOW_PITCH_SPEED * UPDATE_TIME if self.camPitch > 0.0: self.camPitch = 0.0 elif self.camPitch < -90: self.camPitch = -90 def manualYaw(self, channels): if channels[YAW] == 0: return self.camYaw += channels[YAW] * FOLLOW_YAW_SPEED * UPDATE_TIME if channels[YAW] > 0: self.camDir = 1 else: self.camDir = -1 self.camYaw = location_helpers.wrapTo360(self.camYaw) def handleFreeLookPointing(self): # if we do have a gimbal, use mount_control to set pitch and yaw if self.vehicle.mount_status[0] is not None: msg = self.vehicle.message_factory.mount_control_encode( 0, 1, # Target system, target component self.camPitch * 100, # Pitch in centidegrees 0.0, # Roll not used self.camYaw * 100, # Yaw in centidegrees 0 # save position ) self.vehicle.send_mavlink(msg) else: # if we don't have a gimbal, just set CONDITION_YAW msg = self.vehicle.message_factory.command_long_encode( 0, 1, # target system, target component mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command 0, # confirmation self.camYaw, # param 1 - target angle YAW_SPEED, # param 2 - yaw speed self.camDir, # param 3 - direction 0.0, # relative offset 0, 0, 0 # params 5-7 (unused) ) self.vehicle.send_mavlink(msg) def handleLookAtPointing(self, tempROI): # set ROI msg = self.vehicle.message_factory.command_int_encode( 0, 1, # target system, target component mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, #frame mavutil.mavlink.MAV_CMD_DO_SET_ROI, #command 0, #current 0, #autocontinue 0, 0, 0, 0, #params 1-4 tempROI.lat*1.E7, tempROI.lon*1.E7, tempROI.alt + self.ROIAltitudeOffset #offset for ROI ) # send pointing message self.vehicle.send_mavlink(msg) def handlePacket(self, packetType, packetLength, packetValue): try: if packetType == app_packet.SOLO_FOLLOW_OPTIONS: logger.log("[follow]: Received Follow Me Options v1 packet.") self.handleOptions(packetValue, version=1) elif packetType == app_packet.SOLO_FOLLOW_OPTIONS_V2: logger.log("[follow]: Received Follow Me Options v2 packet.") self.handleOptions(packetValue, version=2) else: return False except Exception as e: logger.log('[follow]: Error handling packet. (%s)' % e) return False else: return True