def generateSplines(self):
'''Generate the multi-point spline'''
logger.log("[Rewind] generateSplines")
# store the Lat,lon,alt locations
ctrlPtsLLA = []
# store vectors for relative offsets
ctrlPtsCart = []
# set initial control point as origin
ctrlPtsCart.append(Vector3(0, 0, 0))
# try and load a point
loc = self.rewindManager.queueNextloc()
if loc is None:
return False
logger.log("[Rewind] read loc: %f %f %f" % (loc.lat, loc.lon, loc.alt))
ctrlPtsLLA.append(loc)
# store as spline origin
self.splineOrigin = ctrlPtsLLA[0]
# read all available locations
while (loc is not None):
loc = self.rewindManager.queueNextloc()
if loc is not None:
logger.log("[Rewind] read loc: %f %f %f" %
(loc.lat, loc.lon, loc.alt))
ctrlPtsLLA.append(loc)
else:
print "loc: None"
# try and have a 3 point spline or longer:
if len(ctrlPtsLLA) < 2:
return False
# Save offsets from home for spline
for n in range(1, len(ctrlPtsLLA)):
ctrlPtsCart.append(
location_helpers.getVectorFromPoints(self.splineOrigin,
ctrlPtsLLA[n]))
ctrlPtsCart[-1].z *= -1. #NED
# Construct spline object
try:
self.cable = cableController.CableController(
points=ctrlPtsCart,
maxSpeed=REWIND_SPEED,
minSpeed=REWIND_MIN_SPEED,
tanAccelLim=TANGENT_ACCEL_LIMIT,
normAccelLim=NORM_ACCEL_LIMIT,
smoothStopP=0.7,
maxAlt=400)
except ValueError, e:
logger.log("%s", e)
return False
def setUp(self):
points = [
Vector3(0, 0, 0),
Vector3(1, 0, 0),
Vector3(2, 0, 0),
Vector3(3, 0, 0)
]
maxSpeed = MAX_SPEED
minSpeed = MIN_CRUISE_SPEED
tanAccelLim = TANGENT_ACCEL_LIMIT
normAccelLim = NORM_ACCEL_LIMIT
smoothStopP = 0.7
maxAlt = 50
self.controller = cableController.CableController(
points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP,
maxAlt)
self.controller.killCurvatureMapThread() #send thread poison pill
self.controller.curvatureMapThread.join() #wait for thread to die
class MultipointShot():
def __init__(self, vehicle, shotmgr):
# assign vehicle object
self.vehicle = vehicle
# assign shotmanager object
self.shotmgr = shotmgr
# initialize YawPitchOffsetter object
self.yawPitchOffsetter = yawPitchOffsetter.YawPitchOffsetter()
# enable yaw nudge by default
self.yawPitchOffsetter.enableNudge()
# initializes/resets most member vars
self.resetShot()
# get current altitude limit
self.maxAlt = self.shotmgr.getParam("FENCE_ALT_MAX",
DEFAULT_FENCE_ALT_MAX) # in meters
logger.log("[Multipoint]: Maximum altitude stored: %f" % self.maxAlt)
# check APM params to see if Altitude Limit should apply
if self.shotmgr.getParam("FENCE_ENABLE", DEFAULT_FENCE_ENABLE) == 0:
self.maxAlt = None
logger.log("[Multipoint]: Altitude Limit disabled.")
def resetShot(self):
'''initializes/resets most member vars'''
# camInterpolation == 1: free look
# camInterpolation == 0: tween pitch/yaw between keyframes
self.camInterpolation = 0
# True - Play mode, False - Record mode
self.cableCamPlaying = False
# initialize waypoints list
self.waypoints = []
# stores list of which direction to yaw while on the cable
self.yawDirections = []
# desired speed set by sticks or cruise speed
self.desiredSpeed = 0.0
# active cruise speed being used by controller
self.cruiseSpeed = 0
# stored cruise speed set from app
self.storedCruiseSpeed = 0
# current state of cruise (-1 = moving left, 0 = paused, 1 = moving right)
self.cruiseState = PAUSED
# declare camera spline object
self.camSpline = None
# initialize commanded velocity
self.commandVel = None
# initialize commanded position
self.commandPos = None
# flag that indicates whether we are in the attaching state
self.attaching = True
# last speed that was reported to the app for visualization purposes
self.lastNotifiedSpeed = 0.0
# last segment that was reported to the app for visualization purposes
self.lastNotifiedSegment = None
# Index to attach on spline during a cable load
self.attachIndex = -1
# ticks to track when to notify app with playback status update
self.ticksSinceNotify = 0
# default targetP
self.targetP = 0.0
# initialize cable to None
self.cable = None
# solo spline point version
self.splinePointVersion = 0
# absolute altitude reference
self.absAltRef = None
# initialize min/max times
self.maxTime = None
self.minTime = None
# last time that the controller was advanced
self.lastTime = None
def handleRCs(self, channels):
'''Handles RC inputs and runs the high level shot controller'''
# channels are expected to be floating point values
# in the (-1.0, 1.0) range don't enter the guided shot
# mode until the user has recorded the cable endpoints
# block controller from running until we enter play mode
if not self.cableCamPlaying:
return
# if we are still attaching to the spline
if self.attaching:
self.listenForAttach()
return
# determine if we should send a PLAYBACK_STATUS update to app
self.checkToNotifyApp()
# select the larger of the two stick values (between pitch and roll)
if abs(channels[ROLL]) > abs(channels[PITCH]):
value = channels[ROLL]
else:
value = -channels[PITCH]
# Check if we're in a cruise mode or not
if self.cruiseSpeed == 0:
# scale max speed by stick value
self.desiredSpeed = value * MAX_SPEED
else:
speed = abs(self.cruiseSpeed)
# if sign of stick and cruiseSpeed don't match then...
# slow down
if math.copysign(1, value) != math.copysign(1, self.cruiseSpeed):
speed *= (1.0 - abs(value))
else: # speed up
speed += (MAX_SPEED - speed) * abs(value)
# speedlimit
if speed > MAX_SPEED:
speed = MAX_SPEED
# carryover user input sign
if self.cruiseSpeed < 0:
speed = -speed
# assign to desired velocity
self.desiredSpeed = speed
if self.desiredSpeed > 0:
self.targetP = 1.0
elif self.desiredSpeed < 0:
self.targetP = 0.0
self.cable.setTargetP(self.targetP)
# give cable controller our desired speed
self.cable.trackSpeed(abs(self.desiredSpeed))
# advance cable controller by dt (time elapsed)
now = monotonic.monotonic()
if self.lastTime is None:
dt = UPDATE_TIME
else:
dt = now - self.lastTime
self.lastTime = now
self.cable.update(dt)
# add NED position vector to spline origin (addVectorToLocation needs NEU)
self.commandPos = location_helpers.addVectorToLocation(
self.splineOrigin,
Vector3(self.cable.position.x, self.cable.position.y,
-self.cable.position.z))
# assign velocity from controller
self.commandVel = self.cable.velocity
# formulate mavlink message for pos-vel controller
posVelMsg = self.vehicle.message_factory.set_position_target_global_int_encode(
0, # time_boot_ms (not used)
0,
1, # target system, target component
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame
0b0000110111000000, # type_mask - enable pos/vel
int(self.commandPos.lat * 10000000), # latitude (degrees*1.0e7)
int(self.commandPos.lon * 10000000), # longitude (degrees*1.0e7)
self.commandPos.alt, # altitude (meters)
self.commandVel.x,
self.commandVel.y,
self.commandVel.z, # North, East, Down velocity (m/s)
0,
0,
0, # x, y, z acceleration (not used)
0,
0) # yaw, yaw_rate (not used)
# send pos-vel command to vehicle
self.vehicle.send_mavlink(posVelMsg)
if self.camInterpolation == 0:
# calculate interpolated pitch & yaw
newPitch, newYaw = self.interpolateCamera()
else:
# free look pitch and yaw
newPitch, newYaw = (0.0, 0.0)
# calculate nudge offset or free-look offset
self.yawPitchOffsetter.Update(channels)
# add offsets
newPitch += self.yawPitchOffsetter.pitchOffset
newYaw += self.yawPitchOffsetter.yawOffset
# Log whatever we're sending for yaw for troubleshooting
# try:
# handleRCs.bullshitCounter += 1
# except AttributeError:
# handleRCs.bullshitCounter = 0
# if bullshitCounter > 200:
# self.bullshitCounter = 0
# logger.log("[multipoint]: MAV_CMD_DO_MOUNT_CONTROL yaw param: %f" %newYaw)
# Formulate mavlink message for mount controller. Use mount_control if using a gimbal. Use just condition_yaw if no gimbal.
# Modified by Matt 2017-05-18 for ArducCopter master compatibility
# mav_cmd_do_mount_control should handle gimbal pitch and copter yaw, but yaw is not working in master.
# So this mount_control command is only going to do the gimbal pitch for now.
if self.vehicle.mount_status[0] is not None:
pointingMsg = self.vehicle.message_factory.command_long_encode(
0,
0, # target system, target component
mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, # command
0, # confirmation
newPitch * 100, # param1: Pitch in centidegrees
0, # param2: Roll (obviously not used)
0, # param3: Yaw in centidegrees (not working, so using condition_yaw below)
0,
0,
0, # param 4-6 not used
mavutil.mavlink.
MAV_MOUNT_MODE_MAVLINK_TARGETING # param7: MAV_MOUNT_MODE
)
self.vehicle.send_mavlink(pointingMsg)
# Temporary fix while the yaw control is not working in the above mount_control command.
pointingMsg = 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 (degrees)
YAW_SPEED, # param 2 - yaw speed (deg/s)
0, # param 3 - direction, always shortest route for now...
0.0, # relative offset
0,
0,
0 # params 5-7 (unused)
)
self.vehicle.send_mavlink(pointingMsg)
# if no gimbal, assume fixed mount and use condition_yaw only
else:
# if we don't have a gimbal, just set CONDITION_YAW
pointingMsg = 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 (degrees)
YAW_SPEED, # param 2 - yaw speed (deg/s)
0, # param 3 - direction, always shortest route for now...
0.0, # relative offset
0,
0,
0 # params 5-7 (unused)
)
self.vehicle.send_mavlink(pointingMsg)
def interpolateCamera(self):
'''Interpolate (linear) pitch and yaw between cable control points'''
perc = self.cable.currentU
# sanitize perc
if perc < 0.0:
perc = 0.0
elif perc > 1.0:
perc = 1.0
# get pitch and yaw from spline (x is pitch, y is yaw)
pitchYaw = self.camSpline.position(self.cable.currentSeg, perc)
return pitchYaw.x, location_helpers.wrapTo360(pitchYaw.y)
def recordLocation(self):
'''record a cable control point'''
# don't run this function if we're already in the cable
if self.cableCamPlaying:
logger.log(
"[multipoint]: Cannot record a location when in PLAY mode.")
return
# record HOME absolute altitude reference if this is the first waypoint
if len(self.waypoints) == 0:
self.absAltRef = self.vehicle.location.global_frame.alt - self.vehicle.location.global_relative_frame.alt
logger.log(
"[multipoint]: HOME absolute altitude recorded: %f meters" %
self.absAltRef)
# capture current pitch and yaw state
pitch = camera.getPitch(self.vehicle)
degreesYaw = location_helpers.wrapTo360(camera.getYaw(self.vehicle))
# check if last recorded point is a duplicate with this new one
if self.duplicateCheck(self.vehicle.location.global_relative_frame,
len(self.waypoints)):
logger.log("[multipoint]: Overwriting last point.")
# overwrite last point
self.waypoints[
-1].loc = self.vehicle.location.global_relative_frame
self.waypoints[-1].pitch = pitch
self.waypoints[-1].yaw = degreesYaw
else:
# append a new point
self.waypoints.append(
Waypoint(self.vehicle.location.global_relative_frame, pitch,
degreesYaw))
# log this point
logger.log(
"[multipoint]: Successfully recorded waypoint #%d. Lat: %f, Lon: %f, Alt: %f, Pitch: %f, Yaw: %f"
% (len(self.waypoints) - 1,
self.vehicle.location.global_relative_frame.lat,
self.vehicle.location.global_relative_frame.lon,
self.vehicle.location.global_relative_frame.alt, pitch,
degreesYaw))
# send the spline point to the app
self.sendSoloSplinePoint(
self.splinePointVersion, self.absAltRef,
len(self.waypoints) - 1,
self.vehicle.location.global_relative_frame.lat,
self.vehicle.location.global_relative_frame.lon,
self.vehicle.location.global_relative_frame.alt, pitch, degreesYaw,
0, app_packet.SPLINE_ERROR_NONE)
logger.log("[multipoint]: Sent spline point #%d to app." %
(len(self.waypoints) - 1))
# update button mappings
self.setButtonMappings()
def loadSplinePoint(self, point):
'''load a cable control point'''
(version, absAltRef, index, lat, lon, alt, pitch, yaw, uPosition,
app_packet.SPLINE_ERROR_MODE) = point
if self.cableCamPlaying:
logger.log(
"[multipoint]: Shot in PLAY mode, cannot load waypoint.")
self.sendSoloSplinePoint(version, absAltRef, index, lat, lon, alt,
pitch, yaw, uPosition,
app_packet.SPLINE_ERROR_MODE)
logger.log("[multipoint]: Sent failed spline point #%d to app." %
index)
return
if version == 0:
if self.cableCamPlaying:
logger.log(
"[multipoint]: Shot in PLAY mode, cannot load waypoint.")
self.sendSoloSplinePoint(version, absAltRef, index, lat, lon,
alt, pitch, yaw, uPosition,
app_packet.SPLINE_ERROR_MODE)
logger.log(
"[multipoint]: Sent failed spline point #%d to app." %
index)
return
if self.duplicateCheck(LocationGlobalRelative(lat, lon, alt),
index):
logger.log(
"[multipoint]: Duplicate detected, rejecting waypoint #%d."
% index)
self.sendSoloSplinePoint(version, absAltRef, index, lat, lon,
alt, pitch, yaw, uPosition,
app_packet.SPLINE_ERROR_DUPLICATE)
logger.log(
"[multipoint]: Sent failed spline point #%d to app." %
index)
return
# if this is the first loaded waypoint, store absolute altitude reference and version
if len(self.waypoints) == 0:
self.splinePointVersion = version
self.absAltRef = absAltRef
logger.log(
"[multipoint]: previous HOME absolute altitude loaded: %f meters"
% self.absAltRef)
else:
logger.log(
"[multipoint]: Spline point version (%d) not supported, cannot load waypoint."
% version)
return
# if loaded waypoint index is higher than current waypoint list size
# then extend waypoint list to accomodate
if (index + 1) > len(self.waypoints):
self.waypoints.extend([None] * (index + 1 - len(self.waypoints)))
# store received waypoint
self.waypoints[index] = Waypoint(LocationGlobalRelative(lat, lon, alt),
pitch, yaw)
# log waypoint
logger.log(
"[multipoint]: Successfully loaded waypoint #%d. Lat: %f, Lon: %f, Alt: %f, Pitch: %f, Yaw: %f"
% (index, lat, lon, alt, pitch, yaw))
# send the spline point to the app
self.sendSoloSplinePoint(self.splinePointVersion, self.absAltRef,
index, lat, lon, alt, pitch, yaw, uPosition,
app_packet.SPLINE_ERROR_NONE)
logger.log("[multipoint]: Sent spline point #%d to app." % index)
# update button mappings
self.setButtonMappings()
def sendSoloSplinePoint(self, version, absAltReference, index, lat, lon,
alt, pitch, yaw, uPosition, status):
if version == 0:
packet = struct.pack('<IIhfIddffffh', app_packet.SOLO_SPLINE_POINT,
44, version, absAltReference, index, lat, lon,
alt, pitch, yaw, uPosition, status)
self.shotmgr.appMgr.sendPacket(packet)
def duplicateCheck(self, loc, desiredIndex):
'''Checks for duplicate waypoints
loc - desired waypoint location
desiredIndex - index that we would like to add the waypoint to in self.waypoints
'''
isDuplicate = False
# check point on the right
if desiredIndex < len(self.waypoints) - 1:
if self.waypoints[desiredIndex + 1] is not None:
if location_helpers.getDistanceFromPoints3d(
self.waypoints[desiredIndex + 1].loc,
loc) < WAYPOINT_NEARNESS_THRESHOLD:
logger.log(
"[multipoint]: Duplicate waypoint detected. Conflicts with index %d."
% (desiredIndex + 1))
isDuplicate = True
# check point on the left
if 0 < desiredIndex <= len(self.waypoints):
if self.waypoints[desiredIndex - 1] is not None:
if location_helpers.getDistanceFromPoints3d(
self.waypoints[desiredIndex - 1].loc,
loc) < WAYPOINT_NEARNESS_THRESHOLD:
logger.log(
"[multipoint]: Duplicate waypoint detected. Conflicts with index %d."
% (desiredIndex - 1))
isDuplicate = True
return isDuplicate
def setButtonMappings(self):
buttonMgr = self.shotmgr.buttonManager
if len(self.waypoints) == 0:
buttonMgr.setArtooButton(btn_msg.ButtonA,
shots.APP_SHOT_MULTIPOINT,
btn_msg.ARTOO_BITMASK_ENABLED,
"Record Point\0")
buttonMgr.setArtooButton(btn_msg.ButtonB,
shots.APP_SHOT_MULTIPOINT, 0, "\0")
elif not self.cableCamPlaying:
buttonMgr.setArtooButton(btn_msg.ButtonA,
shots.APP_SHOT_MULTIPOINT,
btn_msg.ARTOO_BITMASK_ENABLED,
"Record Point\0")
buttonMgr.setArtooButton(btn_msg.ButtonB,
shots.APP_SHOT_MULTIPOINT,
btn_msg.ARTOO_BITMASK_ENABLED,
"Finish Point\0")
else:
buttonMgr.setArtooButton(btn_msg.ButtonA,
shots.APP_SHOT_MULTIPOINT, 0, "\0")
buttonMgr.setArtooButton(btn_msg.ButtonB,
shots.APP_SHOT_MULTIPOINT, 0, "\0")
def handleButton(self, button, event):
'''handle actions for button presses'''
if not self.cableCamPlaying:
if button == btn_msg.ButtonA and event == btn_msg.Press:
self.recordLocation()
if button == btn_msg.ButtonB and event == btn_msg.Press:
self.recordLocation()
# only try to start a cable if we have more than 2 points
if len(self.waypoints) > 1:
# initialize multiPoint cable
self.enterPlayMode()
# handle pause button
if button == btn_msg.ButtonLoiter and event == btn_msg.Press:
self.setCruiseSpeed(state=PAUSED)
logger.log("[multipoint]: Pause button pressed on Artoo.")
def handlePathSettings(self, pathSettings):
'''pass in the value portion of the SOLO_SPLINE_PATH_SETTINGS packet'''
(cameraMode, desiredTime) = pathSettings
# don't run this function if we're not in Play Mode
if not self.cableCamPlaying:
logger.log(
"[multipoint]: Can't set cable settings. Not in play mode.")
return
# change pointing modes
if cameraMode != self.camInterpolation:
if cameraMode == 0:
self.yawPitchOffsetter.enableNudge()
logger.log("[multipoint]: Entering InterpolateCam.")
self.camInterpolation = 0
elif cameraMode == 1:
self.yawPitchOffsetter.disableNudge(
camera.getPitch(self.vehicle), camera.getYaw(self.vehicle))
logger.log("[multipoint]: Entering FreeCam.")
self.camInterpolation = 1
else:
logger.log("[multipoint]: Camera mode not recognized (%d)." %
self.camInterpolation)
self.yawPitchOffsetter.enableNudge()
# calculate cruise speed from desiredTime
self.setCruiseSpeed(speed=self.estimateCruiseSpeed(desiredTime))
def setCruiseSpeed(self, speed=None, state=None):
'''Uses storedCruiseSpeed and cruiseState to assign a cruiseSpeed for the controller'''
# RIGHT == 1
# PAUSED == 0
# LEFT == -1
if speed is not None and speed != self.storedCruiseSpeed:
# limit it
if abs(speed) > MAX_SPEED:
speed = MAX_SPEED
self.storedCruiseSpeed = speed
logger.log("[multipoint]: New cruise speed stored: %f m/s." %
self.storedCruiseSpeed)
if state is not None and state != self.cruiseState:
self.cruiseState = state
logger.log("[multipoint]: New cruise state set: %d" %
self.cruiseState)
if self.cruiseState == RIGHT:
self.cruiseSpeed = self.storedCruiseSpeed * math.copysign(
1, self.cruiseState)
self.cable.setTargetP(1.0)
elif self.cruiseState == LEFT:
self.cruiseSpeed = self.storedCruiseSpeed * math.copysign(
1, self.cruiseState)
self.cable.setTargetP(0.0)
else:
self.cruiseSpeed = 0
def updatePathTimes(self):
'''Sends min and max path times to app'''
# don't run this function if we're not in Play Mode
if self.cable is None:
logger.log(
"[multipoint]: Can't estimate cable times. A spline hasn't been generated yet!"
)
return
# pack it up and send to app
packet = struct.pack('<IIff', app_packet.SOLO_SPLINE_DURATIONS, 8,
self.minTime, self.maxTime)
self.shotmgr.appMgr.sendPacket(packet)
logger.log("[multipoint]: Sent times to app.")
def updatePlaybackStatus(self):
if self.cable is None:
logger.log("[multipoint]: A spline hasn't been generated yet!")
return
if self.cable.reachedTarget():
reportP = self.targetP
self.setCruiseSpeed(state=PAUSED)
else:
reportP = self.cable.currentP
packet = struct.pack('<IIfi', app_packet.SOLO_SPLINE_PLAYBACK_STATUS,
8, reportP, self.cruiseState)
self.shotmgr.appMgr.sendPacket(packet)
def handleSeek(self, seek):
(p, cruiseState) = seek
if self.cable is None:
logger.log("[multipoint]: A spline hasn't been generated yet!")
return
if self.attaching:
logger.log(
"[multipoint]: Can't seek yet, Solo still attaching to cable.")
return
# update cruise state
self.setCruiseSpeed(state=cruiseState)
logger.log(
"[multipoint]: New SEEK packet received. p: %f, cruiseState: %d" %
(p, cruiseState))
# notify the app
self.checkToNotifyApp(notify=True)
def enterRecordMode(self):
logger.log("[multipoint]: Entering RECORD mode.")
# send record to app
packet = struct.pack('<II', app_packet.SOLO_SPLINE_RECORD, 0)
self.shotmgr.appMgr.sendPacket(packet)
logger.log("[multipoint]: Sent RECORD mode to app.")
# Change the vehicle into loiter mode
self.vehicle.mode = VehicleMode("LOITER")
# reset shot
self.resetShot()
def enterPlayMode(self):
logger.log("[multipoint]: Entering PLAY mode.")
if self.cableCamPlaying:
logger.log("[multipoint]: Already in PLAY mode.")
return
# make sure no waypoints are empty
if None in self.waypoints:
logger.log(
"[multipoint]: Missing at least one keyframe, reverting to RECORD mode."
)
self.enterRecordMode()
return
# make sure we have at least 2 waypoints recorded before continuing
if len(self.waypoints) < 2:
logger.log(
"[multipoint]: Tried to begin multipoint cable with less than 2 keyframes. Reverting to RECORD mode."
)
self.enterRecordMode()
return
# generate position and camera splines
if not self.generateSplines(self.waypoints):
logger.log(
"[multipoint]: Spline generation failed. Reverting to RECORD mode."
)
self.enterRecordMode()
return
# send play to app
packet = struct.pack('<II', app_packet.SOLO_SPLINE_PLAY, 0)
self.shotmgr.appMgr.sendPacket(packet)
logger.log("[multipoint]: Sent PLAY mode to app.")
# send path min/max times to app
self.updatePathTimes()
# send all waypoints to app with arc length normalized parameters
for index, pt in enumerate(self.waypoints):
# special case: handle last point
if index == len(self.waypoints) - 1:
seg = index - 1
u = 1
else:
seg = index
u = 0
# calculate p for point (segment usually at index, u = 0, v doesn't
# matter)
p = self.cable.spline.nonDimensionalToArclength(seg, u, 0)[0]
# send point packet to app
self.sendSoloSplinePoint(self.splinePointVersion, self.absAltRef,
index, pt.loc.lat, pt.loc.lon, pt.loc.alt,
pt.pitch, pt.yaw, p,
app_packet.SPLINE_ERROR_NONE)
logger.log("[multipoint]: Sent spline point %d of %d to app." %
(index, len(self.waypoints) - 1))
# Change the vehicle into guided mode
self.vehicle.mode = VehicleMode("GUIDED")
# set gimbal to MAVLink 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
)
# send gimbal targeting message to vehicle
self.vehicle.send_mavlink(msg)
# remap the sticks
self.shotmgr.rcMgr.enableRemapping(True)
# set state flag to true (partially unblocks controller)
# (also needs unblock of attaching flag)
self.cableCamPlaying = True
# update button mappings
self.setButtonMappings()
# wait for app to send an attach request
logger.log(
"[multipoint]: Waiting for ATTACH command from app to continue."
) # wait for a SPLINE_ATTACH message to tell us where to go
def generateSplines(self, waypoints):
'''Generate the multi-point spline'''
# shortest distance between each angle
for i in range(0, len(waypoints) - 1):
# calculate difference of yaw and next yaw
delta = self.waypoints[i + 1].yaw - self.waypoints[i].yaw
# don't take the long way around
if abs(delta) > 180:
# add 360 to all following yaws (CW)
if delta < 0.0:
direction = 1
# or remove 360 from all following yaws (CCW)
else:
direction = -1
# list tracking directions
self.yawDirections.append(direction)
# update all following yaws
for j in range(i + 1, len(self.waypoints)):
self.waypoints[
j].yaw = self.waypoints[j].yaw + direction * 360
# generate camera spline
camPts = [Vector2(x.pitch, x.yaw) for x in self.waypoints]
# Generate artificial end points for spline
# extend slope of first two points to generate initial control point
endPt1 = camPts[0] + (camPts[0] - camPts[1])
# extend slope of last two points to generate final control point
endPt2 = camPts[-1] + (camPts[-1] - camPts[-2])
# append virtual control points to cartesian list
camPts = [endPt1] + camPts + [endPt2]
# Build spline object
try:
self.camSpline = CatmullRom(camPts)
except ValueError, e:
logger.log("%s", e)
self.shotMgr.enterShot(shots.APP_SHOT_NONE) # exit the shot
return False
# generate 3d position spline
ctrlPtsLLA = [x.loc for x in self.waypoints]
ctrlPtsCart = []
# set initial control point as origin
ctrlPtsCart.append(Vector3(0, 0, 0))
self.splineOrigin = ctrlPtsLLA[0]
for n in range(1, len(ctrlPtsLLA)):
ctrlPtsCart.append(
location_helpers.getVectorFromPoints(self.splineOrigin,
ctrlPtsLLA[n]))
ctrlPtsCart[-1].z *= -1. #NED
# Build spline object
try:
self.cable = cableController.CableController(
points=ctrlPtsCart,
maxSpeed=MAX_SPEED,
minSpeed=MIN_CRUISE_SPEED,
tanAccelLim=TANGENT_ACCEL_LIMIT,
normAccelLim=NORM_ACCEL_LIMIT,
smoothStopP=0.7,
maxAlt=self.maxAlt)
except ValueError, e:
logger.log("%s", e)
self.shotMgr.enterShot(shots.APP_SHOT_NONE) # exit the shot
return False
