mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 22:24:32 +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.
392 lines
13 KiB
Python
392 lines
13 KiB
Python
#
|
|
# zipline.py
|
|
# shotmanager
|
|
#
|
|
# The zipline smart shot controller.
|
|
# Runs as a DroneKit-Python script.
|
|
#
|
|
# Created by Jason Short
|
|
# 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 vectorPathHandler
|
|
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
|
|
|
|
SHALLOW_ANGLE_THRESHOLD = -5
|
|
ROI_ALT_MAX_DELTA = 50
|
|
|
|
# States
|
|
ZIPLINE_SETUP = 0
|
|
ZIPLINE_RUN = 1
|
|
|
|
# Camera modes
|
|
FREE_LOOK = 0
|
|
SPOT_LOCK = 1
|
|
|
|
YAW_SPEED = 60.0
|
|
PITCH_SPEED = 60.0
|
|
YAW_NUDGE_SPEED = 35.0
|
|
FOLLOW_ALT_NUDGE_SPEED = 6
|
|
|
|
MIN_PADDLE_THRESHOLD = 0.02
|
|
ROI_ALT_MARGIN = 5
|
|
|
|
logger = shotLogger.logger
|
|
|
|
class ZiplineShot():
|
|
|
|
def __init__(self, vehicle, shotmgr):
|
|
self.vehicle = vehicle
|
|
self.shotmgr = shotmgr
|
|
|
|
# Limit ziplines to a plane parallel with Earth surface
|
|
self.is3D = False
|
|
|
|
# default pathHandler to none
|
|
self.pathHandler = None
|
|
|
|
# track cruise speed between pathHandler instances
|
|
self.cruiseSpeed = 0
|
|
|
|
# tracks the camera control mode
|
|
self.camPointing = FREE_LOOK
|
|
|
|
# ROI will update when this is True
|
|
self.needsUpdate = True
|
|
|
|
# Camera control
|
|
self.camYaw = camera.getYaw(self.vehicle)
|
|
self.camPitch = camera.getPitch(self.vehicle)
|
|
self.camDir = 1
|
|
|
|
# initialize roi object to none
|
|
# roi used to store spot lock location object
|
|
self.roi = None
|
|
|
|
self.state = ZIPLINE_SETUP
|
|
|
|
|
|
|
|
# channels are expected to be floating point values in the (-1.0, 1.0) range
|
|
def handleRCs(self, channels):
|
|
if self.state == ZIPLINE_SETUP:
|
|
return
|
|
|
|
# handle camera per camera mode
|
|
if self.camPointing == SPOT_LOCK:
|
|
self.handleSpotLock(channels)
|
|
else:
|
|
# Freelook
|
|
self.manualPitch(channels)
|
|
self.manualYaw(channels)
|
|
self.handleFreeLookPointing()
|
|
|
|
# Vechicle control on the zipline
|
|
if self.pathHandler is not None:
|
|
self.pathHandler.move(channels)
|
|
|
|
|
|
def setupZipline(self):
|
|
if self.state == ZIPLINE_SETUP:
|
|
# enter GUIDED mode
|
|
self.vehicle.mode = VehicleMode("GUIDED")
|
|
|
|
# Take over RC
|
|
self.shotmgr.rcMgr.enableRemapping( True )
|
|
|
|
# default camera mode to FREE LOOK
|
|
self.camPointing = -1
|
|
self.initCam(FREE_LOOK)
|
|
|
|
self.state = ZIPLINE_RUN
|
|
|
|
|
|
# re-init Yaw
|
|
self.camYaw = camera.getYaw(self.vehicle)
|
|
|
|
# null pitch if we only want 2D ziplines
|
|
if self.is3D == 0:
|
|
self.camPitch = 0
|
|
|
|
# create a new pathHandler obejct with our new points
|
|
self.pathHandler = vectorPathHandler.VectorPathHandler(self.vehicle, self.shotmgr, self.camYaw, self.camPitch)
|
|
self.pathHandler.setCruiseSpeed(self.cruiseSpeed)
|
|
|
|
# re-init Pitch
|
|
self.camPitch = camera.getPitch(self.vehicle)
|
|
|
|
# update the app
|
|
self.updateAppOptions()
|
|
self.updateAppStart()
|
|
|
|
|
|
def setButtonMappings(self):
|
|
buttonMgr = self.shotmgr.buttonManager
|
|
buttonMgr.setArtooButton(
|
|
btn_msg.ButtonA, shots.APP_SHOT_ZIPLINE, btn_msg.ARTOO_BITMASK_ENABLED, "New Zipline\0")
|
|
|
|
if self.camPointing == SPOT_LOCK:
|
|
buttonMgr.setArtooButton(
|
|
btn_msg.ButtonB, shots.APP_SHOT_ZIPLINE, btn_msg.ARTOO_BITMASK_ENABLED, "Free Look\0")
|
|
else:
|
|
buttonMgr.setArtooButton(
|
|
btn_msg.ButtonB, shots.APP_SHOT_ZIPLINE, btn_msg.ARTOO_BITMASK_ENABLED, "Spot Lock\0")
|
|
|
|
|
|
def handleButton(self, button, event):
|
|
if button == btn_msg.ButtonA and event == btn_msg.ClickRelease:
|
|
self.setupZipline()
|
|
|
|
if button == btn_msg.ButtonB and event == btn_msg.ClickRelease:
|
|
# Toggle between free look and spot lock
|
|
if self.camPointing is FREE_LOOK:
|
|
self.initCam(SPOT_LOCK)
|
|
else:
|
|
self.initCam(FREE_LOOK)
|
|
|
|
if button == btn_msg.ButtonLoiter and event == btn_msg.ClickRelease:
|
|
if self.pathHandler:
|
|
self.pathHandler.togglePause()
|
|
self.cruiseSpeed = self.pathHandler.cruiseSpeed
|
|
self.updateAppOptions()
|
|
|
|
|
|
def initCam(self, _camPointing):
|
|
if _camPointing == self.camPointing:
|
|
return
|
|
|
|
if _camPointing is SPOT_LOCK:
|
|
self.spotLock()
|
|
self.camPointing = _camPointing
|
|
else:
|
|
self.manualGimbalTargeting()
|
|
self.camPointing = _camPointing
|
|
|
|
self.setButtonMappings()
|
|
self.updateAppOptions()
|
|
|
|
|
|
def updateAppOptions(self):
|
|
'''send our current set of options to the app'''
|
|
# B = uint_8
|
|
if self.pathHandler is None:
|
|
return
|
|
|
|
packet = struct.pack('<IIfBB', app_packet.SOLO_ZIPLINE_OPTIONS, 6, self.pathHandler.cruiseSpeed, self.is3D, self.camPointing)
|
|
self.shotmgr.appMgr.sendPacket(packet)
|
|
|
|
|
|
def updateAppStart(self):
|
|
'''Let app know we've started'''
|
|
if self.pathHandler is None:
|
|
return
|
|
logger.log("[ZIPLINE]: send App Start")
|
|
packet = struct.pack('<II', app_packet.SOLO_ZIPLINE_LOCK, 0)
|
|
self.shotmgr.appMgr.sendPacket(packet)
|
|
|
|
|
|
def handlePacket(self, packetType, packetLength, packetValue):
|
|
'''handle incoming data from the client app'''
|
|
try:
|
|
if packetType == app_packet.SOLO_MESSAGE_LOCATION:
|
|
(lat, lon, alt) = struct.unpack('<ddf', packetValue)
|
|
logger.log("[ZIPLINE]: Location received from app: %f, %f, %f." %( lat, lon, alt ) )
|
|
# dont read alt from App - it has no way to set it from UI
|
|
self.addLocation(LocationGlobalRelative(lat, lon, self.roi.alt))
|
|
|
|
elif packetType == app_packet.SOLO_ZIPLINE_OPTIONS:
|
|
(self.cruiseSpeed, self.is3D, _camPointing) = struct.unpack('<fBB', packetValue)
|
|
logger.log( "[ZIPLINE]: Set cruise speed to %f"% (self.cruiseSpeed,))
|
|
logger.log( "[ZIPLINE]: Set 3D path %d"% (self.is3D,))
|
|
logger.log( "[ZIPLINE]: Cam pointing %d"% (_camPointing,))
|
|
self.setButtonMappings()
|
|
self.initCam(_camPointing)
|
|
if self.pathHandler:
|
|
self.pathHandler.setCruiseSpeed(self.cruiseSpeed)
|
|
|
|
elif packetType == app_packet.SOLO_ZIPLINE_LOCK:
|
|
self.setupZipline()
|
|
|
|
else:
|
|
return False
|
|
except Exception as e:
|
|
logger.log('[ZIPLINE]: Error handling packet. (%s)' % e)
|
|
return False
|
|
else:
|
|
return True
|
|
|
|
|
|
def addLocation(self, loc):
|
|
'''called by shot manager to set new ROI from App'''
|
|
# replaces our ROI
|
|
self.roi = loc
|
|
|
|
# send this ROI to the app
|
|
packet = struct.pack('<IIddf', app_packet.SOLO_MESSAGE_LOCATION, 20, loc.lat, loc.lon, loc.alt)
|
|
self.shotmgr.appMgr.sendPacket(packet)
|
|
|
|
|
|
def spotLock(self):
|
|
'''take the angle of the copter and lock onto a ground level target'''
|
|
if self.camPointing == SPOT_LOCK:
|
|
return
|
|
|
|
self.needsUpdate = True
|
|
|
|
# don't use a shallow angle resulting in massively distant ROIs
|
|
pitch = min(camera.getPitch(self.vehicle), SHALLOW_ANGLE_THRESHOLD)
|
|
|
|
# Get ROI for the vehicle to look at
|
|
spotLock = location_helpers.getSpotLock(self.vehicle.location.global_relative_frame, pitch, camera.getYaw(self.vehicle))
|
|
self.addLocation(spotLock)
|
|
|
|
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)
|
|
|
|
# init the open loop gimbal pointing vars
|
|
self.camYaw = camera.getYaw(self.vehicle)
|
|
self.camPitch = camera.getPitch(self.vehicle)
|
|
|
|
|
|
def manualPitch(self, channels):
|
|
if abs(channels[RAW_PADDLE]) > abs(channels[THROTTLE]):
|
|
value = channels[RAW_PADDLE]
|
|
else:
|
|
value = channels[THROTTLE]
|
|
|
|
self.camPitch += PITCH_SPEED * UPDATE_TIME * value
|
|
|
|
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] * 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)
|
|
|
|
|
|
def handleSpotLock(self, channels):
|
|
'''handle spot lock'''
|
|
# we rotate this value for re-pointing
|
|
dist = location_helpers.getDistanceFromPoints(self.vehicle.location.global_relative_frame, self.roi)
|
|
|
|
# rotate the ROI point
|
|
if abs(channels[YAW]) > 0:
|
|
self.needsUpdate = True
|
|
tmp = self.roi.alt
|
|
az = location_helpers.calcAzimuthFromPoints(self.vehicle.location.global_relative_frame, self.roi)
|
|
az += (channels[YAW] * YAW_NUDGE_SPEED * UPDATE_TIME)
|
|
newRoi = location_helpers.newLocationFromAzimuthAndDistance(self.vehicle.location.global_relative_frame, az, dist)
|
|
newRoi.alt = tmp
|
|
self.addLocation(newRoi)
|
|
|
|
self.updateROIAlt(channels[RAW_PADDLE])
|
|
|
|
# nothing to do if no user interaction
|
|
if not self.needsUpdate:
|
|
return
|
|
|
|
# clear update flag
|
|
self.needsUpdate = False
|
|
|
|
# Tell Gimbal ROI Location
|
|
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
|
|
self.roi.lat*1.E7,
|
|
self.roi.lon*1.E7,
|
|
self.roi.alt)
|
|
|
|
self.vehicle.send_mavlink(msg)
|
|
|
|
# moves an offset of the ROI altitude up or down
|
|
def updateROIAlt(self, rawPaddleValue):
|
|
# no gimbal, no reason to change ROI
|
|
if self.vehicle.mount_status[0] == None:
|
|
return
|
|
|
|
if abs(rawPaddleValue) > MIN_PADDLE_THRESHOLD:
|
|
self.roi.alt += (rawPaddleValue * FOLLOW_ALT_NUDGE_SPEED * UPDATE_TIME)
|
|
self.roi.alt = min(self.vehicle.location.global_relative_frame.alt + ROI_ALT_MARGIN, self.roi.alt)
|
|
self.roi.alt = max(self.vehicle.location.global_relative_frame.alt - ROI_ALT_MAX_DELTA, self.roi.alt)
|
|
|