mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 14:14:30 +02:00
294 lines
9.3 KiB
Python
294 lines
9.3 KiB
Python
#
|
|
# 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
|