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 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 manualYaw(self, channels):
if channels[YAW] == 0:
return
self.camYaw += channels[YAW] * FOLLOW_YAW_SPEED * UPDATE_TIME
if channels[YAW] > 0:
self.camDir = 1
else:
self.camDir = -1
self.camYaw = location_helpers.wrapTo360(self.camYaw)
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 _approach(self,stick):
'''Handles approach stick input to grow or shrink orbit radius'''
# set to vehicle max speed
maxSpeed = MAX_SPEED
# Approach speed is controlled by user PITCH stick
speed = stick * maxSpeed
# Synthetic acceleration limit
if speed > self.approachSpeed:
self.approachSpeed += ACCEL_PER_TICK
self.approachSpeed = min(self.approachSpeed, speed)
elif speed < self.approachSpeed:
self.approachSpeed -= ACCEL_PER_TICK
self.approachSpeed = max(self.approachSpeed, speed)
else:
self.approachSpeed = speed
# update current radius
self.radius -= ( self.approachSpeed * UPDATE_TIME )
# az from vehicle to ROI
#az = location_helpers.calcAzimuthFromPoints(self.vehicle.location, self.roi)
az = location_helpers.wrapTo360(self.azimuth + 180.0)
az = math.radians( az )
# rotate vehicle from entering min radius
xVel = math.cos(az) * self.approachSpeed
yVel = math.sin(az) * self.approachSpeed
# stop vehicle from entering min radius
if self.radius < ORBIT_RAD_FOR_MIN_SPEED:
self.radius = ORBIT_RAD_FOR_MIN_SPEED
xVel = yVel = 0.0
return Vector3(xVel,yVel,0)
def move(self, channels, newroi=None, roiVel=None):
'''Handles RC channels to return a position and velocity command
location: location object, lat (deg), lon (deg), alt (meters)
velocity: vector3 object, x, y, z (m/s) command'''
# If we are inside of the circle, we overide the radius
# with our position to prevent sudden flight to or away from pilot
if self.approachSpeed != 0 and self.distance < self.radius:
self.radius = self.distance
if roiVel is None:
roiVel = Vector3(0, 0, 0)
# Approach
approachVel = self._approach(-channels[PITCH])
# Climb
climbVel = self._climb(channels[THROTTLE])
# ROI heading is calculated and flipped 180deg to determine leash angle
roiHeading = location_helpers.calcAzimuthFromPoints(self.roi, newroi)
roiHeading += 180
roiHeading = location_helpers.wrapTo360(roiHeading)
# roiSpeed is used to determine if we apply crosstrack error correction
roiSpeed = location_helpers.getDistanceFromPoints(self.roi,
newroi) / UPDATE_TIME
if roiSpeed < CROSSTRACK_MINSPEED or self.approachSpeed != 0:
# we are not moving very fast, don't crosstrack
# prevents swinging while we are still.
crosstrackGain = 0
else:
crosstrackGain = CROSSTRACK_GAIN
# Used to test are we inside the radius or on it?
# we use old ROI because current location is not set yet
# and wont be for the test function
self.distance = location_helpers.getDistanceFromPoints(
self.roi, self.currentLoc)
# overwrite old ROI with new
self.roi = newroi
# new Az from ROI to current position
self.azimuth = location_helpers.calcAzimuthFromPoints(
self.roi, self.currentLoc)
# angle error of ROI Heading vs Azimuth
headingError = roiHeading - self.azimuth
headingError = location_helpers.wrapTo180(headingError)
# used to determine if the copter in front of us or behind
angleErrorTest = abs(headingError)
# limit error
if headingError > 90:
headingError = 90
elif headingError < -90:
headingError = -90
if self.distance < (self.radius - 1) and self.approachSpeed == 0:
# we are inside the circle with a margin of error to prevent small jerks
# -1 on z is used as a dataflag for no desired velocity
currentVel = Vector3(0, 0, 0)
elif angleErrorTest > 90 and self.approachSpeed == 0:
# We are outside the circle
# We are walking toward copter
currentVel = Vector3(0, 0, 0)
else:
# Follow leash and manage crosstrack
# bring in the Az to match the ROI heading
crosstrack = headingError * crosstrackGain * UPDATE_TIME
crosstrack = min(crosstrack, CROSSTRACK_MAX)
# scale down the crosstracking with the distance (min 1m to avoid div/0)
self.azimuth += crosstrack / max(self.distance - 1, 1)
# for calculating velocity vector (unpack and then pack object to copy the values not the reference)
oldPos = LocationGlobalRelative(self.currentLoc.lat,
self.currentLoc.lon,
self.currentLoc.alt)
# get new location from Az and radius
self.currentLoc = location_helpers.newLocationFromAzimuthAndDistance(
self.roi, self.azimuth, self.radius)
# calc velocity to new position
currentVel = location_helpers.getVectorFromPoints(
oldPos, self.currentLoc)
# convert to speed
currentVel = (currentVel * UPDATE_TIME) + approachVel + roiVel
# Climb/descend in all cases, even if holding still translationally
currentVel += climbVel
# set altitude
self.currentLoc.alt = self.roi.alt + self.zOffset
# check altitude limit
if self.maxAlt is not None:
self.currentLoc.alt = min(self.currentLoc.alt, self.maxAlt)
return self.currentLoc, currentVel
def _strafe(self, stick, strafeSpeed = 0):
'''Handles strafe stick input and cruiseSpeed to strafe left or right on the circle'''
# get max speed at current radius
maxSpeed = self._maxStrafeSpeed(self.radius)
# cruiseSpeed can be permuted by user ROLL stick
if strafeSpeed == 0.0:
speed = stick * maxSpeed
else:
speed = abs(strafeSpeed)
# if sign of stick and cruiseSpeed don't match then...
if math.copysign(1,stick) != math.copysign(1,strafeSpeed): # slow down
speed *= (1.0 - abs(stick))
else: # speed up
speed += (maxSpeed - speed) * abs(stick)
# carryover user input sign
if strafeSpeed < 0:
speed = -speed
# limit speed
if speed > maxSpeed:
speed = maxSpeed
elif -speed > maxSpeed:
speed = -maxSpeed
# Synthetic acceleration limit
if speed > self.strafeSpeed:
self.strafeSpeed += ACCEL_PER_TICK
self.strafeSpeed = min(self.strafeSpeed, speed)
elif speed < self.strafeSpeed:
self.strafeSpeed -= ACCEL_PER_TICK
self.strafeSpeed = max(self.strafeSpeed, speed)
else:
self.strafeSpeed = speed
# circumference of current circle
circum = self.radius * 2.0 * math.pi
# # of degrees of our circle to shoot for moving to this update
# had problems when we made this too small
degrees = abs(self.strafeSpeed) * UPDATE_TIME / circum * 360.0
# rotate heading by the number of degrees/update
if self.strafeSpeed > 0:
self.azimuth -= degrees
else:
self.azimuth += degrees
# make sure we keep it 0-360
self.azimuth = location_helpers.wrapTo360(self.azimuth)
# generate a velocity tangent to our circle from our destination point
if self.strafeSpeed > 0:
tangent = self.azimuth - 90.0
else:
tangent = self.azimuth + 90.0
tangent = math.radians(tangent)
#based on that, construct a vector to represent what direction we should go in, scaled by our speed
xVel = math.cos(tangent) * abs(self.strafeSpeed)
yVel = math.sin(tangent) * abs(self.strafeSpeed)
return Vector3(xVel,yVel,0)
