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)