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.
292 lines
10 KiB
Python
292 lines
10 KiB
Python
# selfie.py
|
|
# shotmanager
|
|
#
|
|
# The selfie smart shot.
|
|
# Press Fly Out in the app and the drone flies up and out, keeping the camera pointed at the ROI
|
|
# Runs as a DroneKit-Python script.
|
|
#
|
|
# Created by Will Silva and Eric Liao in 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 VehicleMode, LocationGlobalRelative
|
|
from pymavlink import mavutil
|
|
import os
|
|
from os import sys, path
|
|
import struct
|
|
import math
|
|
|
|
sys.path.append(os.path.realpath(''))
|
|
import app_packet
|
|
import location_helpers
|
|
import pathHandler
|
|
import shotLogger
|
|
import shots
|
|
from shotManagerConstants import *
|
|
# on host systems these files are located here
|
|
sys.path.append(os.path.realpath('../../flightcode/stm32'))
|
|
from sololink import btn_msg
|
|
|
|
YAW_SPEED = 60.0
|
|
PITCH_SPEED = 60.0
|
|
|
|
logger = shotLogger.logger
|
|
|
|
class SelfieShot():
|
|
def __init__(self, vehicle, shotmgr):
|
|
self.vehicle = vehicle
|
|
self.shotmgr = shotmgr
|
|
self.waypoints = []
|
|
self.roi = None
|
|
self.pathHandler = None
|
|
|
|
# Camera control
|
|
self.camPitch = 0
|
|
self.camYaw = 0
|
|
self.camDir = 1
|
|
self.canResetCam = False
|
|
|
|
self.setButtonMappings()
|
|
|
|
# get current altitude limit
|
|
self.altLimit = self.shotmgr.getParam( "FENCE_ALT_MAX", DEFAULT_FENCE_ALT_MAX ) # in meters
|
|
|
|
# check APM params to see if Altitude Limit should apply
|
|
if self.shotmgr.getParam( "FENCE_ENABLE", DEFAULT_FENCE_ENABLE ) == 0:
|
|
self.altLimit = None
|
|
logger.log("[Selfie]: Altitude Limit is disabled.")
|
|
|
|
# channels are expected to be floating point values in the (-1.0, 1.0) range
|
|
def handleRCs( self, channels ):
|
|
#selfie needs 2 waypoints and an ROI to be ready
|
|
if len(self.waypoints) != 2 or not self.roi:
|
|
return
|
|
|
|
self.pathHandler.MoveTowardsEndpt(channels)
|
|
self.manualPitch(channels)
|
|
self.manualYaw(channels)
|
|
self.handleFreeLookPointing()
|
|
|
|
if self.pathHandler.isNearTarget():
|
|
# if we reached the end, flip
|
|
if self.pathHandler.cruiseSpeed > 0.0:
|
|
self.pathHandler.cruiseSpeed = -self.pathHandler.cruiseSpeed
|
|
logger.log("[Selfie]: reached end of selfie path, flipping")
|
|
elif self.pathHandler.cruiseSpeed < 0.0:
|
|
logger.log("[Selfie]: reached end of selfie path, pausing")
|
|
self.pathHandler.pause()
|
|
|
|
self.updateAppOptions()
|
|
|
|
|
|
def setButtonMappings(self):
|
|
buttonMgr = self.shotmgr.buttonManager
|
|
|
|
if self.canResetCam:
|
|
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_SELFIE, btn_msg.ARTOO_BITMASK_ENABLED, "Look At Me\0")
|
|
else:
|
|
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_SELFIE, 0, "\0")
|
|
|
|
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_SELFIE, 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:
|
|
self.pointAtROI()
|
|
|
|
|
|
if button == btn_msg.ButtonLoiter and event == btn_msg.ClickRelease:
|
|
if self.pathHandler:
|
|
self.shotmgr.notifyPause(True)
|
|
self.pathHandler.togglePause()
|
|
self.updateAppOptions()
|
|
else:
|
|
# notify autopilot of pause press (technically not in shot)
|
|
self.shotmgr.notifyPause(False)
|
|
|
|
|
|
# send our current set of options to the app
|
|
def updateAppOptions(self):
|
|
speed = 0.0
|
|
|
|
if self.pathHandler:
|
|
speed = self.pathHandler.cruiseSpeed
|
|
|
|
packet = struct.pack('<IIf', app_packet.SOLO_SHOT_OPTIONS, 4, speed)
|
|
self.shotmgr.appMgr.sendPacket(packet)
|
|
|
|
|
|
def handlePacket(self, packetType, packetLength, packetValue):
|
|
try:
|
|
if packetType == app_packet.SOLO_MESSAGE_LOCATION:
|
|
(lat, lon, alt) = struct.unpack('<ddf', packetValue)
|
|
logger.log("[selfie]: Location received from app: %f, %f, %f." %( lat, lon, alt ) )
|
|
self.addLocation(LocationGlobalRelative(lat, lon, alt))
|
|
|
|
elif packetType == app_packet.SOLO_SHOT_OPTIONS:
|
|
(cruiseSpeed,) = struct.unpack('<f', packetValue)
|
|
|
|
if self.pathHandler:
|
|
self.pathHandler.setCruiseSpeed(cruiseSpeed)
|
|
logger.log("[selfie]: Cruise speed set to %.2f." % (cruiseSpeed,))
|
|
else:
|
|
return False
|
|
except Exception as e:
|
|
logger.log('[selfie]: Error handling packet. (%s)' % e)
|
|
return False
|
|
else:
|
|
return True
|
|
|
|
|
|
def addLocation(self, loc):
|
|
# we're ready once we have 2 waypoints and an ROI
|
|
if len(self.waypoints) < 2:
|
|
|
|
# check altitude limit on 2nd point
|
|
if self.altLimit is not None and len(self.waypoints) > 0:
|
|
if loc.alt > self.altLimit:
|
|
logger.log("[Selfie]: 2nd selfie point breaches user-set altitude limit (%.1f meters)." % (self.altLimit))
|
|
|
|
# find vector to 2nd point
|
|
selfieVector = location_helpers.getVectorFromPoints(self.waypoints[0],loc)
|
|
|
|
# normalize it
|
|
selfieVector.normalize()
|
|
|
|
# calculate distance between two points
|
|
d = location_helpers.getDistanceFromPoints3d(self.waypoints[0],loc)
|
|
|
|
# calculate hypotenuse
|
|
hyp = (self.altLimit-self.waypoints[0].alt)/((loc.alt-self.waypoints[0].alt)/d)
|
|
|
|
# scale selfie vector by hypotenuse
|
|
selfieVector *= hyp
|
|
|
|
# add selfieVector to original selfie point to create a new 2nd point
|
|
loc = location_helpers.addVectorToLocation(self.waypoints[0],selfieVector)
|
|
|
|
self.waypoints.append(loc)
|
|
logger.log("[Selfie]: Added a selfie point: %f, %f, %f." % ( loc.lat, loc.lon,loc.alt))
|
|
|
|
elif not self.roi:
|
|
self.roi = loc
|
|
|
|
logger.log("[Selfie]: Added a selfie ROI: %f, %f, %f." %
|
|
( loc.lat, loc.lon,
|
|
loc.alt))
|
|
|
|
self.pathHandler = pathHandler.TwoPointPathHandler( self.waypoints[0], self.waypoints[1], self.vehicle, self.shotmgr )
|
|
# ready! set up everything
|
|
self.shotmgr.rcMgr.enableRemapping( True )
|
|
# Now change the vehicle into guided mode
|
|
self.vehicle.mode = VehicleMode("GUIDED")
|
|
self.manualGimbalTargeting()
|
|
self.setButtonMappings()
|
|
|
|
|
|
def manualGimbalTargeting(self):
|
|
'''set gimbal targeting mode to manual'''
|
|
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)
|
|
self.pointAtROI()
|
|
|
|
|
|
def pointAtROI(self):
|
|
logger.log("[Selfie]: Point at ROI")
|
|
# calcs the yaw and pitch to point the gimbal
|
|
self.camYaw, self.camPitch = location_helpers.calcYawPitchFromLocations(self.vehicle.location.global_relative_frame, self.roi)
|
|
|
|
if self.canResetCam:
|
|
self.canResetCam = False
|
|
self.setButtonMappings()
|
|
|
|
|
|
def updateCanResetCam(self, resetCam):
|
|
# only send button label updates on change
|
|
if resetCam != self.canResetCam:
|
|
logger.log("[Selfie]: can reset to ROI")
|
|
self.canResetCam = resetCam
|
|
self.setButtonMappings()
|
|
|
|
|
|
def manualPitch(self, channels):
|
|
if abs(channels[RAW_PADDLE]) > abs(channels[THROTTLE]):
|
|
value = channels[RAW_PADDLE]
|
|
else:
|
|
value = channels[THROTTLE]
|
|
|
|
if value == 0:
|
|
return
|
|
|
|
self.updateCanResetCam(True)
|
|
self.camPitch += value * 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.updateCanResetCam(True)
|
|
self.camYaw += channels[YAW] * YAW_SPEED * UPDATE_TIME
|
|
if self.camYaw > 360:
|
|
self.camYaw -= 360
|
|
if self.camYaw < 0:
|
|
self.camYaw += 360
|
|
|
|
# required for gimbals w/o Yaw
|
|
if channels[YAW] > 0:
|
|
self.camDir = 1
|
|
else:
|
|
self.camDir = -1
|
|
|
|
|
|
def handleFreeLookPointing(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
|
|
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)
|