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)