def move(self, channels, cruiseSpeed, newroi=None, roiVel=None):
'''Handles RC channels and a given strafeSpeed 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 roiVel is None:
roiVel = Vector3(0,0,0)
# Approach
approachVel = self._approach(-channels[PITCH])
# Strafe
strafeVel = self._strafe(channels[ROLL], cruiseSpeed)
# Climb
climbVel = self._climb(channels[THROTTLE])
# calculate new LLA position
if newroi is not None:
self.roi = newroi
currentPos = location_helpers.newLocationFromAzimuthAndDistance(self.roi, self.azimuth, self.radius)
# set altitude (This is for generality with follow. In the Orbit shot, roi.alt should be zero.)
currentPos.alt = self.roi.alt + self.zOffset
# check altitude limit
if self.maxAlt is not None:
currentPos.alt = min(currentPos.alt, self.maxAlt)
# sum velocities
currentVel = approachVel + strafeVel + climbVel + roiVel
return currentPos, currentVel
def setUp(self):
self.v = mock.create_autospec(Vehicle)
self.v.location.global_relative_frame = LocationGlobalRelative(
-48.5468695, 5.68464, 10.5)
self.v.mount_status = [-80]
self.mockMgr = Mock()
self.mockMgr.buttonManager = Mock()
self.mockMgr.getParam = Mock(return_value=500.0)
self.controller = CableCamShot(self.v, self.mockMgr)
loc2 = location_helpers.newLocationFromAzimuthAndDistance(
self.v.location.global_relative_frame, 23.4, 25.0)
self.startYaw = 12.4
self.startPitch = -16.7
waypt1 = cable_cam.Waypoint(loc2, self.startYaw, self.startPitch)
self.controller.waypoints.append(waypt1)
self.endYaw = 175.4
self.endPitch = -83.4
waypt2 = cable_cam.Waypoint(self.v.location.global_relative_frame,
self.endYaw, self.endPitch)
self.controller.waypoints.append(waypt2)
self.controller.deadReckoningTicks = 0
self.controller.accel = 0.0
self.controller.totalDistance = location_helpers.getDistanceFromPoints3d(
self.v.location.global_relative_frame, loc2)
self.v.message_factory = Mock()
# turn off dead reckoning
self.v.groundspeed = 0.0
self.controller.desiredSpeed = 0.0
def __init__(self, roi, radius, azimuth, zOffset):
super(LeashController, self).__init__(roi, radius, azimuth, zOffset)
#initialize location
self.currentLoc = location_helpers.newLocationFromAzimuthAndDistance(
self.roi, self.azimuth, self.radius)
self.currentLoc.altitude = roi.alt + zOffset
self.distance = radius
def testSecondLocation(self):
""" Test that newLocationFromAzimuthAndDistance works """
az = 84.546
dist = 37.5464
loc = LocationGlobalRelative(-22.65465, 4.351654, 0.0)
newloc = location_helpers.newLocationFromAzimuthAndDistance(
loc, az, dist)
calcDist = location_helpers.getDistanceFromPoints3d(loc, newloc)
self.assertTrue(abs(dist - calcDist) < ERROR)
calcAz = location_helpers.calcAzimuthFromPoints(loc, newloc)
self.assertTrue(abs(az - calcAz) < ERROR)
def testFirstLocation(self):
""" Test that newLocationFromAzimuthAndDistance works """
az = 17.234
dist = 45.23643
loc = LocationGlobalRelative(-43.2346234, 15.2385, 0.0)
newloc = location_helpers.newLocationFromAzimuthAndDistance(
loc, az, dist)
calcDist = location_helpers.getDistanceFromPoints3d(loc, newloc)
self.assertTrue(abs(dist - calcDist) < ERROR)
calcAz = location_helpers.calcAzimuthFromPoints(loc, newloc)
self.assertTrue(abs(az - calcAz) < ERROR)
def setupWaypoints(self):
'''setup our two waypoints'''
# we want two points that are far apart
# both in the direction we're looking and the opposite direction
# get vehicle state
self.start_loc = self.vehicle.location.global_relative_frame
self.triggerDist = self.start_loc.alt * OVERLAP
self.triggerDist = max(self.triggerDist, MIN_TRIGGER_DIST)
# calculate the foward point
forwardPt = location_helpers.newLocationFromAzimuthAndDistance(self.start_loc, self.camYaw, self.transectLength)
# create a new pathHandler obejct with our new points
# this will automatically reset resumeSpeed, cruiseSpeed etc...
self.pathHandler = pathHandler.TwoPointPathHandler(self.start_loc, forwardPt, self.vehicle, self.shotmgr)
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)
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 handleRCs(self, channels):
# don't continue until an roi is set
if not self.pathHandler or not self.roi:
return
# allow user to rotate the ROI about the vehicle
if abs(channels[YAW]) > 0:
# adding 180 flips the az (ROI to vehicle) to (vehicle to ROI)
tmp = self.roi.alt
self.pathController.azimuth += channels[
YAW] * YAW_SPEED * UPDATE_TIME
self.roi = location_helpers.newLocationFromAzimuthAndDistance(
self.vehicle.location.global_relative_frame,
180 + self.pathController.azimuth, self.pathController.radius)
self.roi.alt = tmp
self.roi_needsUpdate = True
# call path controller
pos, vel = self.pathController.move(channels,
self.pathHandler.cruiseSpeed,
self.roi)
# mavlink expects 10^7 integer for accuracy
latInt = int(pos.lat * 10000000)
lonInt = int(pos.lon * 10000000)
# create the SET_POSITION_TARGET_GLOBAL_INT command
msg = 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
latInt,
lonInt,
pos.alt, # x, y, z positions
vel.x,
vel.y,
vel.z, # x, y, z velocity in m/s
0,
0,
0, # x, y, z acceleration (not used)
0,
0) # yaw, yaw_rate (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
# pointing logic
# Adjust the height of the ROI using the paddle
# we pass in both the filtered gimbal paddle value as well as the raw one
self.setROIAltitude(channels[5], channels[7])
# set ROI
msg = self.vehicle.message_factory.command_long_encode(
0,
1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_SET_ROI, #command
0, #confirmation
0,
0,
0,
0, #params 1-4
self.roi.lat,
self.roi.lon,
self.roi.alt + self.ROIAltitudeOffset)
# send pointing message for either cam mode
self.vehicle.send_mavlink(msg)
self.updateApp()