mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 14:14:30 +02:00
41e7130b0f
The bug in ArduCopter master that made MSG_MOUNT_CONTROL fail to yaw the copter in smart shots has been found and fixed as of Fri 12/22. It is merged into master and begin backported to 3.5.5. As such, the workaround (simultaneous CONDITION_YAW commands) can now be removed from the smart shots. Also cleaned up the code around it a bit.
498 lines
16 KiB
Python
498 lines
16 KiB
Python
#
|
|
# pano.py
|
|
# shotmanager
|
|
#
|
|
# The pano smart shot controller.
|
|
# Runs as a DroneKit-Python script under MAVProxy.
|
|
#
|
|
# Created by Jason Short and Will Silva on 11/30/2015.
|
|
# 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.
|
|
|
|
|
|
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 shotLogger
|
|
import shots
|
|
from shotManagerConstants import *
|
|
import GoProManager
|
|
from GoProConstants import *
|
|
from sololink import btn_msg
|
|
|
|
|
|
# in degrees per second
|
|
YAW_SPEED = 10.0
|
|
|
|
# Cylinder Pano constants
|
|
CYLINDER_LENS_ANGLE = 160.0
|
|
PANO_CAPTURE_DELAY = int(UPDATE_RATE * 2.5)
|
|
PANO_MOVE_DELAY = int(UPDATE_RATE * 3.5)
|
|
|
|
PANO_YAW_SPEED = 60.0 # deg/s
|
|
PANO_PITCH_SPEED = 60.0 # deg/s
|
|
|
|
PANO_DEFAULT_VIDEO_YAW_RATE = 2.0
|
|
PANO_DEFAULT_FOV = 220
|
|
|
|
MAX_YAW_RATE = 60 # deg/s
|
|
MAX_YAW_ACCEL_PER_TICK = (MAX_YAW_RATE) / (4 * UPDATE_RATE) # deg/s^2/tick
|
|
|
|
# modes for Pano
|
|
PANO_CYLINDER = 0
|
|
PANO_SPHERE = 1
|
|
PANO_VIDEO = 2
|
|
|
|
PANO_SETUP = 0
|
|
PANO_RUN = 1
|
|
PANO_EXIT = 2
|
|
|
|
VIDEO_COUNTER = 15
|
|
|
|
TICKS_TO_BEGIN = -25
|
|
|
|
|
|
logger = shotLogger.logger
|
|
|
|
class PanoShot():
|
|
|
|
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 )
|
|
|
|
|
|
# channels are expected to be floating point values in the (-1.0, 1.0)
|
|
# range
|
|
def handleRCs(self, channels):
|
|
if self.panoType == PANO_VIDEO:
|
|
self.runVideo(channels)
|
|
self.manualPitch(channels)
|
|
self.handlePitchYaw()
|
|
|
|
else:
|
|
if self.state == PANO_SETUP:
|
|
self.manualPitch(channels)
|
|
self.manualYaw(channels[YAW])
|
|
self.handlePitchYaw()
|
|
else:
|
|
if self.panoType == PANO_SPHERE:
|
|
self.runSphere()
|
|
self.handlePitchYaw()
|
|
|
|
elif self.panoType == PANO_CYLINDER:
|
|
self.runCylinder()
|
|
self.handlePitchYaw()
|
|
|
|
|
|
def initPano(self):
|
|
# We reset to the beginning for all states
|
|
if self.panoType == PANO_SPHERE:
|
|
logger.log("[PANO]: Init Sphere")
|
|
self.enterPhotoMode()
|
|
self.initSphere()
|
|
|
|
elif self.panoType == PANO_CYLINDER:
|
|
logger.log("[PANO]: Init Cylinder")
|
|
self.enterPhotoMode()
|
|
self.initCylinder()
|
|
else:
|
|
logger.log("[PANO]: Init Video Pan")
|
|
|
|
|
|
def resetPano(self):
|
|
self.cylinderAngles = None
|
|
self.sphereAngles = None
|
|
|
|
|
|
def initSphere(self):
|
|
'''Initialize the cylinder pano shot'''
|
|
|
|
# give first move some room to get there
|
|
self.ticks = TICKS_TO_BEGIN
|
|
|
|
# Store angle presets in array
|
|
# more steps
|
|
if self.lensFOV < 140:
|
|
self.sphereAngles = [[-90,0], [-45,300], [-45,240], [-45,180], [-45,120], [-45,60], [-45,0], [0,315], [0,270], [0,225], [0,180], [0,135], [0,90], [0,45], [0,0]]
|
|
else:
|
|
# Fewer steps
|
|
self.sphereAngles = [[-90,0], [-45,270], [-45,180], [-45,90], [-45,0], [0,300], [0,240], [0,180], [0,120], [0,60], [0,0]]
|
|
|
|
self.stepsTotal = len(self.sphereAngles)
|
|
self.updatePanoStatus(0, self.stepsTotal)
|
|
|
|
# go to first angle
|
|
tmp = self.sphereAngles.pop()
|
|
self.camPitch = tmp[0]
|
|
self.camYaw = tmp[1]
|
|
|
|
|
|
|
|
def runSphere(self):
|
|
'''Run the Hemi pano program'''
|
|
self.ticks += 1
|
|
if self.sphereAngles is None:
|
|
self.initPano()
|
|
|
|
if self.state == PANO_RUN:
|
|
# time delay between camera moves
|
|
if self.ticks == PANO_CAPTURE_DELAY:
|
|
self.shotmgr.goproManager.handleRecordCommand(self.shotmgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE)
|
|
self.updatePanoStatus((self.stepsTotal) - len(self.sphereAngles), self.stepsTotal)
|
|
|
|
if self.ticks > PANO_MOVE_DELAY:
|
|
self.ticks = 0
|
|
|
|
if len(self.sphereAngles) == 0:
|
|
self.state = PANO_EXIT
|
|
# grab a last photo?
|
|
else:
|
|
# select new angle
|
|
tmp = self.sphereAngles.pop()
|
|
self.camPitch = tmp[0]
|
|
self.camYaw = tmp[1]
|
|
|
|
elif self.state == PANO_EXIT:
|
|
self.camPitch = -10
|
|
|
|
logger.log("[PANO]: completed Hemi")
|
|
# let user take a new Pano
|
|
self.state = PANO_SETUP
|
|
self.resetPano()
|
|
self.updateAppOptions()
|
|
self.setButtonMappings()
|
|
|
|
|
|
def initCylinder(self):
|
|
'''Initialize the cylinder pano shot'''
|
|
# limit the inputed FOV from App
|
|
self.cylinder_fov = max(self.cylinder_fov, 91.0)
|
|
self.cylinder_fov = min(self.cylinder_fov, 360.0)
|
|
|
|
self.yaw_total = self.cylinder_fov - (self.lensFOV/4)
|
|
steps = math.ceil(self.cylinder_fov / (self.lensFOV/4))
|
|
num_photos = math.ceil(self.cylinder_fov / (self.lensFOV/4))
|
|
yawDelta = self.yaw_total / (num_photos - 1)
|
|
|
|
self.origYaw = camera.getYaw(self.vehicle)
|
|
yawStart = self.origYaw - (self.yaw_total / 2.0)
|
|
|
|
self.cylinderAngles = []
|
|
|
|
for x in range(0, int(steps)):
|
|
tmp = yawStart + (x * yawDelta)
|
|
if tmp < 0:
|
|
tmp += 360
|
|
elif tmp > 360:
|
|
tmp -= 360
|
|
self.cylinderAngles.append(int(tmp))
|
|
|
|
self.stepsTotal = len(self.cylinderAngles)
|
|
|
|
# go to first angle
|
|
self.camYaw = self.cylinderAngles.pop()
|
|
|
|
# give first move an extra second to get there
|
|
self.ticks = TICKS_TO_BEGIN
|
|
self.updatePanoStatus(0, self.stepsTotal)
|
|
|
|
|
|
def runCylinder(self):
|
|
'''Run the Cylinder pano program'''
|
|
self.ticks += 1
|
|
|
|
if self.cylinderAngles is None:
|
|
self.initPano()
|
|
|
|
if self.state == PANO_RUN:
|
|
# time delay between camera moves
|
|
if self.ticks == PANO_CAPTURE_DELAY:
|
|
self.shotmgr.goproManager.handleRecordCommand(self.shotmgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE)
|
|
self.updatePanoStatus((self.stepsTotal) - len(self.cylinderAngles), self.stepsTotal)
|
|
|
|
if self.ticks > PANO_MOVE_DELAY:
|
|
self.ticks = 0
|
|
|
|
if len(self.cylinderAngles) == 0:
|
|
self.state = PANO_EXIT
|
|
else:
|
|
# select new angle
|
|
self.camYaw = self.cylinderAngles.pop()
|
|
|
|
elif self.state == PANO_EXIT:
|
|
self.camYaw = self.origYaw
|
|
# let user take a new Pano
|
|
self.state = PANO_SETUP
|
|
self.resetPano()
|
|
self.updateAppOptions()
|
|
self.setButtonMappings()
|
|
|
|
|
|
def runVideo(self, channels):
|
|
'''Run the Video pano program'''
|
|
# modulate yaw rate based on yaw stick input
|
|
if channels[YAW] != 0:
|
|
self.degSecondYaw = self.degSecondYaw + (channels[YAW] * MAX_YAW_ACCEL_PER_TICK)
|
|
|
|
# limit yaw rate
|
|
self.degSecondYaw = min(self.degSecondYaw, MAX_YAW_RATE)
|
|
self.degSecondYaw = max(self.degSecondYaw, -MAX_YAW_RATE)
|
|
|
|
# increment desired yaw angle
|
|
self.camYaw += (self.degSecondYaw * UPDATE_TIME)
|
|
self.camYaw %= 360.0
|
|
#logger.log("[PANO]: self.camYaw %f" % self.camYaw)
|
|
|
|
self.counter += 1
|
|
if self.counter > VIDEO_COUNTER:
|
|
self.counter = 0
|
|
self.updateAppOptions()
|
|
|
|
|
|
# send our current set of options to the app
|
|
def updateAppOptions(self):
|
|
# B = uint_8
|
|
# h = int_16
|
|
# f = float_32
|
|
# d = float_64
|
|
packet = struct.pack('<IIBBhff', app_packet.SOLO_PANO_OPTIONS, 12, self.panoType, self.state, self.cylinder_fov, self.degSecondYaw, self.lensFOV)
|
|
self.shotmgr.appMgr.sendPacket(packet)
|
|
|
|
|
|
def updatePanoStatus(self, _index, _total):
|
|
#logger.log("[Pano]: steps %d of %d" % (_index, _total))
|
|
packet = struct.pack('<IIBB', app_packet.SOLO_PANO_STATE, 2, _index, _total)
|
|
self.shotmgr.appMgr.sendPacket(packet)
|
|
|
|
|
|
def handlePacket(self, packetType, packetLength, packetValue):
|
|
try:
|
|
if packetType == app_packet.SOLO_PANO_OPTIONS:
|
|
(self.panoType, _run, self.cylinder_fov, self.degSecondYaw, self.lensFOV) = struct.unpack('<BBhff', packetValue)
|
|
logger.log("[PANO]: panoType %d" % self.panoType)
|
|
logger.log("[PANO]: state %d" % _run)
|
|
logger.log("[PANO]: cylinder_fov %d" % self.cylinder_fov)
|
|
logger.log("[PANO]: video degSecondYaw %f" % self.degSecondYaw)
|
|
logger.log("[PANO]: lens fov %f" % self.lensFOV)
|
|
|
|
# range limit lens
|
|
self.lensFOV = max(self.lensFOV, 60)
|
|
self.lensFOV = min(self.lensFOV, 180)
|
|
|
|
# tell main loop to run Pano
|
|
if _run == PANO_RUN:
|
|
self.state = PANO_RUN
|
|
else:
|
|
self.resetPano()
|
|
self.state = PANO_SETUP
|
|
|
|
self.setButtonMappings()
|
|
else:
|
|
return False
|
|
|
|
except Exception as e:
|
|
logger.log('[PANO]: Error handling packet. (%s)' % e)
|
|
return False
|
|
else:
|
|
return True
|
|
|
|
|
|
def setButtonMappings(self):
|
|
buttonMgr = self.shotmgr.buttonManager
|
|
|
|
if self.panoType == PANO_VIDEO:
|
|
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_PANO, 0, "\0")
|
|
else:
|
|
if self.state == PANO_RUN:
|
|
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_PANO, btn_msg.ARTOO_BITMASK_ENABLED, "Cancel\0")
|
|
else:
|
|
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_PANO, btn_msg.ARTOO_BITMASK_ENABLED, "Begin\0")
|
|
|
|
if self.state == PANO_SETUP:
|
|
if self.panoType == PANO_VIDEO:
|
|
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_PANO, btn_msg.ARTOO_BITMASK_ENABLED, "Video\0")
|
|
elif self.panoType == PANO_SPHERE:
|
|
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_PANO, btn_msg.ARTOO_BITMASK_ENABLED, "Sphere\0")
|
|
elif self.panoType == PANO_CYLINDER:
|
|
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_PANO, btn_msg.ARTOO_BITMASK_ENABLED, "Cylinder\0")
|
|
else:
|
|
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_PANO, 0, "\0")
|
|
|
|
|
|
# if we can handle the button we do
|
|
def handleButton(self, button, event):
|
|
if button == btn_msg.ButtonA and event == btn_msg.ClickRelease:
|
|
# don't use A button for video ever
|
|
if self.panoType != PANO_VIDEO:
|
|
if self.state == PANO_SETUP:
|
|
self.state = PANO_RUN
|
|
else:
|
|
# enter standby
|
|
self.resetPano()
|
|
self.state = PANO_SETUP
|
|
logger.log("[PANO]: Cancel Pano")
|
|
# if we are in video mode, stop Yawing
|
|
if self.panoType == PANO_VIDEO:
|
|
self.degSecondYaw = 0
|
|
self.setButtonMappings()
|
|
|
|
|
|
if button == btn_msg.ButtonLoiter and event == btn_msg.ClickRelease:
|
|
if self.panoType == PANO_VIDEO:
|
|
self.degSecondYaw = 0
|
|
|
|
# cycle through options
|
|
if self.state == PANO_SETUP and button == btn_msg.ButtonB and event == btn_msg.ClickRelease:
|
|
#clear Pano Video yaw speed
|
|
self.degSecondYaw = 0
|
|
if self.panoType == PANO_VIDEO:
|
|
self.panoType = PANO_SPHERE
|
|
elif self.panoType == PANO_SPHERE:
|
|
self.panoType = PANO_CYLINDER
|
|
elif self.panoType == PANO_CYLINDER:
|
|
self.panoType = PANO_VIDEO
|
|
self.setButtonMappings()
|
|
|
|
# tell the app what's happening
|
|
self.updateAppOptions()
|
|
|
|
|
|
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.vehicle.send_mavlink(msg)
|
|
|
|
def manualPitch(self, channels):
|
|
if abs(channels[THROTTLE]) > abs(channels[RAW_PADDLE]):
|
|
value = channels[THROTTLE]
|
|
else:
|
|
value = channels[RAW_PADDLE]
|
|
|
|
self.camPitch += value * PANO_PITCH_SPEED * UPDATE_TIME
|
|
|
|
if self.camPitch > 0.0:
|
|
self.camPitch = 0.0
|
|
elif self.camPitch < -90:
|
|
self.camPitch = -90
|
|
|
|
|
|
def manualYaw(self, stick):
|
|
if stick == 0:
|
|
return
|
|
self.camYaw += stick * PANO_YAW_SPEED * UPDATE_TIME
|
|
if stick > 0:
|
|
self.camDir = 1
|
|
else:
|
|
self.camDir = -1
|
|
|
|
self.camYaw = location_helpers.wrapTo360(self.camYaw)
|
|
|
|
|
|
def handlePitchYaw(self):
|
|
'''Send pitch and yaw commands to gimbal or fixed mount'''
|
|
# 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 enterPhotoMode(self):
|
|
# switch into photo mode if we aren't already in it
|
|
self.shotmgr.goproManager.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, (CAPTURE_MODE_PHOTO, 0 ,0 , 0))
|
|
|