def testAddCancel(self): """ Test adding a vector and its inverse """ loc = LocationGlobalRelative(-45.6549814, 65.216548, 45.25641) vec = Vector3(85.6, -23.4, 3.4) vecInv = Vector3(-85.6, 23.4, -3.4) newloc = location_helpers.addVectorToLocation(loc, vec) newloc = location_helpers.addVectorToLocation(newloc, vecInv) dist = location_helpers.getDistanceFromPoints3d(loc, newloc) self.assertTrue(abs(dist) < ERROR)
def testKnown(self): """ Test adding a zero vector to a location """ loc = LocationGlobalRelative(83.5, 9.2, 0) vec = Vector3(1111.95, 125.876314, 0) newloc = location_helpers.addVectorToLocation(loc, vec) self.assertTrue(abs(newloc.lon - 9.21) < ERROR_LOC) self.assertTrue(abs(newloc.lat - 83.51) < ERROR_LOC)
def testAddZero(self): """ Test adding a zero vector to a location """ loc = LocationGlobalRelative(34.54656, -20.846948, 8.654654) vec = Vector3(0.0, 0.0, 0.0) newloc = location_helpers.addVectorToLocation(loc, vec) dist = location_helpers.getDistanceFromPoints3d(loc, newloc) self.assertTrue(dist < ERROR)
def travel(self): ''' generate a new location from our distance offset and initial position ''' # the location of the vehicle in meters from the origin offsetVector = self.unitVector * self.distance # Scale unit vector by speed velVector = self.unitVector * self.currentSpeed # Convert NEU to NED velocity #velVector.z = -velVector.z # generate a new Location from our offset vector and initial location loc = location_helpers.addVectorToLocation(self.initialLocation, offsetVector) # calc dot product so we can assign a sign to the distance vectorToTarget = location_helpers.getVectorFromPoints( self.initialLocation, self.vehicle.location.global_relative_frame) dp = self.unitVector.x * vectorToTarget.x dp += self.unitVector.y * vectorToTarget.y dp += self.unitVector.z * vectorToTarget.z self.actualDistance = location_helpers.getDistanceFromPoints3d( self.initialLocation, self.vehicle.location.global_relative_frame) if (dp < 0): self.actualDistance = -self.actualDistance # We can now compare the actual vs vector distance self.distError = self.actualDistance - self.distance # formulate mavlink message for pos-vel controller posVelMsg = 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 int(loc.lat * 10000000), # latitude (degrees*1.0e7) int(loc.lon * 10000000), # longitude (degrees*1.0e7) loc.alt, # altitude (meters) velVector.x, velVector.y, velVector.z, # North, East, Down velocity (m/s) 0, 0, 0, # x, y, z acceleration (not used) 0, 0) # yaw, yaw_rate (not used) # send pos-vel command to vehicle self.vehicle.send_mavlink(posVelMsg)
def addLocation(self, loc): # we're ready once we have 2 waypoints and an ROI if len(self.waypoints) < 2: # check altitude limit on 2nd point if self.altLimit is not None and len(self.waypoints) > 0: if loc.alt > self.altLimit: logger.log( "[Selfie]: 2nd selfie point breaches user-set altitude limit (%.1f meters)." % (self.altLimit)) # find vector to 2nd point selfieVector = location_helpers.getVectorFromPoints( self.waypoints[0], loc) # normalize it selfieVector.normalize() # calculate distance between two points d = location_helpers.getDistanceFromPoints3d( self.waypoints[0], loc) # calculate hypotenuse hyp = (self.altLimit - self.waypoints[0].alt) / ( (loc.alt - self.waypoints[0].alt) / d) # scale selfie vector by hypotenuse selfieVector *= hyp # add selfieVector to original selfie point to create a new 2nd point loc = location_helpers.addVectorToLocation( self.waypoints[0], selfieVector) self.waypoints.append(loc) logger.log("[Selfie]: Added a selfie point: %f, %f, %f." % (loc.lat, loc.lon, loc.alt)) elif not self.roi: self.roi = loc logger.log("[Selfie]: Added a selfie ROI: %f, %f, %f." % (loc.lat, loc.lon, loc.alt)) self.pathHandler = pathHandler.TwoPointPathHandler( self.waypoints[0], self.waypoints[1], self.vehicle, self.shotmgr) # ready! set up everything self.shotmgr.rcMgr.enableRemapping(True) # Now change the vehicle into guided mode self.vehicle.mode = VehicleMode("GUIDED") self.manualGimbalTargeting() self.setButtonMappings()
def travel(self): # advance cable controller by dt (time elapsed) now = monotonic.monotonic() if self.lastTime is None: dt = UPDATE_TIME else: dt = now - self.lastTime self.lastTime = now self.cable.update(dt) # add NED position vector to spline origin (addVectorToLocation needs NEU) self.commandPos = location_helpers.addVectorToLocation( self.splineOrigin, Vector3(self.cable.position.x, self.cable.position.y, -self.cable.position.z)) # assign velocity from controller self.commandVel = self.cable.velocity # formulate mavlink message for pos-vel controller posVelMsg = 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 int(self.commandPos.lat * 10000000), # latitude (degrees*1.0e7) int(self.commandPos.lon * 10000000), # longitude (degrees*1.0e7) self.commandPos.alt, # altitude (meters) self.commandVel.x, self.commandVel.y, self.commandVel.z, # North, East, Down velocity (m/s) 0, 0, 0, # x, y, z acceleration (not used) 0, 0) # yaw, yaw_rate (not used) # send pos-vel command to vehicle self.vehicle.send_mavlink(posVelMsg)
def move(self, channels, newHeading=None, newOrigin=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 newOrigin is not None: self.origin = newOrigin if newHeading is not None: self.heading = newHeading * math.pi / 180. if roiVel is None: roiVel = Vector3(0, 0, 0) # Approach approachVel = self._approach(-channels[PITCH]) # Strafe strafeVel = self._strafe(channels[ROLL]) # Climb climbVel = self._climb(channels[RAW_PADDLE]) # add up velocities currentVel = approachVel + strafeVel + climbVel # add up positions self.offset += currentVel * UPDATE_TIME # check altitude limit if self.maxAlt is not None: self.offset.z = min(self.maxAlt, self.offset.z) # convert x,y,z to inertial LLA currentPos = location_helpers.addVectorToLocation( self.origin, self.offset) currentVel += roiVel return currentPos, currentVel
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 handleRCs(self, channels): '''Handles RC inputs and runs the high level shot controller''' # channels are expected to be floating point values # in the (-1.0, 1.0) range don't enter the guided shot # mode until the user has recorded the cable endpoints # block controller from running until we enter play mode if not self.cableCamPlaying: return # if we are still attaching to the spline if self.attaching: self.listenForAttach() return # determine if we should send a PLAYBACK_STATUS update to app self.checkToNotifyApp() # select the larger of the two stick values (between pitch and roll) if abs(channels[ROLL]) > abs(channels[PITCH]): value = channels[ROLL] else: value = -channels[PITCH] # Check if we're in a cruise mode or not if self.cruiseSpeed == 0: # scale max speed by stick value self.desiredSpeed = value * MAX_SPEED else: speed = abs(self.cruiseSpeed) # if sign of stick and cruiseSpeed don't match then... # slow down if math.copysign(1, value) != math.copysign(1, self.cruiseSpeed): speed *= (1.0 - abs(value)) else: # speed up speed += (MAX_SPEED - speed) * abs(value) # speedlimit if speed > MAX_SPEED: speed = MAX_SPEED # carryover user input sign if self.cruiseSpeed < 0: speed = -speed # assign to desired velocity self.desiredSpeed = speed if self.desiredSpeed > 0: self.targetP = 1.0 elif self.desiredSpeed < 0: self.targetP = 0.0 self.cable.setTargetP(self.targetP) # give cable controller our desired speed self.cable.trackSpeed(abs(self.desiredSpeed)) # advance cable controller by dt (time elapsed) now = monotonic.monotonic() if self.lastTime is None: dt = UPDATE_TIME else: dt = now - self.lastTime self.lastTime = now self.cable.update(dt) # add NED position vector to spline origin (addVectorToLocation needs NEU) self.commandPos = location_helpers.addVectorToLocation( self.splineOrigin, Vector3(self.cable.position.x, self.cable.position.y, -self.cable.position.z)) # assign velocity from controller self.commandVel = self.cable.velocity # formulate mavlink message for pos-vel controller posVelMsg = 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 int(self.commandPos.lat * 10000000), # latitude (degrees*1.0e7) int(self.commandPos.lon * 10000000), # longitude (degrees*1.0e7) self.commandPos.alt, # altitude (meters) self.commandVel.x, self.commandVel.y, self.commandVel.z, # North, East, Down velocity (m/s) 0, 0, 0, # x, y, z acceleration (not used) 0, 0) # yaw, yaw_rate (not used) # send pos-vel command to vehicle self.vehicle.send_mavlink(posVelMsg) if self.camInterpolation == 0: # calculate interpolated pitch & yaw newPitch, newYaw = self.interpolateCamera() else: # free look pitch and yaw newPitch, newYaw = (0.0, 0.0) # calculate nudge offset or free-look offset self.yawPitchOffsetter.Update(channels) # add offsets newPitch += self.yawPitchOffsetter.pitchOffset newYaw += self.yawPitchOffsetter.yawOffset # Log whatever we're sending for yaw for troubleshooting # try: # handleRCs.bullshitCounter += 1 # except AttributeError: # handleRCs.bullshitCounter = 0 # if bullshitCounter > 200: # self.bullshitCounter = 0 # logger.log("[multipoint]: MAV_CMD_DO_MOUNT_CONTROL yaw param: %f" %newYaw) # Formulate mavlink message for mount controller. Use mount_control if using a gimbal. Use just condition_yaw if no gimbal. # Modified by Matt 2017-05-18 for ArducCopter master compatibility # mav_cmd_do_mount_control should handle gimbal pitch and copter yaw, but yaw is not working in master. # So this mount_control command is only going to do the gimbal pitch for now. if self.vehicle.mount_status[0] is not None: pointingMsg = self.vehicle.message_factory.command_long_encode( 0, 0, # target system, target component mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, # command 0, # confirmation newPitch * 100, # param1: Pitch in centidegrees 0, # param2: Roll (obviously not used) 0, # param3: Yaw in centidegrees (not working, so using condition_yaw below) 0, 0, 0, # param 4-6 not used mavutil.mavlink. MAV_MOUNT_MODE_MAVLINK_TARGETING # param7: MAV_MOUNT_MODE ) self.vehicle.send_mavlink(pointingMsg) # Temporary fix while the yaw control is not working in the above mount_control command. pointingMsg = self.vehicle.message_factory.command_long_encode( 0, 0, # target system, target component mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command 0, # confirmation newYaw, # param 1 - target angle (degrees) YAW_SPEED, # param 2 - yaw speed (deg/s) 0, # param 3 - direction, always shortest route for now... 0.0, # relative offset 0, 0, 0 # params 5-7 (unused) ) self.vehicle.send_mavlink(pointingMsg) # if no gimbal, assume fixed mount and use condition_yaw only else: # if we don't have a gimbal, just set CONDITION_YAW pointingMsg = self.vehicle.message_factory.command_long_encode( 0, 0, # target system, target component mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command 0, # confirmation newYaw, # param 1 - target angle (degrees) YAW_SPEED, # param 2 - yaw speed (deg/s) 0, # param 3 - direction, always shortest route for now... 0.0, # relative offset 0, 0, 0 # params 5-7 (unused) ) self.vehicle.send_mavlink(pointingMsg)
def handleRCs(self, channels): '''Handles RC inputs and runs the high level shot controller''' # channels are expected to be floating point values # in the (-1.0, 1.0) range don't enter the guided shot # mode until the user has recorded the cable endpoints # block controller from running until we enter play mode if not self.cableCamPlaying: return # if we are still attaching to the spline if self.attaching: self.listenForAttach() return # determine if we should send a PLAYBACK_STATUS update to app self.checkToNotifyApp() # select the larger of the two stick values (between pitch and roll) if abs(channels[ROLL]) > abs(channels[PITCH]): value = channels[ROLL] else: value = -channels[PITCH] # Check if we're in a cruise mode or not if self.cruiseSpeed == 0: # scale max speed by stick value self.desiredSpeed = value * MAX_SPEED else: speed = abs(self.cruiseSpeed) # if sign of stick and cruiseSpeed don't match then... # slow down if math.copysign(1, value) != math.copysign(1, self.cruiseSpeed): speed *= (1.0 - abs(value)) else: # speed up speed += (MAX_SPEED - speed) * abs(value) # speedlimit if speed > MAX_SPEED: speed = MAX_SPEED # carryover user input sign if self.cruiseSpeed < 0: speed = -speed # assign to desired velocity self.desiredSpeed = speed if self.desiredSpeed > 0: self.targetP = 1.0 elif self.desiredSpeed < 0: self.targetP = 0.0 self.cable.setTargetP(self.targetP) # give cable controller our desired speed self.cable.trackSpeed(abs(self.desiredSpeed)) # advance cable controller by dt (time elapsed) now = monotonic.monotonic() if self.lastTime is None: dt = UPDATE_TIME else: dt = now - self.lastTime self.lastTime = now self.cable.update(dt) # add NED position vector to spline origin (addVectorToLocation needs NEU) self.commandPos = location_helpers.addVectorToLocation(self.splineOrigin, Vector3(self.cable.position.x, self.cable.position.y, -self.cable.position.z)) # assign velocity from controller self.commandVel = self.cable.velocity # formulate mavlink message for pos-vel controller posVelMsg = 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 int(self.commandPos.lat * 10000000), # latitude (degrees*1.0e7) int(self.commandPos.lon * 10000000), # longitude (degrees*1.0e7) self.commandPos.alt, # altitude (meters) self.commandVel.x, self.commandVel.y, self.commandVel.z, # North, East, Down velocity (m/s) 0, 0, 0, # x, y, z acceleration (not used) 0, 0) # yaw, yaw_rate (not used) # send pos-vel command to vehicle self.vehicle.send_mavlink(posVelMsg) if self.camInterpolation == 0: # calculate interpolated pitch & yaw newPitch, newYaw = self.interpolateCamera() else: # free look pitch and yaw newPitch, newYaw = (0.0, 0.0) # calculate nudge offset or free-look offset self.yawPitchOffsetter.Update(channels) # add offsets newPitch += self.yawPitchOffsetter.pitchOffset newYaw += self.yawPitchOffsetter.yawOffset # formulate mavlink message for mount controller # do we have a gimbal? if self.vehicle.mount_status[0] is not None: pointingMsg = self.vehicle.message_factory.mount_control_encode( 0, 1, # target system, target component newPitch * 100, # pitch (centidegrees) 0.0, # roll (centidegrees) newYaw * 100, # yaw (centidegrees) 0 # save position ) # if not, assume fixed mount else: # if we don't have a gimbal, just set CONDITION_YAW pointingMsg = self.vehicle.message_factory.command_long_encode( 0, 0, # target system, target component mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command 0, # confirmation newYaw, # param 1 - target angle (degrees) YAW_SPEED, # param 2 - yaw speed (deg/s) 0, # param 3 - direction, always shortest route for now... 0.0, # relative offset 0, 0, 0 # params 5-7 (unused) ) # send pointing command to vehicle self.vehicle.send_mavlink(pointingMsg)