def testZeroDist(self):
""" These points should be zero distance apart """
loc = LocationGlobalRelative(-36.37485, 24.23846, 32.6545)
self.assertTrue(
abs(location_helpers.getDistanceFromPoints(loc, loc)) < ERROR)
loc = LocationGlobalRelative(72.4564, 26.23422, 0.0)
self.assertTrue(
abs(location_helpers.getDistanceFromPoints(loc, loc)) < ERROR)
loc = LocationGlobalRelative(-75.23453, -12.21835, 14.234873)
self.assertTrue(
abs(location_helpers.getDistanceFromPoints(loc, loc)) < ERROR)
def isNearHome(self):
if self.homeLocation is None:
return True
mydist = location_helpers.getDistanceFromPoints(
self.vehicle.location.global_relative_frame, self.homeLocation)
return (mydist < REWIND_MIN_HOME_DISTANCE)
def _checkTetherLocation(self):
"""
Check if the copter has reached the target location, if so, put the copter back to FLY
"""
if self.tetherLocation is None or self.vehicle.location.global_relative_frame is None:
logger.log(
"[GeoFenceManager]: something is not set, tetherLoc:%s vehicleLoc:%s"
% (self.tetherLocation,
self.vehicle.location.global_relative_frame))
return
distance = location_helpers.getDistanceFromPoints(
self.vehicle.location.global_relative_frame, self.tetherLocation)
# If vehicle is within 1 meter of destination or if vehicle is already within the shrunk polygon
if distance < 1.0:
self.tetherState = _GeoFenceManagerTetherState.notActive
self.vehicle.mode = VehicleMode("LOITER")
self.tetherLocation = None
logger.log("[GeoFenceManager]: Deactivating GeoFence")
else:
# Re-enforce tether location in case user hit FLY
if self.tetherLocation is not None and self.vehicle.mode != VehicleMode(
"GUIDED"):
self._stopAtCoord(self.tetherLocation)
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 testSmallDist(self):
""" These points are a known distance apart """
loc = LocationGlobalRelative(-45.23462, -22.2384)
loc2 = LocationGlobalRelative(-45.2673, -22.123512)
dist = location_helpers.getDistanceFromPoints(loc, loc2)
# Google Earth Pro Answer: 9724
delta = abs(dist - 9702.4)
self.assertTrue(delta < ERROR)
def testLargeDist(self):
""" These points are a known distance apart """
loc = LocationGlobalRelative(50.2356, 5.2835723)
loc2 = LocationGlobalRelative(50.7837, 3.444)
dist = location_helpers.getDistanceFromPoints(loc, loc2)
# Google Earth Pro Answer: 144022
delta = abs(dist - 144336)
self.assertTrue(delta < ERROR)
def coneOfAwesomeness(self, _rtlAlt):
''' creates a cone above home that prevents massive altitude changes during RTL '''
homeDistance = location_helpers.getDistanceFromPoints(
self.vehicle.location.global_relative_frame, self.homeLocation)
coneLimit = max(
homeDistance * RTL_CONE_SLOPE,
self.vehicle.location.global_relative_frame.alt + RTL_MIN_RISE)
return min(coneLimit, _rtlAlt)
def calcPhoto(self):
'''determins when to shoot photo based on distance'''
# calc distance from home
self.dist = location_helpers.getDistanceFromPoints(self.pathHandler.pt1, self.vehicle.location.global_relative_frame)
# find a slot
index = math.floor(self.dist / self.triggerDist)
if self.lastShot != index:
self.lastShot = index
self.shotmgr.goproManager.handleRecordCommand(self.shotmgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE)
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 activateGeoFenceIfNecessary(self):
"""
Test if GeoFence is about to be breached, if so, put copter into GUIDED and guide the copter to the best
location. If not, do nothing.
"""
if not self.vehicle.armed:
return
if self.vehicle.system_status in ['CRITICAL', 'EMERGENCY']:
# Not enforcing Geofence in emergency
return
if self.tetherState is _GeoFenceManagerTetherState.active:
# Still guiding to edge
self._checkTetherLocation()
return
vehicleLocation = self.vehicle.location.global_relative_frame
velocity = self.vehicle.velocity
if vehicleLocation is None or vehicleLocation.lat is None or vehicleLocation.lon is None or vehicleLocation.alt is None:
logger.log("[GeoFenceManager]: GeoFence: vehicle location not set")
return
if velocity is None:
logger.log("[GeoFenceManager]: GeoFence: velocity is not set")
return
if len(self.polygons) == 0:
# If GeoFence not set
return
velocityDirection = Vector2(velocity[0], velocity[1])
collidingPoint = (-1, -1, float("inf"), None)
# TODO: We are only supporting a single GeoFence right now, but it's subject to change in the future
fence = self.polygons[0]
if fence is None or fence.origin is None or fence.vertices is None:
logger.log("[GeoFenceManager]: something is not right, fence:%s" %
fence)
return
polygon = map(lambda coord: Vector2(coord.x, coord.y), fence.vertices)
subPolygon = map(lambda coord: Vector2(coord.x, coord.y),
fence.subVertices)
v0_3d = location_helpers.getVectorFromPoints(fence.origin,
vehicleLocation)
v0 = Vector2(v0_3d.x, v0_3d.y)
v1 = Vector2(v0.x + velocityDirection.x, v0.y + velocityDirection.y)
# If not in fence, pull copter back into fence and return
if GeoFenceHelper.isPointInPolygon(v0, polygon) == 0:
logger.log("[GeoFenceManager]: Not in Fence!! v:%s poly: %s" %
(v0, polygon))
stopPoint2D = GeoFenceHelper.closestPointToPolygon(v0, subPolygon)
if stopPoint2D is not None:
# stopPoint2D can be None if an illegal polygon is passed
stopPoint3D = Vector3(stopPoint2D.x, stopPoint2D.y, 0)
stopCoordinate = location_helpers.addVectorToLocation(
fence.origin, stopPoint3D)
stopCoordinate.alt = vehicleLocation.alt
self._stopAtCoord(stopCoordinate)
return
# Test if is going to collide
currentCollidingPoint = GeoFenceHelper.closestCollisionVectorToPolygon(
ray=(v0, v1), polygon=polygon)
if currentCollidingPoint is not None and currentCollidingPoint[
2] < collidingPoint[2]:
collidingPoint = (0, currentCollidingPoint[0],
currentCollidingPoint[1],
currentCollidingPoint[2])
if collidingPoint[0] != -1 and collidingPoint[1] != -1:
fence = self.polygons[collidingPoint[0]]
scalarSpeed = sqrt(velocity[0] * velocity[0] +
velocity[1] * velocity[1]) # TODO: m/s ??
currentCollidingPoint3D = Vector3(collidingPoint[3].x,
collidingPoint[3].y, 0)
collidingPointCoord = location_helpers.addVectorToLocation(
fence.origin, currentCollidingPoint3D)
scalarDistance = location_helpers.getDistanceFromPoints(
vehicleLocation, collidingPointCoord)
# Compensate for the latency
scalarDistance -= scalarSpeed * GEO_FENCE_LATENCY_COEFF
scalarDistance = max(scalarDistance, 0.0)
maximumStoppingSpeed = self._stoppingSpeed(scalarDistance,
scalarSpeed)
if scalarDistance < 0 or scalarSpeed >= maximumStoppingSpeed:
# If collision is None, that means copter has breached the subPolygon, tether to closest point on subpolygon,
# otherwise, tether to the collision point on subpolygon
collision = GeoFenceHelper.closestCollisionVectorToPolygon(
ray=(v0, v1), polygon=subPolygon)
if collision is None:
targetStopPoint2D = GeoFenceHelper.closestPointToPolygon(
v0, subPolygon)
else:
targetStopPoint2D = collision[2]
if targetStopPoint2D is None:
logger.log(
"[GeoFenceManager]: Failed to activate geofence")
return
logger.log(
"[GeoFenceManager]: Activating geofence, speed: %s max_speed:%s"
% (scalarSpeed, maximumStoppingSpeed))
logger.log(
"[GeoFenceManager]: copterLoc: %s will collide with %s, tether to %s"
% (v0, collidingPoint[3], targetStopPoint2D))
# If currently in a shot, end shot
if self.shotMgr.currentShot != shots.APP_SHOT_NONE:
self.shotMgr.enterShot(shots.APP_SHOT_NONE)
targetStopPoint3D = Vector3(targetStopPoint2D.x,
targetStopPoint2D.y, 0)
targetStopCoordinate = location_helpers.addVectorToLocation(
fence.origin, targetStopPoint3D)
targetStopCoordinate.alt = vehicleLocation.alt
self._stopAtCoord(targetStopCoordinate)
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
