mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 22:24:32 +02:00
424 lines
18 KiB
Python
424 lines
18 KiB
Python
#
|
|
# Implement cable cam using DroneAPI
|
|
# This is called from shotManager running in MavProxy
|
|
# As of Solo version 2.0.0 this shot is no longer supported. It is replaced with multipoint.py.
|
|
# It is preserved here for compatibility with old versions of the app.
|
|
#
|
|
from dronekit.lib import VehicleMode
|
|
from dronekit.lib import LocationGlobal
|
|
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 yawPitchOffsetter
|
|
# on host systems these files are located here
|
|
sys.path.append(os.path.realpath('../../flightcode/stm32'))
|
|
from sololink import btn_msg
|
|
|
|
# in degrees per second
|
|
YAW_SPEED = 120.0
|
|
|
|
# cm / second
|
|
DEFAULT_WPNAV_ACCEL_VALUE = 100.0
|
|
|
|
logger = shotLogger.logger
|
|
|
|
class Waypoint():
|
|
def __init__(self, loc, yaw, pitch):
|
|
self.loc = loc
|
|
self.yaw = yaw
|
|
self.pitch = pitch
|
|
if self.pitch is None:
|
|
self.pitch = 0.0
|
|
|
|
|
|
class CableCamShot():
|
|
def __init__(self, vehicle, shotmgr):
|
|
self.vehicle = vehicle
|
|
self.shotmgr = shotmgr
|
|
self.waypoints = []
|
|
self.totalDistance = 0.0
|
|
self.yawPitchOffsetter = yawPitchOffsetter.YawPitchOffsetter()
|
|
self.pathHandler = None
|
|
# this is the total yaw between the recording of point 1 and point 2
|
|
self.aggregateYaw = 0.0
|
|
self.lastYaw = 0.0
|
|
|
|
# Cable cam options - set from the app
|
|
# cam interpolation is whether to do camera interpolation or not
|
|
self.camInterpolation = 1
|
|
# yaw direction is which way direction to turn while on the cable
|
|
# (from endpoint 0 to 1)
|
|
# This is automatically set by seeing how the user yaws while flying
|
|
# but it can be overridden by the app
|
|
self.yawDirection = 0
|
|
|
|
# use this to perform dead reckoning
|
|
self.lastPerc = 0.0
|
|
self.deadReckoningTicks = 0
|
|
|
|
self.accel = shotmgr.getParam( "WPNAV_ACCEL", DEFAULT_WPNAV_ACCEL_VALUE ) / 100.0
|
|
logger.log("[cable cam]: Retrieved WPNAV_ACCEL: %f" % (self.accel * 100.0))
|
|
self.desiredSpeed = 0.0
|
|
|
|
# set this to True once our targeting is set up
|
|
self.haveSetupTargeting = False
|
|
|
|
# channels are expected to be floating point values in the (-1.0, 1.0) range
|
|
def handleRCs( self, channels ):
|
|
if len(self.waypoints) < 2:
|
|
# if we've already recorded one waypoint, start
|
|
# trying to remember the intended rotation direction here
|
|
currentYaw = camera.getYaw(self.vehicle)
|
|
yawDiff = currentYaw - self.lastYaw
|
|
if yawDiff > 180.0:
|
|
yawDiff -= 360.0
|
|
elif yawDiff < -180.0:
|
|
yawDiff += 360.0
|
|
self.aggregateYaw += yawDiff
|
|
self.lastYaw = currentYaw
|
|
return
|
|
|
|
self.desiredSpeed, goingForwards = self.pathHandler.MoveTowardsEndpt(channels)
|
|
|
|
# the moment we enter guided for the first time, setup our targeting
|
|
# For some as yet unknown reason, this still doesn't work unless we call
|
|
# InterpolateCamera a few times, which is the reason for the
|
|
# not self.haveSetupTargeting check below
|
|
if not self.haveSetupTargeting and self.vehicle.mode.name == "GUIDED" and self.pathHandler.curTarget:
|
|
self.setupTargeting()
|
|
self.haveSetupTargeting = True
|
|
logger.log("[cable cam]: did initial setup of targeting")
|
|
|
|
self.yawPitchOffsetter.Update(channels)
|
|
|
|
if self.camInterpolation > 0 or not self.haveSetupTargeting:
|
|
# because we flipped cable cam around, we actually need to flip goingForwards
|
|
self.InterpolateCamera(not goingForwards)
|
|
else:
|
|
self.handleFreePitchYaw()
|
|
|
|
if self.pathHandler.isNearTarget():
|
|
self.pathHandler.currentSpeed = 0.0
|
|
self.pathHandler.pause()
|
|
self.updateAppOptions()
|
|
|
|
def recordLocation(self):
|
|
degreesYaw = camera.getYaw(self.vehicle)
|
|
pitch = camera.getPitch(self.vehicle)
|
|
|
|
# don't allow two waypoints near each other
|
|
if len(self.waypoints) == 1:
|
|
if location_helpers.getDistanceFromPoints3d(self.waypoints[0].loc, self.vehicle.location.global_relative_frame) < WAYPOINT_NEARNESS_THRESHOLD:
|
|
logger.log("[cable cam]: attempted to record a point too near the first point")
|
|
# update our waypoint in case yaw, pitch has changed
|
|
self.waypoints[0].yaw = degreesYaw
|
|
self.waypoints[0].pitch = pitch
|
|
|
|
# force the app to exit and restart cable cam
|
|
packet = struct.pack('<IIi', app_packet.SOLO_MESSAGE_GET_CURRENT_SHOT, 4, shots.APP_SHOT_NONE)
|
|
self.shotmgr.appMgr.sendPacket(packet)
|
|
packet = struct.pack('<IIi', app_packet.SOLO_MESSAGE_GET_CURRENT_SHOT, 4, shots.APP_SHOT_CABLECAM)
|
|
self.shotmgr.appMgr.sendPacket(packet)
|
|
packet = struct.pack('<IIddfff', app_packet.SOLO_CABLE_CAM_WAYPOINT,
|
|
28, self.vehicle.location.global_relative_frame.lat, self.vehicle.location.global_relative_frame.lon,
|
|
self.vehicle.location.global_relative_frame.alt, degreesYaw, pitch)
|
|
self.shotmgr.appMgr.sendPacket(packet)
|
|
return
|
|
|
|
# create a waypoint and add it to our list
|
|
waypt = Waypoint(self.vehicle.location.global_relative_frame, degreesYaw, pitch)
|
|
|
|
logger.log("[cable cam]: Recorded location %f, %f, %f. Yaw = %f" %
|
|
( self.vehicle.location.global_relative_frame.lat, self.vehicle.location.global_relative_frame.lon,
|
|
self.vehicle.location.global_relative_frame.alt, degreesYaw))
|
|
logger.log("[cable cam]: gimbal pitch is " + str(pitch))
|
|
|
|
self.waypoints.append(waypt)
|
|
self.setButtonMappings()
|
|
|
|
# send this waypoint to the app
|
|
packet = struct.pack('<IIddfff', app_packet.SOLO_CABLE_CAM_WAYPOINT,
|
|
28, self.vehicle.location.global_relative_frame.lat, self.vehicle.location.global_relative_frame.lon,
|
|
self.vehicle.location.global_relative_frame.alt, degreesYaw, pitch)
|
|
self.shotmgr.appMgr.sendPacket(packet)
|
|
|
|
#start monitoring heading changes
|
|
if len(self.waypoints) == 1:
|
|
self.aggregateYaw = 0.0
|
|
self.lastYaw = degreesYaw
|
|
elif len(self.waypoints) == 2:
|
|
# Now change the vehicle into guided mode
|
|
self.vehicle.mode = VehicleMode("GUIDED")
|
|
|
|
logger.log("[cable cam]: Got second cable point. Should be in guided %s" % (str(self.vehicle.mode)))
|
|
|
|
self.totalDistance = location_helpers.getDistanceFromPoints3d(
|
|
self.waypoints[0].loc, self.waypoints[1].loc)
|
|
|
|
# handle the 0-360 border
|
|
if self.waypoints[1].yaw - self.waypoints[0].yaw > 180.0:
|
|
self.waypoints[0].yaw += 360.0
|
|
elif self.waypoints[1].yaw - self.waypoints[0].yaw < -180.0:
|
|
self.waypoints[1].yaw += 360.0
|
|
|
|
#disregard aggregate yaw if it's less than 180
|
|
if abs(self.aggregateYaw) < 180.0:
|
|
self.aggregateYaw = self.waypoints[1].yaw - self.waypoints[0].yaw
|
|
|
|
self.yawDirection = 1 if self.aggregateYaw > 0.0 else 0
|
|
|
|
logger.log("[cable cam]: Aggregate yaw = %f. Yaw direction saved as %s" % (self.aggregateYaw, "CCW" if self.yawDirection == 1 else "CW"))
|
|
|
|
# send this yawDir to the app
|
|
self.updateAppOptions()
|
|
self.pathHandler = pathHandler.TwoPointPathHandler( self.waypoints[1].loc, self.waypoints[0].loc, self.vehicle, self.shotmgr )
|
|
|
|
def InterpolateCamera(self, goingForwards):
|
|
# Current request is to interpolate yaw and gimbal pitch separately
|
|
# This can result in some unsmooth motion, but maybe it's good enough.
|
|
# Alternatives would include slerp, but we're not interested in the shortest path.
|
|
|
|
# first, see how far along the path we are
|
|
dist = location_helpers.getDistanceFromPoints3d(self.waypoints[0].loc, self.vehicle.location.global_relative_frame)
|
|
if self.totalDistance == 0.0:
|
|
perc = 0.0
|
|
else:
|
|
perc = dist / self.totalDistance
|
|
|
|
# offset perc using dead reckoning
|
|
if self.lastPerc == perc:
|
|
self.deadReckoningTicks += 1
|
|
|
|
# adjust our last seen velocity based on WPNAV_ACCEL
|
|
DRspeed = self.vehicle.groundspeed
|
|
|
|
timeElapsed = self.deadReckoningTicks * UPDATE_TIME
|
|
|
|
# one problem here is we don't simulate deceleration when reaching the endpoints
|
|
if self.desiredSpeed > DRspeed:
|
|
DRspeed += (timeElapsed * self.accel)
|
|
if DRspeed > self.desiredSpeed:
|
|
DRspeed = self.desiredSpeed
|
|
else:
|
|
DRspeed -= (timeElapsed * self.accel)
|
|
if DRspeed < self.desiredSpeed:
|
|
DRspeed = self.desiredSpeed
|
|
|
|
if self.totalDistance > 0.0:
|
|
percSpeed = DRspeed * timeElapsed / self.totalDistance
|
|
|
|
#logger.log("same location, dead reckoning with groundspeed: %f"%(DRspeed))
|
|
|
|
# and adjust perc
|
|
if goingForwards:
|
|
perc += percSpeed
|
|
else:
|
|
perc -= percSpeed
|
|
|
|
else:
|
|
self.deadReckoningTicks = 0
|
|
self.lastPerc = perc
|
|
# logger.log("NEW location, resetting dead reckoning. Groundspeed: %f"%(self.vehicle.groundspeed))
|
|
|
|
if perc < 0.0:
|
|
perc = 0.0
|
|
elif perc > 1.0:
|
|
perc = 1.0
|
|
|
|
invPerc = 1.0 - perc
|
|
|
|
yawPt0 = self.waypoints[0].yaw
|
|
yawPt1 = self.waypoints[1].yaw
|
|
|
|
# handle if we're going other than the shortest dir
|
|
if yawPt1 > yawPt0 and self.yawDirection == 0:
|
|
yawPt0 += 360.0
|
|
elif yawPt0 > yawPt1 and self.yawDirection == 1:
|
|
yawPt1 += 360.0
|
|
|
|
# interpolate vehicle yaw and gimbal pitch
|
|
newYaw = perc * yawPt1 + invPerc * yawPt0
|
|
|
|
# add in yaw offset
|
|
newYaw += self.yawPitchOffsetter.yawOffset
|
|
|
|
# since we possibly added 360 to either waypoint, correct for it here
|
|
newYaw %= 360.0
|
|
# print "point 0 yaw: " + str(self.waypoints[0].yaw) + " point 1 yaw: " + str(self.waypoints[1].yaw) + " interpolated yaw = " + str(newYaw) + " at position " + str(perc)
|
|
newPitch = perc * self.waypoints[1].pitch + invPerc * self.waypoints[0].pitch
|
|
newPitch += self.yawPitchOffsetter.pitchOffset
|
|
|
|
if newPitch >= 0:
|
|
newPitch = 0
|
|
elif newPitch <= -90:
|
|
newPitch = -90
|
|
|
|
# if we do have a gimbal, mount_control it
|
|
if self.vehicle.mount_status[0] is not None:
|
|
msg = self.vehicle.message_factory.mount_control_encode(
|
|
0, 1, # target system, target component
|
|
newPitch * 100, # pitch is in centidegrees
|
|
0.0, # roll
|
|
newYaw * 100, # yaw is in centidegrees
|
|
0 # save position
|
|
)
|
|
else:
|
|
# if we don't have a gimbal, just set CONDITION_YAW
|
|
msg = self.vehicle.message_factory.command_long_encode(
|
|
0, 0, # target system, target component
|
|
mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
|
|
0, #confirmation
|
|
newYaw, # param 1 - target angle
|
|
YAW_SPEED, # param 2 - yaw speed
|
|
self.yawDirection, # param 3 - direction
|
|
0.0, # relative offset
|
|
0, 0, 0 # params 5-7 (unused)
|
|
)
|
|
|
|
self.vehicle.send_mavlink(msg)
|
|
|
|
# if we can handle the button we do
|
|
def handleButton(self, button, event):
|
|
if button == btn_msg.ButtonA and event == btn_msg.Press:
|
|
if len(self.waypoints) == 0:
|
|
self.recordLocation()
|
|
if button == btn_msg.ButtonB and event == btn_msg.Press:
|
|
if len(self.waypoints) == 1:
|
|
self.recordLocation()
|
|
if button == btn_msg.ButtonLoiter and event == btn_msg.Press:
|
|
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)
|
|
|
|
# pass in the value portion of the SOLO_CABLE_CAM_OPTIONS packet
|
|
def handleOptions(self, options):
|
|
oldInterp = self.camInterpolation
|
|
|
|
(self.camInterpolation, self.yawDirection, cruiseSpeed) = options
|
|
|
|
logger.log( "[cable cam]: Set cable cam options to interpolation: %d, \
|
|
yawDir: %d, cruising speed %f"%(self.camInterpolation, self.yawDirection, cruiseSpeed))
|
|
|
|
# don't handle these if we're not ready
|
|
if not self.pathHandler:
|
|
return
|
|
|
|
# pause or set new cruise speed
|
|
if cruiseSpeed == 0.0:
|
|
self.pathHandler.pause()
|
|
else:
|
|
self.pathHandler.setCruiseSpeed(cruiseSpeed)
|
|
|
|
# change to a different targeting mode
|
|
if oldInterp != self.camInterpolation:
|
|
self.setupTargeting()
|
|
|
|
|
|
def setButtonMappings(self):
|
|
buttonMgr = self.shotmgr.buttonManager
|
|
|
|
if len( self.waypoints ) == 0:
|
|
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_CABLECAM, btn_msg.ARTOO_BITMASK_ENABLED, "Record Point\0")
|
|
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_CABLECAM, 0, "\0")
|
|
elif len( self.waypoints ) == 1:
|
|
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_CABLECAM, 0, "\0")
|
|
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_CABLECAM, btn_msg.ARTOO_BITMASK_ENABLED, "Record Point\0")
|
|
else:
|
|
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_CABLECAM, 0, "\0")
|
|
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_CABLECAM, 0, "\0")
|
|
|
|
# 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('<IIHHf', app_packet.SOLO_CABLE_CAM_OPTIONS,
|
|
8, self.camInterpolation, self.yawDirection, speed)
|
|
self.shotmgr.appMgr.sendPacket(packet)
|
|
|
|
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.shotmgr.rcMgr.enableRemapping( True )
|
|
logger.log("[cable cam]: setting gimbal to mavlink mode")
|
|
|
|
if self.camInterpolation > 0:
|
|
logger.log("[cable cam]: turning on camera interpolation")
|
|
self.yawPitchOffsetter.enableNudge()
|
|
else:
|
|
logger.log("[cable cam]: turning off camera interpolation")
|
|
self.yawPitchOffsetter.disableNudge( camera.getPitch(self.vehicle), camera.getYaw(self.vehicle))
|
|
|
|
self.vehicle.send_mavlink(msg)
|
|
|
|
"""
|
|
In the case that we don't have view lock on, we enable 'free' movement of
|
|
pitch/yaw. Previously, we allowed flight code to control yaw, but since
|
|
we want the throttle stick to control pitch, we need to handle it here.
|
|
Our yawPitchOffsetter will contain the absolute pitch/yaw we want, so
|
|
this function just needs to set them.
|
|
"""
|
|
def handleFreePitchYaw(self):
|
|
# 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.yawPitchOffsetter.pitchOffset * 100, # pitch is in centidegrees
|
|
0.0, # roll
|
|
self.yawPitchOffsetter.yawOffset * 100, # yaw is in centidegrees
|
|
0 # save position
|
|
)
|
|
else:
|
|
# if we don't have a gimbal, just set CONDITION_YAW
|
|
msg = self.vehicle.message_factory.command_long_encode(
|
|
0, 0, # target system, target component
|
|
mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
|
|
0, #confirmation
|
|
self.yawPitchOffsetter.yawOffset, # param 1 - target angle
|
|
YAW_SPEED, # param 2 - yaw speed
|
|
self.yawPitchOffsetter.yawDir, # param 3 - direction
|
|
0.0, # relative offset
|
|
0, 0, 0 # params 5-7 (unused)
|
|
)
|
|
|
|
self.vehicle.send_mavlink(msg)
|
|
|
|
def handlePacket(self, packetType, packetLength, packetValue):
|
|
try:
|
|
if packetType == app_packet.SOLO_RECORD_POSITION:
|
|
self.recordLocation()
|
|
|
|
elif packetType == app_packet.SOLO_CABLE_CAM_OPTIONS:
|
|
options = struct.unpack('<HHf', packetValue)
|
|
self.handleOptions(options)
|
|
|
|
else:
|
|
return False
|
|
except Exception as e:
|
|
logger.log('[cable cam]: Error handling packet. (%s)' % e)
|
|
return False
|
|
else:
|
|
return True |