def addLocation(self, loc):
'''Adds a new or overwrites the current orbit ROI'''
self.roi = loc
# tell the app about the new ROI
packet = struct.pack('<IIddf', app_packet.SOLO_MESSAGE_LOCATION, 20,
self.roi.lat, self.roi.lon, self.roi.alt)
self.shotmgr.appMgr.sendPacket(packet)
# should we init the Orbit state machine
if self.pathHandler is None:
self.initOrbitShot()
# Initialize the location of the vehicle
startRadius = location_helpers.getDistanceFromPoints(
self.roi, self.vehicle.location.global_relative_frame)
startAz = location_helpers.calcAzimuthFromPoints(
self.roi, self.vehicle.location.global_relative_frame)
startAlt = self.vehicle.location.global_relative_frame.alt
logger.log("[Orbit]: Add Location for Orbit controller.")
logger.log("[Orbit]: -->radius: %f, azimuth: %f" %
(startRadius, startAz))
logger.log("[Orbit]: -->lat: %f, lon: %f, alt: %f" %
(self.roi.lat, self.roi.lon, self.roi.alt))
# Initialize the open-loop orbit controller
self.pathController = orbitController.OrbitController(
self.roi, startRadius, startAz, startAlt)
self.pathController.setOptions(maxClimbRate=self.maxClimbRate,
maxAlt=self.maxAlt)
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 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 initLeashController(self):
'''Resets the controller'''
# reset leash
resetRadius = location_helpers.getDistanceFromPoints(self.filteredROI, self.vehicle.location.global_relative_frame)
resetAz = location_helpers.calcAzimuthFromPoints(self.filteredROI, self.vehicle.location.global_relative_frame)
# Initialize the feed-forward orbit controller
self.pathController = LeashController(self.filteredROI, resetRadius, resetAz, self.followControllerAltOffset)
# set controller options
self.pathController.setOptions(maxClimbRate = self.maxClimbRate, maxAlt = self.maxAlt)
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 testKnownAz2(self):
""" Test2 that calcAzimuthFromPoints correctly calculates a known azimuth """
loc = LocationGlobalRelative(83.5, 9.2)
loc2 = LocationGlobalRelative(83.51, 9.21)
az = location_helpers.calcAzimuthFromPoints(loc, loc2)
self.assertTrue(abs(az - 6.458) < ERROR)
def testKnownAz(self):
""" Test that calcAzimuthFromPoints correctly calculates a known azimuth """
loc = LocationGlobalRelative(83.4523, 9.34521)
loc2 = LocationGlobalRelative(83.45233, 9.34524)
az = location_helpers.calcAzimuthFromPoints(loc, loc2)
self.assertTrue(abs(az - 6.5) < ERROR)
def testWest(self):
""" Test that calcAzimuthFromPoints knows when a point is west of another """
loc = LocationGlobalRelative(22.35465, 120.6546)
loc2 = LocationGlobalRelative(22.35465, 120.5465)
az = location_helpers.calcAzimuthFromPoints(loc, loc2)
self.assertTrue(abs(az - 270.0) < ERROR)
def testEast(self):
""" Test that calcAzimuthFromPoints knows when a point is east of another """
loc = LocationGlobalRelative(12.6465, 50.46845)
loc2 = LocationGlobalRelative(12.6465, 50.55464)
az = location_helpers.calcAzimuthFromPoints(loc, loc2)
self.assertTrue(abs(az - 90.0) < ERROR)
def testSouth(self):
""" Test that calcAzimuthFromPoints knows when a point is south of another """
loc = LocationGlobalRelative(63.2346234, 32.3546)
loc2 = LocationGlobalRelative(33.2346234, 32.3546)
az = location_helpers.calcAzimuthFromPoints(loc, loc2)
self.assertTrue(abs(az - 180.0) < ERROR)
def testNorth(self):
""" Test that calcAzimuthFromPoints knows when a point is north of another """
loc = LocationGlobalRelative(-63.2346234, 15.2385)
loc2 = LocationGlobalRelative(-33.2346234, 15.2385)
az = location_helpers.calcAzimuthFromPoints(loc, loc2)
self.assertTrue(abs(az) < ERROR)