# # transect.py # shotmanager # # The transect smart shot controller. # Runs as a DroneKit-Python script under MAVProxy. # # Created by Jason Short # Copyright (c) 2015 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. from dronekit import Vehicle, LocationGlobalRelative, VehicleMode from pymavlink import mavutil import os from os import sys, path import math import struct sys.path.append(os.path.realpath('')) import app_packet import camera import location_helpers import pathHandler import shotLogger import shots from shotManagerConstants import * import GoProManager from GoProConstants import * from sololink import btn_msg # length of the transect line (meters) MAX_TRANSECT_LENGTH = 500 MIN_TRANSECT_LENGTH = 50 #speed of vehicle m/s TRANSECT_SPEED = 1.5 # we can take a photo every 2 seconds MIN_TRIGGER_DIST = TRANSECT_SPEED * 2 OVERLAP = .8 STARTUP_DELAY = UPDATE_RATE * 2 RETURN_TO_START = 4 FINISH = 3 RUN = 2 TILT = 1 SETUP = 0 YAW_SPEED = 60.0 logger = shotLogger.logger class TransectShot(): 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() # channels are expected to be floating point values in the (-1.0, 1.0) range def handleRCs(self, channels): if self.state == SETUP: # point stright ahead self.camPitch = 0 # allow user to point Solo self.manualYaw(channels) self.ticks = 0 elif self.state == TILT: # point straight down self.camPitch = -90 self.ticks += 1 if self.ticks > STARTUP_DELAY: # take first shot self.shotmgr.goproManager.handleRecordCommand(self.shotmgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE) self.pathHandler.setCruiseSpeed(TRANSECT_SPEED) #temp self.state = RUN elif self.state == RUN: self.ticks = 0 self.calcPhoto() self.pathHandler.MoveTowardsEndpt(channels) # pause at the end of "infinity"! if self.pathHandler.isNearTarget(): self.pathHandler.pause() #take 1 more photo self.shotmgr.goproManager.handleRecordCommand(self.shotmgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE) self.state = FINISH elif self.state == FINISH: self.ticks += 1 if self.ticks > STARTUP_DELAY: logger.log("[Transect] Finished") self.state = RETURN_TO_START self.pathHandler.setCruiseSpeed(-6) elif self.state == RETURN_TO_START: self.pathHandler.MoveTowardsEndpt(channels) if self.pathHandler.isNearTarget(): self.pathHandler.currentSpeed = 0.0 self.pathHandler.pause() self.shotmgr.enterShot(shots.APP_SHOT_NONE) self.handlePitchYaw() def calcPhoto(self): '''determins when to shoot photo based on distance''' # calc distance from home self.dist = location_helpers.getDistanceFromPoints(self.pathHandler.pt1, self.vehicle.location.global_relative_frame) # find a slot index = math.floor(self.dist / self.triggerDist) if self.lastShot != index: self.lastShot = index self.shotmgr.goproManager.handleRecordCommand(self.shotmgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE) def setupWaypoints(self): '''setup our two waypoints''' # we want two points that are far apart # both in the direction we're looking and the opposite direction # get vehicle state self.start_loc = self.vehicle.location.global_relative_frame self.triggerDist = self.start_loc.alt * OVERLAP self.triggerDist = max(self.triggerDist, MIN_TRIGGER_DIST) # calculate the foward point forwardPt = location_helpers.newLocationFromAzimuthAndDistance(self.start_loc, self.camYaw, self.transectLength) # create a new pathHandler obejct with our new points # this will automatically reset resumeSpeed, cruiseSpeed etc... self.pathHandler = pathHandler.TwoPointPathHandler(self.start_loc, forwardPt, self.vehicle, self.shotmgr) #self.addLocation(forwardPt) def handleButton(self, button, event): if self.state == SETUP and button == btn_msg.ButtonA and event == btn_msg.Press: self.state = TILT self.setupWaypoints() self.setButtonMappings() if self.state == SETUP and button == btn_msg.ButtonB and event == btn_msg.Press: self.transectLength += 25 if self.transectLength > MAX_TRANSECT_LENGTH: self.transectLength = MIN_TRANSECT_LENGTH # redo path self.setButtonMappings() if button == btn_msg.ButtonLoiter and event == btn_msg.Press: if self.pathHandler: self.pathHandler.togglePause() self.updateAppOptions() def setButtonMappings(self): buttonMgr = self.shotmgr.buttonManager if self.state == SETUP: buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_TRANSECT, btn_msg.ARTOO_BITMASK_ENABLED, "Begin\0") buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_TRANSECT, btn_msg.ARTOO_BITMASK_ENABLED, str(self.transectLength)+"m\0") else: buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_TRANSECT, 0, "\0") buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_TRANSECT, btn_msg.ARTOO_BITMASK_ENABLED, "("+str(self.transectLength)+"m)\0") def updateAppOptions(self): '''send our current set of options to the app''' return def addLocation(self, loc): '''called by shot manager to set new ROI from App''' return def manualYaw(self, channels): if channels[YAW] == 0: return self.camYaw += channels[YAW] * YAW_SPEED * UPDATE_TIME if self.camYaw > 360: self.camYaw -= 360 if self.camYaw < 0: self.camYaw += 360 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 ) logger.log("setting gimbal to mavlink mode") self.vehicle.send_mavlink(msg) def handlePitchYaw(self): '''Handle free look''' # 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 # pitch is in centidegrees self.camPitch * 100, 0.0, # roll # yaw is in centidegrees self.camYaw * 100, 0 # save position ) self.vehicle.send_mavlink(msg) def enterPhotoMode(self): # switch into photo mode if we aren't already in it if self.shotmgr.goproManager.captureMode != CAPTURE_MODE_PHOTO: self.shotmgr.goproManager.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, (CAPTURE_MODE_PHOTO, 0 ,0 , 0)) def handlePacket(self, packetType, packetLength, packetValue): try: return False except Exception as e: logger.log('[selfie]: Error handling packet. (%s)' % e) return False else: return True