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