Mirrors/OpenSolo
1
0
Fork 0
mirror of https://github.com/OpenSolo/OpenSolo.git synced 2025-04-29 14:14:30 +02:00
Code Issues Packages Projects Releases Wiki Activity

Initial commit, based on .tar.gz file as provided by 3DR , see SOURCE file.

Browse Source
This commit is contained in:
Buzz 2017-07-29 18:04:17 +10:00
commit b3cd739e46
89 changed files with 21654 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
shotmanager/.gitignore vendored Normal file
View File

@ -0,0 +1,10 @@
mav.parm
mav.tlog
mav.tlog.raw
mav.parm
*.pyc
env
mavlink-solo
shotmanager.back
shotManager_version.py
.idea

14
shotmanager/.travis.yml Normal file
View File

@ -0,0 +1,14 @@
language: python
python:
- "2.7"
# command to install dependencies
install:
- pip install -r requirements.txt
# explicitly overwrite the mavlink that was installed earlier since that will be the wrong public version
- git clone ssh://git@github.com/3drobotics/mavlink-solo
- cd mavlink-solo/pymavlink
- python setup.py install
- cd ../..
sudo : true
# command to run tests
script: sudo cp config/shotmanager.orig /etc/shotmanager.conf && nosetests Test/

36
shotmanager/COPYRIGHT-3DR Normal file
View File

@ -0,0 +1,36 @@
Copyright 2014-2016 3D Robotics
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
~~~~
Contributors prior to public release:
* Eric Liao
* Will Silva
* Jason Short
* Nick Speal
* Yi Lu
* Phu Nguyen
* Ramón Roche
* Tim Ryan
* Robert Cottrell
* Igor Napolskikh
* Hamish Willee
* John Finley
* Allan Matthew
* Angus Peart
* Luke Holoubek
* Jonathan Wight
* Chavi Weingarten
* Siddharth Bharat Purohit

176
shotmanager/GeoFenceHelper.py Normal file
View File

@ -0,0 +1,176 @@
#
# geoFenceHelper.py
# shotmanager
#
# Helper class to do the math in fenceManager
#
# Created by Yi Lu
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from vector2 import *
import shotLogger
logger = shotLogger.logger
class GeoFenceHelper:
@staticmethod
def closestPointToSegment(point, segment):
"""
Find the closest point from a point to a segment
:param point: Vector2, source point
:param segment: list of Vector2 with length of 2, denoting two end points of a segment
:return: Tuple (Float, Vector2), first element is the position t of result point on the segment from 0-1
second element is a Vector2 denoting the resulting point
"""
if len(segment) < 2:
logger.log("[GeoFenceHelper]: Illegal segment %s" % segment)
return None
a = segment[0]
b = segment[1]
aToP = point - a
aToB = b - a
atb2 = aToB.x * aToB.x + aToB.y * aToB.y
atp_dot_atb = aToP.dot(aToB)
if atb2 != 0:
t = float(atp_dot_atb) / atb2
else:
# a and b are infinitely close
return a, (point - a).length()
intersect = Vector2(a.x + aToB.x * t, a.y + aToB.y * t)
distance = (point - intersect).length()
if 0 <= t <= 1:
return intersect, distance
elif t < 0:
return a, (point - a).length()
else:
return b, (point - b).length()
@staticmethod
def closestPointToPolygon(point, polygon):
"""
Find the closest point from a point to a polygon defined by a list of vertices
:param point: Vector2, source point
:param polygon: list of Vector2, defines the vertices of polygon, minimum length 3
:return: None if an illegal polygon has been passed
Vector2 denoting the resulting point if found
"""
# Polygon must has at least 3 vertices plus repeated origin
if len(polygon) < 4:
logger.log("[GeoFenceHelper]: Polygon need at least three vertices")
return None
intersect = None
distance = float("inf")
for i in range(0, len(polygon) - 1):
a = polygon[i]
b = polygon[i + 1]
segIntersect, segDistance = GeoFenceHelper.closestPointToSegment(point, [a, b])
if segDistance < distance:
intersect = segIntersect
distance = segDistance
return intersect
@staticmethod
def closestCollisionVectorToSegment(ray, segment):
"""
http://stackoverflow.com/questions/14307158/how-do-you-check-for-intersection-between-a-line-segment-and-a-line-ray-emanatin
Detect the collision point for a ray with a segment
:param ray: Tuple of Vector2, origin and direction denoting a ray
:param segment: Tuple of Vector2, denoting segment to be tested against collision
:return: None if ray is not intersecting with segment
Float t if intersecting, t is the position of intersection on the ray
in equation: r(t) = ray[0] + t * (ray[1] - ray[0])
"""
denom = ((ray[1].x - ray[0].x) * (segment[1].y - segment[0].y)) - (ray[1].y - ray[0].y) * (segment[1].x - segment[0].x)
if denom == 0:
return None
r = (((ray[0].y - segment[0].y) * (segment[1].x - segment[0].x)) - (ray[0].x - segment[0].x) * (segment[1].y - segment[0].y)) / denom
s = (((ray[0].y - segment[0].y) * (ray[1].x - ray[0].x)) - (ray[0].x - segment[0].x) * (ray[1].y - ray[0].y)) / denom
if r >= 0 and 0 <= s <= 1:
return r
return None
@staticmethod
def closestCollisionVectorToPolygon(ray, polygon):
"""
Detect the closet collision point for a ray with a polygon
:param ray: Tuple of Vector2, origin and direction denoting a ray
:param polygon: list of Vector2
:return: None if ray is not intersecting with polygon
(Int, Double, Vector2) if intersection is found
Int being edge index
Double being position along collision vector
Vector2 being collision point on Polygon
"""
# Polygon must has at least 3 vertices plus repeated origin
if len(polygon) < 4:
logger.log("[GeoFenceHelper]: Illegal polygon, vertex count must be 3 or more, got %s" % len(polygon))
return None
collidingPoint = (-1, float("inf"), None)
for i in range(len(polygon) - 1):
t = GeoFenceHelper.closestCollisionVectorToSegment(ray, (polygon[i], polygon[i + 1]))
if t is not None and 0 < t < collidingPoint[1]:
intersection = Vector2(ray[0].x + t * (ray[1].x - ray[0].x), ray[0].y + t * (ray[1].y - ray[0].y))
collidingPoint = (i, t, intersection)
if collidingPoint[0] == -1:
return None
return collidingPoint
@staticmethod
def isPointInPolygon(point, polygon):
"""
http://geomalgorithms.com/a03-_inclusion.html#wn_PnPoly()
:param point: Vector2 denoting the point
:param polygon: list of Vector2 denoting vertices of polygon
:return: Winding number of point and polygon:
0 if point is outside polygon
>0 if polygon is winding around point 1 or more times
<0 if polygon is not ccw?
"""
# Polygon must has at least 3 vertices plus repeated origin
if len(polygon) < 4:
logger.log("[GeoFenceHelper]: polygon must have 3 or more vertices, got %s" % len(polygon))
return None
wn = 0
for i in range(0, len(polygon) - 1):
v1 = polygon[i]
v2 = polygon[i + 1]
if v1.y <= point.y:
if v2.y > point.y:
if GeoFenceHelper.isLeft(v1, v2, point) > 0:
wn += 1
else:
if v2.y <= point.y:
if GeoFenceHelper.isLeft(v1, v2, point) < 0:
wn -= 1
return wn
@staticmethod
def isLeft(p0, p1, p2):
"""
http://geomalgorithms.com/a01-_area.html
Test if a point is Left|On|Right of an infinite 2D line.
:param p0: Vector2, first point of segment
:param p1: Vector2, second point of segment
:param p2: Vector2, point to be tested against segment
:return: >0 for P2 left of the line through P0 to P1
=0 for P2 on the line
<0 for P2 right of the line
"""
return (p1.x - p0.x) * (p2.y - p0.y) - (p2.x - p0.x) * (p1.y - p0.y)

406
shotmanager/GeoFenceManager.py Normal file
View File

@ -0,0 +1,406 @@
#
# geoFenceManager.py
# shotmanager
#
# Utility to manage Geofence
#
# Created by Yi Lu, improved based on Jason Short and Will Silva's GeoFence prototype
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import struct
from enum import Enum
from dronekit import LocationGlobalRelative, VehicleMode, mavutil
import location_helpers
import app_packet
from vector3 import Vector3
from GeoFenceHelper import *
from math import sqrt
import shots
import json
logger = shotLogger.logger
GEO_FENCE_LATENCY_COEFF = 1.5 # seconds? not sure...
# tuple of message types that we handle
GEO_FENCE_MESSAGES = \
(
app_packet.GEOFENCE_SET_DATA,
app_packet.GEOFENCE_SET_ACK,
app_packet.GEOFENCE_UPDATE_POLY,
app_packet.GEOFENCE_CLEAR,
app_packet.GEOFENCE_ACTIVATED
)
class _GeoFenceManagerState(Enum):
notFenced = 0
fenced = 1
class _GeoFenceManagerTetherState(Enum):
notActive = 0
active = 1
class GeoFence:
def __init__(self, origin, verticesCart, subVerticesCart, stayOut=True):
"""
This class define the structure of GeoFence data
:param origin: GlobalLocationRelative
:param verticesCart: [Vector3]
:param subVerticesCart: [Vector3]
:param stayOut: Bool
"""
self.origin = origin
self.vertices = verticesCart
self.subVertices = subVerticesCart
self.stayOut = stayOut
def __str__(self):
return "<Fence origin: %s, vertices: %s, stayOut: %s>" % (self.origin, self.vertices, self.stayOut)
class GeoFenceManager:
"""
This class manage the GeoFence on Solo
"""
def __init__(self, shotMgr):
# assign the vehicle object
self.vehicle = shotMgr.vehicle
self.shotMgr = shotMgr
self.polygons = []
self.state = _GeoFenceManagerState.notFenced
self.tetherLocation = None
self.tetherState = _GeoFenceManagerTetherState.notActive
def _reset(self):
"""
Reset the states of GeoFence Manager. Will put copter into LOITER.
Should be called to clean stuff up
"""
self.polygons = []
self.state = _GeoFenceManagerState.notFenced
self.tetherLocation = None
self.tetherState = _GeoFenceManagerTetherState.notActive
def handleFenceData(self, packetType, packetValue):
"""
handle all the incoming FLV data for GeoFence
:param packetType: ShotManager packet type
:param packetValue: Packet payloads
"""
if packetType == app_packet.GEOFENCE_SET_DATA:
geoFenceData = json.loads(packetValue)
coordArr = geoFenceData['coord']
subCoordArr = geoFenceData['subCoord']
fenceTypeArr = geoFenceData['type']
self._handleGeoFenceSetDataMessage(coordArr, subCoordArr, fenceTypeArr)
elif packetType == app_packet.GEOFENCE_UPDATE_POLY:
(updateType, polygonIndex, vertexIndex, lat, lng, subLat, subLng) = struct.unpack('<BHHdddd', packetValue)
#TODO: updateType can be 0: update vertex, 1: add new vertex, 2: remove vertex. This parameter is ignored now and assumed to be updating vertex
newCoord = LocationGlobalRelative(lat, lng, 0)
newSubCoord = LocationGlobalRelative(subLat, subLng, 0)
self._handleGeoFenceUpdateMessage(polygonIndex, vertexIndex, newCoord, newSubCoord)
elif packetType == app_packet.GEOFENCE_CLEAR:
self._reset()
self._sendFenceSetAck(0, True)
@staticmethod
def _checkGeoFenceDataValidity(coordArr, subCoordArr, fenceTypeArr):
if len(coordArr) != len(subCoordArr) or len(coordArr) != len(fenceTypeArr):
# Data inconsistency
logger.error("GeoFence data length mismatch: coord: %s, subCoord: %s, type: %s" % (len(coordArr), len(subCoordArr), len(fenceTypeArr)))
return False
for i in range(len(coordArr) - 1):
if len(coordArr[i]) < 3:
# Not a valid polygon
logger.error("Illegal polygon received, polygon edge count: %s" % len(coordArr[i]))
return False
if len(coordArr[i]) != len(subCoordArr[i]):
# Data inconsistency
logger.error("Polygon and subpolygon have different edge count, polygon: %s, subpolygon: %s" % (len(coordArr[i]), len(subCoordArr[i])))
return False
for pair in coordArr[i]:
if len(pair) != 2:
# Coord has length of 2
logger.error("Coordinate must be of length 2, got: %s" % len(pair))
return False
for pair in subCoordArr[i]:
if len(pair) != 2:
logger.error("Coordinate must be of length 2, got: %s" % len(pair))
# Coord has length of 2
return False
return True
def _handleGeoFenceSetDataMessage(self, coordArr, subCoordArr, fenceTypeArr):
self.polygons = []
# Check if data is well formatted
dataValid = self._checkGeoFenceDataValidity(coordArr, subCoordArr, fenceTypeArr)
if not dataValid:
self._sendFenceSetAck(len(coordArr), False)
return
else:
self._sendFenceSetAck(len(coordArr), True)
# Data is good
for i in range(len(coordArr)):
coords = map(lambda coord: LocationGlobalRelative(coord[0], coord[1], 0), coordArr[i])
subCoords = map(lambda coord: LocationGlobalRelative(coord[0], coord[1], 0), subCoordArr[i])
fenceType = fenceTypeArr[i]
# coords[0] is guaranteed to exist because a polygon with less than 3 vertices won't pass the validation
origin = coords[0]
coordsCart = map(lambda coord: location_helpers.getVectorFromPoints(origin, coord), coords)
subCoordsCart = map(lambda coord: location_helpers.getVectorFromPoints(origin, coord), subCoords)
# Repeat first coordinate, first elements are guaranteed to exist or validation will fail
coordsCart.append(coordsCart[0])
subCoordsCart.append(subCoordsCart[0])
newGeoFence = GeoFence(origin, coordsCart, subCoordsCart, fenceType)
logger.log("[GeoFenceManager]: coords: %s" % coordsCart)
logger.log("[GeoFenceManager]: subCoords: %s" % subCoordsCart)
self.polygons.append(newGeoFence)
def _handleGeoFenceUpdateMessage(self, polygonIndex, vertexIndex, coord, subCoord):
if polygonIndex < 0 or polygonIndex > len(self.polygons):
logger.log("[GeoFenceManager]: Illegal polygonIndex, polygon count: %s, polygonIndex: %s" % (len(self.polygons), polygonIndex))
self._sendFenceSetAck(0, False)
return
# Need to take 1 from the polygon vertex count because the first vertex is repeated
if vertexIndex < 0 or vertexIndex > len(self.polygons[polygonIndex]) - 1:
logger.log("[GeoFenceManager]: Illegal vertexIndex, vertices count: %s, vertexIndex: %s" % (len(self.polygons[polygonIndex]) - 1, vertexIndex))
self._sendFenceSetAck(0, False)
return
origin = self.polygons[polygonIndex].origin
coordCart = location_helpers.getVectorFromPoints(origin, coord)
subCoordCart = location_helpers.getVectorFromPoints(origin, subCoord)
self.polygons[polygonIndex].vertices[vertexIndex] = coordCart
self.polygons[polygonIndex].subVertices[vertexIndex] = subCoordCart
# Update extra vertex if the first vertex is being updated
if vertexIndex == 0:
lastIndex = len(self.polygons[polygonIndex])
self.polygons[polygonIndex].vertices[lastIndex] = coordCart
self.polygons[polygonIndex].subVertices[lastIndex] = subCoordCart
self._sendFenceSetAck(len(self.polygons), True)
def clearGeoFence(self):
"""
Clear GeoFence when something bad happens, such as app disconnect
"""
self._reset()
def _sendFenceSetAck(self, count, valid):
"""
Send Ack to app to acknowledge set count command
:param count: the count of polygons received from app
:param valid: Bool indicating if the acknowledging geofence is valid or not
"""
packet = struct.pack('<IIH?', app_packet.GEOFENCE_SET_ACK, 3, count, valid)
self.shotMgr.appMgr.sendPacket(packet)
def sendClear(self):
"""
Send Clear packet to the app to clear GeoFence
"""
logger.log("[GeoFenceManager]: Sending clearing geofence")
packet = struct.pack('<II', app_packet.GEOFENCE_CLEAR, 0)
self.shotMgr.appMgr.sendPacket(packet)
def _sendActivated(self):
"""
Send activated packet to the app to clear GeoFence
"""
packet = struct.pack('<II', app_packet.GEOFENCE_ACTIVATED, 0)
self.shotMgr.appMgr.sendPacket(packet)
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 _stopAtCoord(self, coordinate):
"""
Move a copter to a point
:param coordinate: LocationGlobalRelative, target coordinate
"""
self._sendActivated()
self.vehicle.mode = VehicleMode("GUIDED")
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(coordinate.lat * 10000000), # latitude (degrees*1.0e7)
int(coordinate.lon * 10000000), # longitude (degrees*1.0e7)
coordinate.alt, # altitude (meters)
0, 0, 0, # North, East, Down velocity (m/s)
0, 0, 0, # x, y, z acceleration (not used)
0, 0) # yaw, yaw_rate (not used)
self.vehicle.send_mavlink(posVelMsg)
self.tetherState = _GeoFenceManagerTetherState.active
self.tetherLocation = coordinate
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)
@staticmethod
def _stoppingSpeed(dist, speed):
"""
Return maximum stopping speed, based on distance and current speed
:param dist: Distance to go before stop
:param speed: current scalar speed
:return: Maximum speed at current distance before breach
"""
# TODO: These are constant from multipoint.py
tanAccelLim = 1.0897247358851685
smoothStopP = 0.7
linearVelocity = tanAccelLim / smoothStopP
linearDist = linearVelocity / smoothStopP
if speed > linearVelocity:
return sqrt(2. * tanAccelLim * (speed**2/(2.*tanAccelLim) + dist))
else:
p1 = speed / smoothStopP
p2 = p1 + dist
if p2 > linearDist:
return sqrt(2. * tanAccelLim * (p2 - 0.5*linearDist))
else:
return p2 * smoothStopP

31
shotmanager/GoProConstants.py Normal file
View File

@ -0,0 +1,31 @@
# GoPro constants as defined in https://docs.google.com/document/d/1CcYOCZRw9C4sIQu4xDXjPMkxZYROmTLB0EtpZamnq74/edit#
GOPRO_V1_SPEC_VERSION = 1
GOPRO_V2_SPEC_VERSION = 2
CAPTURE_MODE_VIDEO = 0
CAPTURE_MODE_PHOTO = 1
CAPTURE_MODE_BURST = 2 # Burst only for Hero 3+
CAPTURE_MODE_TIMELAPSE = 3
CAPTURE_MODE_MULTISHOT = 4 # Multishot only for Hero4
SHUTTER_STATUS_STOP_VIDEO = 0
SHUTTER_STATUS_START_VIDEO = 1
SHUTTER_STATUS_STOP_BURST = 2
SHUTTER_STATUS_START_BURST = 3
MODEL_NONE = 0
MODEL_HERO3PLUS_SILVER = 10
MODEL_HERO3PLUS_BLACK = 11
STATUS_NO_GOPRO = 0
STATUS_INCOMPATIBLE_GOPRO = 1
STATUS_GOPRO_CONNECTED = 2
STATUS_GOPRO_ERROR = 3
RECORD_COMMAND_STOP = 0
RECORD_COMMAND_START = 1
RECORD_COMMAND_TOGGLE = 2
VIDEO_FORMAT_NTSC = 0
VIDEO_FORMAT_PAL = 1

547
shotmanager/GoProManager.py Normal file
View File

@ -0,0 +1,547 @@
#
# This file handles GoPro commands and holds GoPro state
#
import os
import Queue
import sys
import threading
import time
import monotonic
import math
from pymavlink import mavutil
sys.path.append(os.path.realpath(''))
import app_packet
from GoProConstants import *
import settings
import shotLogger
import struct
logger = shotLogger.logger
# tuple of message types that we handle
GOPROMESSAGES = \
(
app_packet.GOPRO_SET_ENABLED,
app_packet.GOPRO_SET_REQUEST,
app_packet.GOPRO_RECORD,
app_packet.GOPRO_REQUEST_STATE,
app_packet.GOPRO_SET_EXTENDED_REQUEST
)
# see https://docs.google.com/document/d/1CcYOCZRw9C4sIQu4xDXjPMkxZYROmTLB0EtpZamnq74/edit#heading=h.y6z65lvic5q5
VALID_GET_COMMANDS = \
(
mavutil.mavlink.GOPRO_COMMAND_POWER,
mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE,
mavutil.mavlink.GOPRO_COMMAND_BATTERY,
mavutil.mavlink.GOPRO_COMMAND_MODEL,
mavutil.mavlink.GOPRO_COMMAND_VIDEO_SETTINGS,
mavutil.mavlink.GOPRO_COMMAND_LOW_LIGHT,
mavutil.mavlink.GOPRO_COMMAND_PHOTO_RESOLUTION,
mavutil.mavlink.GOPRO_COMMAND_PHOTO_BURST_RATE,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_WHITE_BALANCE,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_COLOUR,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_GAIN,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_SHARPNESS,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_EXPOSURE,
mavutil.mavlink.GOPRO_COMMAND_TIME,
mavutil.mavlink.GOPRO_COMMAND_CHARGING,
)
VALID_SET_COMMANDS = \
(
mavutil.mavlink.GOPRO_COMMAND_POWER,
mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE,
mavutil.mavlink.GOPRO_COMMAND_SHUTTER,
mavutil.mavlink.GOPRO_COMMAND_VIDEO_SETTINGS,
mavutil.mavlink.GOPRO_COMMAND_LOW_LIGHT,
mavutil.mavlink.GOPRO_COMMAND_PHOTO_RESOLUTION,
mavutil.mavlink.GOPRO_COMMAND_PHOTO_BURST_RATE,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_WHITE_BALANCE,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_COLOUR,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_GAIN,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_SHARPNESS,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_EXPOSURE,
mavutil.mavlink.GOPRO_COMMAND_TIME,
mavutil.mavlink.GOPRO_COMMAND_CHARGING,
)
REQUERY_COMMANDS = \
(
mavutil.mavlink.GOPRO_COMMAND_VIDEO_SETTINGS,
mavutil.mavlink.GOPRO_COMMAND_LOW_LIGHT,
mavutil.mavlink.GOPRO_COMMAND_PHOTO_RESOLUTION,
mavutil.mavlink.GOPRO_COMMAND_PHOTO_BURST_RATE,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_WHITE_BALANCE,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_COLOUR,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_GAIN,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_SHARPNESS,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_EXPOSURE,
mavutil.mavlink.GOPRO_COMMAND_TIME,
)
class GoProManager():
def __init__(self, shotMgr):
# GoPro heartbeat state
self.status = mavutil.mavlink.GOPRO_HEARTBEAT_STATUS_DISCONNECTED
self.captureMode = CAPTURE_MODE_VIDEO
self.isRecording = False
# Additional GoPro state
self.battery = 0
self.model = MODEL_NONE
self.videoFormat = VIDEO_FORMAT_NTSC
self.videoResolution = 0
self.videoFrameRate = 0
self.videoFieldOfView = 0
self.videoLowLight = False
self.photoResolution = 0
self.photoBurstRate = 0
self.videoProtune = False
self.videoProtuneWhiteBalance = 0
self.videoProtuneColor = 0
self.videoProtuneGain = 0
self.videoProtuneSharpness = 0
self.videoProtuneExposure = 0
self.shotMgr = shotMgr
# This exists because we can't seem to send multiple messages in a stream to the gopro.
# Instead, we'll queue up all our messages and wait for a response before sending the next message
self.msgQueue = Queue.Queue()
# is the GoPro currently handling a message?
self.isGoproBusy = False
# when the last message was sent
self.lastRequestSent = 0.0
# lock access to shot manager state
self.lock = threading.Lock()
# check if we should enable GoPro messages at all
try:
enabled = int(settings.readSetting("GoProEnabled"))
logger.log("[gopro]: read enabled value from settings of %d."%(enabled))
self.enabled = enabled > 0
self.setGimbalEnabledParam()
except Exception as ex:
logger.log("[gopro]: Error reading config file.")
logger.log(str(ex))
self.enabled = True
logger.log("[gopro]: Inited GoProManager")
def state_callback(self, vehicle, name, message):
self.lock.acquire()
try:
self.internal_state_callback(message)
except Exception as e:
logger.log("[gopro]: state_callback error: %s" % e)
finally:
self.lock.release()
def internal_state_callback(self, state):
status = state.status
captureMode = state.capture_mode
isRecording = (state.flags & mavutil.mavlink.GOPRO_FLAG_RECORDING) != 0
sendState = False
if self.status != status:
self.status = status
logger.log("[gopro]: Gopro status changed to %d"%(self.status))
sendState = True
# right now, query status when we initially connect
if self.status == mavutil.mavlink.GOPRO_HEARTBEAT_STATUS_CONNECTED:
self.isGoproBusy = False
self.msgQueue = Queue.Queue()
self.sendGoProRequest(mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE)
self.sendGoProRequest(mavutil.mavlink.GOPRO_COMMAND_BATTERY)
self.sendGoProRequest(mavutil.mavlink.GOPRO_COMMAND_MODEL)
self.sendGoProRequest(mavutil.mavlink.GOPRO_COMMAND_VIDEO_SETTINGS)
self.sendGoProRequest(mavutil.mavlink.GOPRO_COMMAND_LOW_LIGHT)
self.sendGoProRequest(mavutil.mavlink.GOPRO_COMMAND_PHOTO_RESOLUTION)
self.sendGoProRequest(mavutil.mavlink.GOPRO_COMMAND_PHOTO_BURST_RATE)
self.sendGoProRequest(mavutil.mavlink.GOPRO_COMMAND_PROTUNE)
self.sendGoProRequest(mavutil.mavlink.GOPRO_COMMAND_PROTUNE_WHITE_BALANCE)
self.sendGoProRequest(mavutil.mavlink.GOPRO_COMMAND_PROTUNE_COLOUR)
self.sendGoProRequest(mavutil.mavlink.GOPRO_COMMAND_PROTUNE_GAIN)
self.sendGoProRequest(mavutil.mavlink.GOPRO_COMMAND_PROTUNE_SHARPNESS)
self.sendGoProRequest(mavutil.mavlink.GOPRO_COMMAND_PROTUNE_EXPOSURE)
if self.captureMode != captureMode:
self.captureMode = captureMode
logger.log("[gopro]: Gopro capture mode changed to %d"%(self.captureMode))
sendState = True
if self.isRecording != isRecording:
self.isRecording = isRecording
logger.log("[gopro]: Gopro recording status changed to %d"%(self.isRecording))
sendState = True
if sendState:
self.sendState()
def get_response_callback(self, vehicle, name, message):
self.lock.acquire()
try:
self.internal_get_response_callback(message)
except Exception as e:
logger.log("[gopro]: get_response_callback error: %s" % e)
finally:
self.lock.release()
def internal_get_response_callback(self, response):
command = response[0]
status = response[1]
value = response[2]
sendState = False
if status != mavutil.mavlink.GOPRO_REQUEST_SUCCESS:
logger.log("[gopro]: Gopro get request for command %d failed with status %d"%(command, status))
self.processMsgQueue()
return
if command == mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE:
captureMode = value[0]
if self.captureMode != captureMode:
self.captureMode = captureMode
sendState = True
logger.log("[gopro]: Gopro capture mode changed to %d"%(self.captureMode))
elif command == mavutil.mavlink.GOPRO_COMMAND_MODEL:
model = value[0]
if self.model != model:
self.model = model
sendState = True
logger.log("[gopro]: Gopro model changed to %d"%(self.model))
elif command == mavutil.mavlink.GOPRO_COMMAND_BATTERY:
battery = value[0]
if self.battery != battery:
self.battery = battery
sendState = True
logger.log("[gopro]: Gopro battery changed to %d"%(self.battery))
elif command == mavutil.mavlink.GOPRO_COMMAND_VIDEO_SETTINGS:
videoResolution = value[0]
videoFrameRate = value[1]
videoFieldOfView = value[2]
videoFormat = VIDEO_FORMAT_NTSC if (value[3] & mavutil.mavlink.GOPRO_VIDEO_SETTINGS_TV_MODE) == 0 else VIDEO_FORMAT_PAL
if self.videoResolution != videoResolution:
self.videoResolution = videoResolution
sendState = True
logger.log("[gopro]: Gopro video resolution changed to %d"%(self.videoResolution))
if self.videoFrameRate != videoFrameRate:
self.videoFrameRate = videoFrameRate
sendState = True
logger.log("[gopro]: Gopro video frame rate changed to %d"%(self.videoFrameRate))
if self.videoFieldOfView != videoFieldOfView:
self.videoFieldOfView = videoFieldOfView
sendState = True
logger.log("[gopro]: Gopro video field of view changed to %d"%(self.videoFieldOfView))
if self.videoFormat != videoFormat:
self.videoFormat = videoFormat
sendState = True
logger.log("[gopro]: Gopro video format changed to %d"%(self.videoFormat))
elif command == mavutil.mavlink.GOPRO_COMMAND_LOW_LIGHT:
videoLowLight = value[0] != 0
if self.videoLowLight != videoLowLight:
self.videoLowLight = videoLowLight
sendState = True
logger.log("[gopro]: Gopro low light changed to %d"%(self.videoLowLight))
elif command == mavutil.mavlink.GOPRO_COMMAND_PHOTO_RESOLUTION:
photoResolution = value[0]
if self.photoResolution != photoResolution:
self.photoResolution = photoResolution
sendState = True
logger.log("[gopro]: Gopro photo resolution changed to %d"%(self.photoResolution))
elif command == mavutil.mavlink.GOPRO_COMMAND_PHOTO_BURST_RATE:
photoBurstRate = value[0]
if self.photoBurstRate != photoBurstRate:
self.photoBurstRate = photoBurstRate
sendState = True
logger.log("[gopro]: Gopro photo burst rate changed to %d"%(self.photoBurstRate))
elif command == mavutil.mavlink.GOPRO_COMMAND_PROTUNE:
videoProtune = value[0] != 0
if self.videoProtune != videoProtune:
self.videoProtune = videoProtune
sendState = True
logger.log("[gopro]: Gopro video protune changed to %d"%(self.videoProtune))
elif command == mavutil.mavlink.GOPRO_COMMAND_PROTUNE_WHITE_BALANCE:
videoProtuneWhiteBalance = value[0]
if self.videoProtuneWhiteBalance != videoProtuneWhiteBalance:
self.videoProtuneWhiteBalance = videoProtuneWhiteBalance
sendState = True
logger.log("[gopro]: Gopro video protune white balance changed to %d"%(self.videoProtuneWhiteBalance))
elif command == mavutil.mavlink.GOPRO_COMMAND_PROTUNE_COLOUR:
videoProtuneColor = value[0]
if self.videoProtuneColor != videoProtuneColor:
self.videoProtuneColor = videoProtuneColor
sendState = True
logger.log("[gopro]: Gopro video protune color changed to %d"%(self.videoProtuneColor))
elif command == mavutil.mavlink.GOPRO_COMMAND_PROTUNE_GAIN:
videoProtuneGain = value[0]
if self.videoProtuneGain != videoProtuneGain:
self.videoProtuneGain = videoProtuneGain
sendState = True
logger.log("[gopro]: Gopro video protune gain changed to %d"%(self.videoProtuneGain))
elif command == mavutil.mavlink.GOPRO_COMMAND_PROTUNE_SHARPNESS:
videoProtuneSharpness = value[0]
if self.videoProtuneSharpness != videoProtuneSharpness:
self.videoProtuneSharpness = videoProtuneSharpness
sendState = True
logger.log("[gopro]: Gopro video protune sharpness changed to %d"%(self.videoProtuneSharpness))
elif command == mavutil.mavlink.GOPRO_COMMAND_PROTUNE_EXPOSURE:
videoProtuneExposure = value[0]
if self.videoProtuneExposure != videoProtuneExposure:
self.videoProtuneExposure = videoProtuneExposure
sendState = True
logger.log("[gopro]: Gopro video protune exposure changed to %d"%(self.videoProtuneExposure))
else:
logger.log("[gopro]: Got unexpected Gopro callback for command %d"%(command))
if sendState:
self.sendState()
self.processMsgQueue()
def set_response_callback(self, vehicle, name, message):
self.lock.acquire()
try:
self.internal_set_response_callback(message)
except Exception as e:
logger.log("[gopro]: set_response_callback error: %s" % e)
finally:
self.lock.release()
def internal_set_response_callback(self, response):
command = response[0]
status = response[1]
logger.log("[gopro]: Got Gopro set response for command %d with status %d"%(command, status))
if status != mavutil.mavlink.GOPRO_REQUEST_SUCCESS:
# if a set request failed, return current state to resynchronize client
self.sendState()
self.processMsgQueue()
# wrapper to create a gopro_get_request mavlink message with the given command
def sendGoProRequest(self, command):
if command not in VALID_GET_COMMANDS:
logger.log("[gopro]: Got invalid Gopro get command %d"%(command))
return
msg = self.shotMgr.vehicle.message_factory.gopro_get_request_encode(
0, mavutil.mavlink.MAV_COMP_ID_GIMBAL, # target system, target component
command
)
self.queueMsg(msg)
# wrapper to create a gopro_set_request mavlink message with the given command and value
def sendGoProCommand(self, command, value):
if command not in VALID_SET_COMMANDS:
logger.log("[gopro]: Got invalid Gopro set command %d"%(command))
return
if not self.isValidCommandValue(value):
logger.log("[gopro]: Invalid value for Gopro set command %d"%(command))
return
msg = self.shotMgr.vehicle.message_factory.gopro_set_request_encode(
0, mavutil.mavlink.MAV_COMP_ID_GIMBAL, # target system, target component
command, value
)
self.queueMsg(msg)
# Follow up with a get request if notification of change is required
if command in REQUERY_COMMANDS:
self.sendGoProRequest(command)
# Updates the gopro time.
def update_gopro_time(self, timeInSecs):
logger.log("[gopro]: Updating gopro time to " + str(timeInSecs))
tm = int(timeInSecs)
tm_list = [tm & 0xff, (tm >> 8) & 0xff, (tm >> 16) & 0xff, (tm >> 24) & 0xff]
self.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_TIME, tm_list)
def isValidCommandValue(self, value):
if value[0] < 0 or value[0] > 255:
return False
if value[1] < 0 or value[1] > 255:
return False
if value[2] < 0 or value[2] > 255:
return False
if value[3] < 0 or value[3] > 255:
return False
return True
# handle a call to start/stop/toggle recording on the given mode
def handleRecordCommand(self, mode, command):
if self.status == mavutil.mavlink.GOPRO_HEARTBEAT_STATUS_DISCONNECTED or self.status == mavutil.mavlink.GOPRO_HEARTBEAT_STATUS_INCOMPATIBLE:
logger.log("[gopro]: handleRecordCommand called but GoPro is not connected")
return
logger.log("[gopro]: handleRecordCommand called with mode %d, command %d"%(mode, command))
if mode == CAPTURE_MODE_VIDEO:
# do we want to start or stop recording?
if command == RECORD_COMMAND_STOP:
startstop = 0
elif command == RECORD_COMMAND_START:
startstop = 1
elif command == RECORD_COMMAND_TOGGLE:
startstop = 0 if self.isRecording else 1
else:
return
# don't start recording if we're already recording
if self.isRecording and startstop == 1:
return
logger.log("[gopro]: Sending command for video recording: %d"%(startstop))
# we're not in video mode, switch to it!
if self.captureMode != CAPTURE_MODE_VIDEO:
self.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, (CAPTURE_MODE_VIDEO, 0 ,0 , 0))
self.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_SHUTTER, (startstop, 0, 0, 0))
elif mode == CAPTURE_MODE_PHOTO or mode == CAPTURE_MODE_BURST or mode == CAPTURE_MODE_MULTISHOT:
# don't handle nonsensical commands
if command == RECORD_COMMAND_STOP:
return
# for now, let's try switching out of video mode, taking a picture, and then switching back
if self.captureMode == CAPTURE_MODE_VIDEO:
self.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, (CAPTURE_MODE_PHOTO, 0, 0, 0))
self.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_SHUTTER, (1, 0, 0, 0))
self.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, (CAPTURE_MODE_VIDEO, 0, 0, 0))
logger.log("[gopro]: Sending command to take go to photo mode, take a still, and return")
else:
self.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_SHUTTER, (1, 0, 0, 0))
logger.log("[gopro]: Sending command to take a still/burst/multishot")
# since the gopro can't handle multiple messages at once, we wait for a response before sending
# each subsequent message. This is how we queue up messages
def queueMsg(self, msg):
if self.isGoproBusy and monotonic.monotonic() > self.lastRequestSent + 2.0:
self.isGoproBusy = False
self.msgQueue = Queue.Queue()
# return current state to resynchronize client
self.sendState()
if self.isGoproBusy:
self.msgQueue.put(msg)
else:
self.isGoproBusy = True
self.lastRequestSent = monotonic.monotonic()
# Need to send False for fix_targeting so our message gets routed to the gimbal
self.shotMgr.vehicle.send_mavlink(msg)
# called whenever the message queue is ready to send another message.
def processMsgQueue(self):
if self.msgQueue.empty():
self.isGoproBusy = False
else:
msg = self.msgQueue.get_nowait()
self.lastRequestSent = monotonic.monotonic()
# Need to send False for fix_targeting so our message gets routed to the gimbal
self.shotMgr.vehicle.send_mavlink(msg)
logger.log("[gopro]: sending message from the queue. Size is now %d"%(self.msgQueue.qsize()))
# shotManager receives this gopro control packet and passes it here
def handlePacket(self, type, data):
self.lock.acquire()
try:
self.internalHandlePacket(type, data)
finally:
self.lock.release()
def internalHandlePacket(self, type, data):
if type == app_packet.GOPRO_SET_ENABLED:
(enabled, ) = struct.unpack('<I', data)
self.setGoProEnabled(enabled > 0)
elif type == app_packet.GOPRO_SET_REQUEST:
(command, value) = struct.unpack('<HH', data)
self.sendGoProCommand(command, (value, 0, 0, 0))
elif type == app_packet.GOPRO_RECORD:
(startstop, ) = struct.unpack('<I', data)
self.handleRecordCommand(self.captureMode, startstop)
elif type == app_packet.GOPRO_REQUEST_STATE:
self.sendState()
elif type == app_packet.GOPRO_SET_EXTENDED_REQUEST:
(command, value1, value2, value3, value4, ) = struct.unpack("<HBBBB", data)
self.sendGoProCommand(command, (value1, value2, value3, value4))
# packages up our entire current state and sends it to the app
def sendState(self):
logger.log("[gopro]: sending Gopro state to app")
# Because of a bug in version 1.2 and below of the iOS app, the
# version 1 packet must be filled with zeros to avoid a memory
# corruption crash.
# 2 unsigned shorts for a header, 26 unsigned bytes, then 5 unsigned shorts
pkt = struct.pack('<IIBBBBBBBBBBBBBBBBBBBBBBBBBBHHHHH', app_packet.GOPRO_V1_STATE, 36, \
GOPRO_V1_SPEC_VERSION,
self.model,
self.status,
self.isRecording,
self.captureMode,
# for now, all the rest is yet to be defined
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0
)
self.shotMgr.appMgr.sendPacket(pkt)
# Now also send a version 2 packet to include the additional GoPro settings
# 2 unsigned shorts for a header, 26 unsigned bytes, then 5 unsigned shorts
pkt = struct.pack('<IIBBBBBBBBBBBBBBBBBBBBBBBBBBHHHHH', app_packet.GOPRO_V2_STATE, 36, \
GOPRO_V2_SPEC_VERSION,
self.model,
self.status,
self.isRecording,
self.captureMode,
self.videoFormat,
self.videoResolution,
self.videoFrameRate,
self.videoFieldOfView,
self.videoLowLight,
self.photoResolution,
self.photoBurstRate,
self.videoProtune,
self.videoProtuneWhiteBalance,
self.videoProtuneColor,
self.videoProtuneGain,
self.videoProtuneSharpness,
self.videoProtuneExposure,
self.enabled,
# for now, all the rest is yet to be defined
0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0
)
self.shotMgr.appMgr.sendPacket(pkt)
# Tell the gimbal to turn Gopro comms on/off depending on if our internal
# flag is set to on/off
def setGimbalEnabledParam(self):
value = 1.0 if self.enabled else 0.0
logger.log("[gopro]: sending gimbal enabled param (GMB_GP_CTRL) to %f"%(value))
msg = self.shotMgr.vehicle.message_factory.param_set_encode(
0, mavutil.mavlink.MAV_COMP_ID_GIMBAL, # target system, target component
"GMB_GP_CTRL", value, mavutil.mavlink.MAV_PARAM_TYPE_REAL32
)
self.shotMgr.vehicle.send_mavlink(msg)
# When the app tells us to enable/disable Gopro controls, we do:
# 1. set our internal flag
# 2. Tell settings to write out this value
# 3. Set the gimbal's parameter
def setGoProEnabled(self, enabled):
self.enabled = enabled
value = 1 if enabled else 0
settings.writeSetting("GoProEnabled", str(value))
self.setGimbalEnabledParam()
logger.log("[gopro]: We have set GoProEnabled to %d"%(value))

202
shotmanager/LICENSE-APACHE Normal file
View File

@ -0,0 +1,202 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

187
shotmanager/README.md Normal file
View File

@ -0,0 +1,187 @@
WARNING - WORK IN PROGRESS
```
This code is known to be broken and/or incomplete. IT DOES NOT WORK.
We are actively working on fixing it, and we really, really do not recommend you download it just yet.
We will remove this warning from the repository when it is no longer required.
```
# ShotManager
<img align="left" src="https://cloud.githubusercontent.com/assets/5368500/12708704/85440436-c8f6-11e5-9bfe-45327c174b28.png"> ShotManager is a Python application that governs smart shots, GoPro control, and some Solo Controller (Artoo) functionality.
## Overview
ShotManager is a Python application that runs on the i.MX6 Companion Computer (sometimes referred to as "SoloLink") and manages smart shots, GoPro control, and some Solo Controller (Artoo) functionality.
The diagram below shows the main ShotManager communications.
<img width="700px" src="https://www.lucidchart.com/publicSegments/view/4257006a-9401-4daf-a77c-afb8e7f316d9/image.png" />
The diagram is fairly straightforward. Shots are implemented using DroneKit-Python scripts and communicated to the autopilot using MAVLink messages. The App<->ShotManager communication travels over TCP on port 5507; the message format and messages are documented in [App-ShotManager Communication](/docs/App-ShotManager%20communication.md).
## Dependencies (upstream and downstream)
The main dependencies of this project are:
* [dronekit-python](https://github.com/dronekit/dronekit-python) - ShotManager uses *DroneKit Python* to implement the movement behavior required by shots. ([Docs here](http://python.dronekit.io/)).
* [dronekit-python-solo](https://github.com/dronekit/dronekit-python-solo) - An extension for dronekit-python that enables Solo-specific functionality.
* [sololink-python](https://github.com/3drobotics/sololink-python) - ShotManager uses this to acquire information about Artoo buttons (A,B,Pause,Fly) and to get and remap RC stick values. This is the Python interface for [SoloLink](https://github.com/3drobotics/SoloLink).
* [mavlink-solo](https://github.com/3drobotics/mavlink-solo) - ShotManager uses this C-library to package (and read) MAVLink messages in communications with the Pixhawk and GoPro.
* [numpy](https://github.com/numpy/numpy) - A popular mathematics library for Python.
The project is a dependency of:
* [SoloLink]() - This contains all the *3DR-created* software in the Yocto image running on the Companion Computer (i.MX6).
* [meta-3dr]() - This contains the central definition of how the whole i.MX6 Yocto image is constructed. It contains [Bitbake](https://www.yoctoproject.org/tools-resources/projects/bitbake) recipes for the different included software, including ShotManager, SoloLink etc.
## Developer Setup
### Creating feature branches
The shotmanager software plays several roles in the Solo software environment. Due to this, it is one of the most actively developed-upon repositories at 3DRobotics. In order to maintain repository cleanliness, branches should be only kept on origin if they have a purpose. Branches should also be named by their creators e.g. (john/some-feature-branch). There are periodic manual audits of any un-claimed branches to keep the branch count low.
### Setting up a simulation environment (OS X ONLY)
These instructions explain how to setup the ardupilot SITL under a virtual machine and setup the shotmanager environment on OS X.
#### Ardupilot Solo simulated in Ubuntu 14.04
1] Download & install [VirtualBox](https://www.virtualbox.org/wiki/Downloads).
2] Download & install [Ubuntu 14.04](http://www.ubuntu.com/download/desktop) in VirtualBox.
3] Install VirtualBox tools.
3] Install required packages.
```
sudo apt-get update
sudo apt-get install python-matplotlib python-serial python-wxgtk2.8 python-lxml
sudo apt-get install python-scipy python-opencv ccache gawk git python-pip python-pexpect
sudo pip install pymavlink MAVProxy==1.4.38
```
4] Clone [ardupilot-solo](https://github.com/3drobotics/ardupilot-solo) to your home directory.
```
git clone https://github.com/3drobotics/ardupilot-solo.git
```
5] Navigate to the ArduCopter directory.
```
cd ~/ardupilot-solo/ArduCopter
```
6] Clear unwanted APM parameter set.
```
../Tools/autotest/sim_vehicle.sh -w
```
**NOTICE**: Wait until you see *GPS Lock* before exiting this process and continuing.
**NOTICE**: Repeat steps 7-9 everytime you want to start up ardupilot-solo.
7] Get your OS X IPv4 address.
In OS X terminal.app:
```
ifconfig | grep inet
```
Sample output:
```
silva$ ifconfig | grep inet
inet6 ::1 prefixlen 128
inet 127.0.0.1 netmask 0xff000000
inet6 fe80::1%lo0 prefixlen 64 scopeid 0x1
inet6 fe80::a299:9bff:fe05:a25d%en0 prefixlen 64 scopeid 0x4
inet 10.1.48.164 netmask 0xfffffc00 broadcast 10.1.51.255
inet6 fe80::f0c4:ffff:fe2f:a1f%awdl0 prefixlen 64 scopeid 0x9
inet6 fe80::9082:2385:dea6:4452%utun0 prefixlen 64 scopeid 0xa
inet6 fdd5:617c:442a:d75a:9082:2385:dea6:4452 prefixlen 64
inet 10.211.55.2 netmask 0xffffff00 broadcast 10.211.55.255
inet 10.37.129.2 netmask 0xffffff00 broadcast 10.37.129.255
```
In this case, our IPv4 address is 10.1.48.164.
8] Navigate to the ArduCopter directory.
```
cd ~/ardupilot-solo/ArduCopter
```
9] Run ardupilot-solo within a simulated environment.
**NOTICE**: If your mobile device is the iOS simulator, then MOBILE_DEVICE_IP is the same as OS_X_IPv4.
```
../Tools/autotest/sim_vehicle.sh --console --map --out OS_X_IPv4_HERE:14560 --out MOBILE_DEVICE_IP_HERE:14550
```
#### Shotmanager simulated in OS X
1] Create a virtualenv for Python to install shotmanager dependencies.
```
sudo pip install virtualenv
virtualenv shotman_venv
source shotman_venv/bin/activate
```
2] Install shotmanager dependencies.
```
pip install dronekit dronekit-solo numpy nose mock
git clone https://github.com/3drobotics/sololink-python.git
cd ~/sololink-python
sudo python setup.py install
cd ..
```
3] Clone this repository.
```
git clone ...
```
4] Copy shotmanager.conf to /etc.
```
sudo cp ~/shotmanager/sim/shotmanager.conf /etc/
```
**NOTICE**: Repeat steps 5-7 everytime you want to launch shotmanager.
5] Launch the Artoo emulator program (TestRC.py).
```
cd ~/shotmanager/sim/
python TestRC.py
```
6] Open a new terminal and launch shotmanager.
```
source shotman_venv/bin/activate
cd ~/shotmanager/
python main.py udp:0.0.0.0:14560 udpout:127.0.0.1:14550
```
7] Open a new terminal and tail the shotmanager logs.
```
tail -f /log/shotlog
```
### Tests
[ShotManager Tests](/Test) are run automatically when a patch is submitted (as part of continuous integration).
To run tests locally, navigate to the root of the ShotManager repository and run:
```
nosetests -s ./Test
```
The `-s` (optional) enables debug printing of commands to the screen.
The tests require that “mock” and “nose” be present. These should already be have been installed when you set up the developer environment, but if not you can do so via pip:
```
sudo pip install nose mock
```
## Resources
* **Documentation:**
* [App-ShotManager Communication](/docs/App-ShotManager%20communication.md)

1
shotmanager/SOURCE Normal file
View File

@ -0,0 +1 @@
This source code was released by 3DR in the file: shotmanager_v2.4.1-1_2d69b277.tar.gz

216
shotmanager/Test/TestAppManager.py Normal file
View File

@ -0,0 +1,216 @@
import unittest
import shotManager
import appManager
from dronekit import Vehicle
import app_packet
import mock
from mock import patch, Mock
import socket
import struct
import shots
from sololink import btn_msg
class TestParse(unittest.TestCase):
@patch.object(socket.socket, 'bind')
def setUp(self, mock_bind):
self.mgr = shotManager.ShotManager()
self.v = mock.create_autospec(Vehicle)
self.mgr.Start(self.v)
self.mgr.appMgr.client = Mock(specs=['recv'])
def tearDown(self):
self.mgr.appMgr.server.close()
def testEnterShot(self):
""" Test parsing entering orbit """
self.mgr.enterShot = Mock()
value = struct.pack('<IIi', app_packet.SOLO_MESSAGE_SET_CURRENT_SHOT, 4, shots.APP_SHOT_ORBIT)
self.mgr.appMgr.client.recv.return_value = value
handled = self.mgr.appMgr.parse()
self.mgr.enterShot.assert_called_with(shots.APP_SHOT_ORBIT)
def testEnterUnknownShot(self):
""" Test parsing an unknown shot """
self.mgr.enterShot = Mock()
value = struct.pack('<IIi', app_packet.SOLO_MESSAGE_SET_CURRENT_SHOT, 99, shots.APP_SHOT_ORBIT)
self.mgr.appMgr.client.recv.return_value = value
handled = self.mgr.appMgr.parse()
assert not self.mgr.enterShot.called
def testGetButtonSettings(self):
""" Test parsing the request for button settings """
self.mgr.sendPacket = Mock()
self.mgr.appMgr.sendPacket = Mock()
self.mgr.buttonManager = Mock()
self.mgr.buttonManager.getFreeButtonMapping = Mock()
self.mgr.buttonManager.getFreeButtonMapping.return_value = (1, 2)
value = struct.pack('<IIiiii', app_packet.SOLO_MESSAGE_GET_BUTTON_SETTING, 16, btn_msg.ButtonA, btn_msg.Press, 4, 12)
self.mgr.appMgr.client.recv.return_value = value
handled = self.mgr.appMgr.parse()
packet = struct.pack('<IIiiii', app_packet.SOLO_MESSAGE_GET_BUTTON_SETTING, 16, btn_msg.ButtonA, btn_msg.Press, 1, 2)
self.mgr.appMgr.sendPacket.assert_called_with(packet)
def testSetButtonSettings(self):
""" Test parsing the setting button settings """
self.mgr.buttonManager = Mock()
value = struct.pack('<IIiiii', app_packet.SOLO_MESSAGE_SET_BUTTON_SETTING, 16, btn_msg.ButtonA, btn_msg.Press, 13, 14)
self.mgr.appMgr.client.recv.return_value = value
handled = self.mgr.appMgr.parse()
self.mgr.buttonManager.setFreeButtonMapping.assert_called_with(btn_msg.ButtonA, 13, 14)
def testSetButtonSettingsNonPress(self):
""" Don't allow setting of non-Press events """
self.mgr.buttonManager = Mock()
value = struct.pack('<IIiiii', app_packet.SOLO_MESSAGE_SET_BUTTON_SETTING, 16, btn_msg.ButtonA, btn_msg.Hold, 13, 14)
self.mgr.appMgr.client.recv.return_value = value
handled = self.mgr.appMgr.parse()
self.assertFalse( self.mgr.buttonManager.setFreeButtonMapping.called )
def testGoProSetRequest(self):
""" Test parsing gopro set request """
self.mgr.goproManager = Mock()
value = struct.pack('<IIHH', app_packet.GOPRO_SET_REQUEST, 4, 12, 2)
trimValue = struct.pack('<HH', 12, 2)
self.mgr.appMgr.client.recv.return_value = value
handled = self.mgr.appMgr.parse()
self.mgr.goproManager.handlePacket.assert_called_with(app_packet.GOPRO_SET_REQUEST, trimValue)
def testGoProRecord(self):
""" Test parsing gopro record """
self.mgr.goproManager = Mock()
value = struct.pack('<III', app_packet.GOPRO_RECORD, 4, 27)
trimValue = struct.pack('<I', 27)
self.mgr.appMgr.client.recv.return_value = value
handled = self.mgr.appMgr.parse()
self.mgr.goproManager.handlePacket.assert_called_with(app_packet.GOPRO_RECORD, trimValue)
def testGoProRequestState(self):
""" Test parsing gopro request state """
self.mgr.goproManager = Mock()
value = struct.pack('<II', app_packet.GOPRO_REQUEST_STATE, 0)
trimValue = ''
self.mgr.appMgr.client.recv.return_value = value
handled = self.mgr.appMgr.parse()
self.mgr.goproManager.handlePacket.assert_called_with(app_packet.GOPRO_REQUEST_STATE, trimValue)
def testGoProSetExtendedRequest(self):
""" Test parsing gopro set request with extended payload """
self.mgr.goproManager = Mock()
value = struct.pack('<IIHBBBB', app_packet.GOPRO_SET_EXTENDED_REQUEST, 6, 5, 0, 3, 7, 1)
trimValue = struct.pack('<HBBBB', 5, 0, 3, 7, 1)
self.mgr.appMgr.client.recv.return_value = value
handled = self.mgr.appMgr.parse()
self.mgr.goproManager.handlePacket.assert_called_with(app_packet.GOPRO_SET_EXTENDED_REQUEST, trimValue)
def testUnknownType(self):
""" Test parsing an incoming unknown packet """
appManager.logger = Mock()
value = struct.pack('<III', 3333, 4, 4444)
self.mgr.appMgr.client = Mock(specs=['recv'])
self.mgr.appMgr.client.recv.return_value = value
handled = self.mgr.appMgr.parse()
appManager.logger.log.assert_called_with("[app]: Got an unknown packet type: %d."%(3333))
class TestConnectClient(unittest.TestCase):
@patch.object(socket.socket, 'bind')
def setUp(self, mock_bind):
self.mgr = shotManager.ShotManager()
self.v = mock.create_autospec(Vehicle)
self.mgr.Start(self.v)
self.mgr.buttonManager = Mock()
self.mgr.appMgr.server = Mock()
self.client = Mock()
address = (3333,)
self.mgr.appMgr.server.accept = Mock(return_value=(self.client, address))
def testConnect(self):
""" Connect should set things up """
self.mgr.goproManager = Mock()
self.mgr.appMgr.connectClient()
self.assertEqual(self.mgr.appMgr.client, self.client)
self.client.setblocking.assert_called_with(0)
self.assertTrue( self.client in self.mgr.inputs )
self.assertTrue( self.mgr.appMgr.clientQueue != None )
self.mgr.buttonManager.setButtonMappings.assert_called_with()
self.mgr.goproManager.sendState.assert_called_with()
def testAlreadyHaveDifferentClient(self):
""" If we're already connected to a client, we should accept and then close it """
self.mgr.appMgr.disconnectClient = Mock()
self.mgr.appMgr.isAppConnected = Mock(return_value=True)
self.mgr.appMgr.client_address = (4444,)
self.mgr.appMgr.connectClient()
# we need to accept this new connection and then close it
self.client.close.assert_called_with()
def testAlreadyHaveSameClient(self):
""" If we're already connected to a client, we should accept and then close it """
self.mgr.appMgr.disconnectClient = Mock()
self.mgr.appMgr.isAppConnected = Mock(return_value=True)
self.mgr.appMgr.client_address = (3333,)
self.mgr.appMgr.connectClient()
# we need to accept this new connection and then close it
self.mgr.appMgr.disconnectClient.assert_called_with()
class TestDisconnectClient(unittest.TestCase):
@patch.object(socket.socket, 'bind')
def setUp(self, mock_bind):
self.mgr = shotManager.ShotManager()
self.v = mock.create_autospec(Vehicle)
self.mgr.Start(self.v)
self.mgr.buttonManager = Mock()
self.mgr.enterShot = Mock()
self.mgr.appMgr.client = Mock()
self.mgr.inputs = [self.mgr.appMgr.client]
def testDisconnectRemovesOutput(self):
""" Make sure disconnecting a client removes it from outputs """
client = self.mgr.appMgr.client
self.mgr.outputs = [self.mgr.appMgr.client]
self.mgr.appMgr.isAppConnected = Mock(return_value=True)
self.mgr.appMgr.disconnectClient()
self.assertFalse( client in self.mgr.outputs )
def testDisconnectRemovesInput(self):
""" Make sure disconnecting a client removes it from inputs """
client = self.mgr.appMgr.client
self.mgr.appMgr.isAppConnected = Mock(return_value=True)
self.mgr.appMgr.disconnectClient()
self.assertFalse( client in self.mgr.inputs )
def testDisconnectClosesClientSocket(self):
""" Make sure disconnecting a client closes the client socket """
client = self.mgr.appMgr.client
self.mgr.appMgr.isAppConnected = Mock(return_value=True)
self.mgr.appMgr.disconnectClient()
client.close.assert_called_with()
def testDisconnectClearsOutClient(self):
""" Make sure disconnecting a client sets client and clientQueue to None """
client = self.mgr.appMgr.client
self.mgr.appMgr.isAppConnected = Mock(return_value=True)
self.mgr.appMgr.disconnectClient()
self.assertEqual( self.mgr.appMgr.client, None )
self.assertEqual( self.mgr.appMgr.clientQueue, None )
def testDisconnectRemapsButtons(self):
""" Make sure disconnecting a client remaps buttons """
client = self.mgr.appMgr.client
self.mgr.appMgr.isAppConnected = Mock(return_value=True)
self.mgr.appMgr.disconnectClient()
self.mgr.buttonManager.setButtonMappings.assert_called_with()
def testDisconnectTurnsOffShots(self):
""" Make sure disconnecting turns off shots """
self.mgr.currentShot = shots.APP_SHOT_ORBIT
client = self.mgr.appMgr.client
self.mgr.appMgr.isAppConnected = Mock(return_value=True)
self.mgr.appMgr.disconnectClient()
self.mgr.enterShot.assert_called_with(shots.APP_SHOT_NONE)
def testDisconnectTwice(self):
""" Make sure we can call disconnect twice in a row without error """
client = self.mgr.appMgr.client
self.mgr.appMgr.isAppConnected = Mock(side_effect=[True,False])
self.mgr.appMgr.disconnectClient()
self.mgr.appMgr.disconnectClient()

287
shotmanager/Test/TestButtonManager.py Normal file
View File

@ -0,0 +1,287 @@
# Unit tests for buttonManager
import mock
from mock import call
from mock import Mock
from mock import patch
import os
from os import sys, path
import unittest
sys.path.append(os.path.realpath('..'))
import buttonManager
import GeoFenceManager
import shots
from dronekit import Vehicle
# on host systems these files are located here
from sololink import btn_msg
import shotManager
import struct
import app_packet
class TestHandleButtons(unittest.TestCase):
def setUp(self):
self.mgr = shotManager.ShotManager()
self.mgr.rcMgr = Mock()
self.mgr.buttonManager = buttonManager.buttonManager(self.mgr)
self.mgr.lastMode = "LOITER"
self.mgr.appMgr = Mock()
self.mgr.vehicle = mock.create_autospec(Vehicle)
self.mgr.vehicle.system_status = 'ACTIVE'
self.mgr.sendPacket = Mock()
self.mgr.goproManager = Mock()
self.mgr.geoFenceManager = GeoFenceManager.GeoFenceManager(self.mgr)
def tearDown(self):
del self.mgr
def testHandleButtonsNoEvent(self):
""" Testing handle Buttons with no event """
self.mgr.buttonManager.handleButtons(None)
def testHandleButtonsStartShot(self):
""" Test starting a shot via button press """
whichShots = [shots.APP_SHOT_ORBIT, shots.APP_SHOT_CABLECAM]
self.mgr.client = 5
self.mgr.vehicle.armed = True
self.mgr.last_ekf_ok = True
for i in whichShots:
self.mgr.buttonManager.getFreeButtonMapping = Mock(return_value = (i, -1))
self.mgr.currentShot = shots.APP_SHOT_NONE
self.mgr.buttonManager.handleButtons((btn_msg.ButtonA, btn_msg.Press))
self.assertEqual(self.mgr.currentShot, i)
def testDisarmedStartShot(self):
""" if we're not armed, we should not be able to start the shot """
whichShots = [shots.APP_SHOT_ORBIT, shots.APP_SHOT_CABLECAM]
self.mgr.client = 5
self.mgr.vehicle.armed = False
self.mgr.last_ekf_ok = True
for i in whichShots:
self.mgr.buttonManager.getFreeButtonMapping = Mock(return_value = (i, -1))
self.mgr.currentShot = shots.APP_SHOT_NONE
self.mgr.buttonManager.handleButtons((btn_msg.ButtonA, btn_msg.Press))
self.assertEqual(self.mgr.currentShot, shots.APP_SHOT_NONE)
packetDisallow = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_UNARMED)
self.mgr.appMgr.sendPacket.assert_any_call(packetDisallow)
def testNotActiveStartShot(self):
""" if we're not active, we should not be able to start the shot """
whichShots = [shots.APP_SHOT_ORBIT, shots.APP_SHOT_CABLECAM, shots.APP_SHOT_MULTIPOINT]
self.mgr.client = 5
self.mgr.vehicle.armed = True
self.mgr.vehicle.system_status = 'STANDBY'
self.mgr.last_ekf_ok = True
for i in whichShots:
self.mgr.buttonManager.getFreeButtonMapping = Mock(return_value=(i, -1))
self.mgr.currentShot = shots.APP_SHOT_NONE
self.mgr.buttonManager.handleButtons((btn_msg.ButtonA, btn_msg.Press))
self.assertEqual(self.mgr.currentShot, shots.APP_SHOT_NONE)
packetDisallow = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_UNARMED)
self.mgr.appMgr.sendPacket.assert_any_call(packetDisallow)
def testHandleButtonsStartMode(self):
""" Test starting a mode via button press """
self.mgr.buttonManager.getFreeButtonMapping = Mock(return_value = (-1, 3))
self.mgr.currentShot = shots.APP_SHOT_NONE
self.mgr.buttonManager.handleButtons((btn_msg.ButtonA, btn_msg.Press))
self.assertEqual(self.mgr.vehicle.mode.name, 'AUTO')
def testHandleButtonsExitShot(self):
""" Test exiting a shot by pressing Fly """
self.mgr.currentShot = shots.APP_SHOT_CABLECAM
self.mgr.last_ekf_ok = True
self.mgr.buttonManager.handleButtons((btn_msg.ButtonFly, btn_msg.Press))
self.assertEqual( self.mgr.currentShot, shots.APP_SHOT_NONE )
def testHandleButtonsPassToShot(self):
""" Test passing a button press to a shot """
self.mgr.currentShot = shots.APP_SHOT_CABLECAM
self.mgr.curController = Mock()
self.mgr.buttonManager.handleButtons((btn_msg.ButtonA, btn_msg.Press))
self.mgr.curController.handleButton.assert_called_with(btn_msg.ButtonA, btn_msg.Press)
class TestButtonMappingSettings(unittest.TestCase):
def setUp(self):
with patch('buttonManager.buttonManager.connect') as mock:
mock.read = Mock()
mock.get = Mock()
shotmgr = Mock()
self.v = Mock()
shotmgr.vehicle = self.v
self.mgr = buttonManager.buttonManager(shotmgr)
self.mgr.setArtooButton = Mock()
buttonManager.connected = True
def testSetButtonMappingsAppConnectedNoShotGoodEKF(self):
""" Set Artoo's button mappings with app connected and no shot set and good EKF """
self.mgr.shotMgr.appMgr.isAppConnected.return_value = True
self.mgr.isButtonConnected = Mock(return_value=True)
self.mgr.shotMgr.currentShot = shots.APP_SHOT_NONE
self.v.ekf_ok = True
self.mgr.freeButtonMappings = [(shots.APP_SHOT_SELFIE, -1), (shots.APP_SHOT_CABLECAM, -1)]
self.mgr.setButtonMappings()
call1 = call(btn_msg.ButtonA, shots.APP_SHOT_SELFIE, btn_msg.ARTOO_BITMASK_ENABLED, "Selfie\0")
call2 = call(btn_msg.ButtonB, shots.APP_SHOT_CABLECAM, btn_msg.ARTOO_BITMASK_ENABLED, "Cable Cam\0")
self.mgr.setArtooButton.assert_has_calls( [call1, call2] )
def testSetButtonMappingsAppConnectedNoShotBadEKF(self):
""" Bad EKF should gray out shots """
self.mgr.shotMgr.appMgr.isAppConnected.return_value = True
self.mgr.isButtonConnected = Mock(return_value=True)
self.mgr.shotMgr.currentShot = shots.APP_SHOT_NONE
self.v.ekf_ok = False
self.mgr.freeButtonMappings = [(shots.APP_SHOT_SELFIE, -1), (shots.APP_SHOT_CABLECAM, -1)]
self.mgr.setButtonMappings()
call1 = call(btn_msg.ButtonA, shots.APP_SHOT_SELFIE, 0, "Selfie\0")
call2 = call(btn_msg.ButtonB, shots.APP_SHOT_CABLECAM, 0, "Cable Cam\0")
self.mgr.setArtooButton.assert_has_calls( [call1, call2] )
def testSetButtonMappingsAppConnectedNoShotBadEKF(self):
""" Modes are still enabled with bad ekf """
self.mgr.shotMgr.appMgr.isAppConnected.return_value = True
self.mgr.isButtonConnected = Mock(return_value=True)
self.mgr.shotMgr.currentShot = shots.APP_SHOT_NONE
self.v.ekf_ok = False
self.mgr.freeButtonMappings = [(shots.APP_SHOT_NONE, 1), (shots.APP_SHOT_NONE, 13)]
self.mgr.setButtonMappings()
call1 = call(btn_msg.ButtonA, shots.APP_SHOT_NONE, btn_msg.ARTOO_BITMASK_ENABLED, "Acro\0")
call2 = call(btn_msg.ButtonB, shots.APP_SHOT_NONE, btn_msg.ARTOO_BITMASK_ENABLED, "Sport\0")
self.mgr.setArtooButton.assert_has_calls( [call1, call2] )
def testSetButtonMappingsAppConnectedNoShotSetModes(self):
""" Set Artoo's button mappings with APM modes with app connected and no shot set """
self.mgr.shotMgr.appMgr.isAppConnected.return_value = True
self.mgr.isButtonConnected = Mock(return_value=True)
self.mgr.shotMgr.currentShot = shots.APP_SHOT_NONE
self.mgr.freeButtonMappings = [(shots.APP_SHOT_NONE, 0), (shots.APP_SHOT_NONE, 11)]
self.mgr.setButtonMappings()
call1 = call(btn_msg.ButtonA, shots.APP_SHOT_NONE, btn_msg.ARTOO_BITMASK_ENABLED, "Stabilize\0")
call2 = call(btn_msg.ButtonB, shots.APP_SHOT_NONE, btn_msg.ARTOO_BITMASK_ENABLED, "Drift\0")
self.mgr.setArtooButton.assert_has_calls( [call1, call2] )
def testHasCurrentShot(self):
""" Have the shot controller set buttons """
self.mgr.shotMgr.appMgr.isAppConnected.return_value = True
self.mgr.isButtonConnected = Mock(return_value=True)
self.mgr.shotMgr.currentShot = shots.APP_SHOT_SELFIE
self.mgr.shotMgr.curController = Mock()
self.mgr.setButtonMappings()
self.mgr.shotMgr.curController.setButtonMappings.assert_called_with()
def testNoAppConnected(self):
""" If there's no app connected, we shouldn't set a shot """
self.mgr.shotMgr.appMgr.isAppConnected.return_value = False
self.mgr.isButtonConnected = Mock(return_value=True)
self.mgr.shotMgr.currentShot = shots.APP_SHOT_NONE
self.mgr.freeButtonMappings = [(shots.APP_SHOT_ORBIT, -1), (shots.APP_SHOT_NONE, 2)]
self.mgr.setButtonMappings()
call1 = call(btn_msg.ButtonA, shots.APP_SHOT_ORBIT, 0, "Orbit\0")
call2 = call(btn_msg.ButtonB, shots.APP_SHOT_NONE, btn_msg.ARTOO_BITMASK_ENABLED, "FLY: Manual\0")
self.mgr.setArtooButton.assert_has_calls( [call1, call2] )
class TestButtonMappingSettingsBrakeButton(unittest.TestCase):
def setUp(self):
with patch('buttonManager.buttonManager.connect') as mock:
mock.read = Mock()
mock.get = Mock()
shotmgr = Mock()
self.v = Mock()
shotmgr.vehicle = self.v
self.mgr = buttonManager.buttonManager(shotmgr)
self.mgr.setArtooButton = Mock()
buttonManager.connected = True
self.mgr.freeButtonMappings = [(shots.APP_SHOT_NONE, 1), (shots.APP_SHOT_NONE, 13)]
def testAppDisarmedVehicleNoEKFNoBrake(self):
""" Brake button is disabled if vehicle is disarmed """
self.mgr.shotMgr.appMgr.isAppConnected.return_value = True
self.mgr.isButtonConnected = Mock(return_value=True)
self.mgr.shotMgr.currentShot = shots.APP_SHOT_NONE
self.v.armed = False
self.mgr.setButtonMappings()
call1 = call(btn_msg.ButtonLoiter, shots.APP_SHOT_NONE, 0, "\0")
self.mgr.setArtooButton.assert_has_calls( [call1] )
def testAppDisarmedVehicleEKFNoBrake(self):
""" Brake button is disabled if vehicle is disarmed """
self.mgr.shotMgr.appMgr.isAppConnected.return_value = True
self.mgr.isButtonConnected = Mock(return_value=True)
self.mgr.shotMgr.currentShot = shots.APP_SHOT_NONE
self.v.armed = False
self.v.ekf_ok = True
self.mgr.setButtonMappings()
call1 = call(btn_msg.ButtonLoiter, shots.APP_SHOT_NONE, 0, "\0")
self.mgr.setArtooButton.assert_has_calls( [call1] )
def testAppArmedVehicleEKFBrake(self):
""" Brake button is enabled if vehicle is armed and we have EKF """
self.mgr.shotMgr.isAppConnected.return_value = True
self.mgr.isButtonConnected = Mock(return_value=True)
self.mgr.shotMgr.currentShot = shots.APP_SHOT_NONE
self.v.armed = True
self.v.ekf_ok = True
self.mgr.setButtonMappings()
call1 = call(btn_msg.ButtonLoiter, shots.APP_SHOT_NONE, btn_msg.ARTOO_BITMASK_ENABLED, "\0")
self.mgr.setArtooButton.assert_has_calls( [call1] )
@patch('threading.Thread')
class TestSetFreeButtonMappings(unittest.TestCase):
def setUp(self):
with patch('buttonManager.buttonManager.connect') as mock:
mock.read = Mock()
mock.get = Mock()
shotmgr = Mock()
self.v = Mock()
shotmgr.vehicle = self.v
self.mgr = buttonManager.buttonManager(shotmgr)
self.mgr.setArtooButton = Mock()
buttonManager.connected = True
self.mgr.freeButtonMappings = [(-1, -1), (-1, -1)]
self.mgr.setButtonMappings = Mock()
def testSetACableCam(self, mockThread):
""" Set A to cable cam """
self.mgr.setFreeButtonMapping( btn_msg.ButtonA, shots.APP_SHOT_CABLECAM, -1)
self.mgr.setButtonMappings.assert_called_with()
self.assertEqual( self.mgr.freeButtonMappings, [(shots.APP_SHOT_CABLECAM, -1), (-1, -1)])
def testSetBManual(self, mockThread):
""" Set B to manual """
self.mgr.setFreeButtonMapping( btn_msg.ButtonB, -1, 2)
self.mgr.setButtonMappings.assert_called_with()
self.assertEqual( self.mgr.freeButtonMappings, [(-1, -1), (-1, 2)])
def testSetFlyManual(self, mockThread):
""" Shouldn't allow setting of Fly button """
self.mgr.setFreeButtonMapping( btn_msg.ButtonFly, -1, 2)
self.assertFalse( self.mgr.setButtonMappings.called )
self.assertEqual( self.mgr.freeButtonMappings, [(-1, -1), (-1, -1)])
def testSetInvalidShot(self, mockThread):
""" Shouldn't allow setting an invalid shot """
self.mgr.setFreeButtonMapping( btn_msg.ButtonA, 100, -1)
self.assertFalse( self.mgr.setButtonMappings.called )
self.assertEqual( self.mgr.freeButtonMappings, [(-1, -1), (-1, -1)])
def testSetInvalidMode(self, mockThread):
""" Shouldn't allow setting an invalid mode """
self.mgr.setFreeButtonMapping( btn_msg.ButtonB, -1, 20)
self.assertFalse( self.mgr.setButtonMappings.called )
self.assertEqual( self.mgr.freeButtonMappings, [(-1, -1), (-1, -1)])

536
shotmanager/Test/TestCableCam.py Normal file
View File

@ -0,0 +1,536 @@
# Unit tests for CableCamShot
import math
import mock
from mock import call
from mock import Mock
from mock import patch
import os
from os import sys, path
from pymavlink import mavutil
import struct
import unittest
from dronekit import LocationGlobalRelative, Vehicle
sys.path.append(os.path.realpath('..'))
import location_helpers
import cable_cam
from cable_cam import CableCamShot
from shotManagerConstants import *
import shots
# on host systems these files are located here
from sololink import btn_msg
import app_packet
ERROR = 0.1
class TestRecordLocation(unittest.TestCase):
def setUp(self):
self.v = mock.create_autospec(Vehicle)
mgr = Mock(spec = ["sendPacket", "remapper", "appMgr"])
mgr.buttonManager = Mock()
mgr.getParam = Mock(return_value=500.0)
self.controller = CableCamShot(self.v, mgr)
def testRecordingLocations(self):
""" Test recording locations """
self.v.location.global_relative_frame = LocationGlobalRelative(37.242124, -122.12841, 15.3)
self.controller.recordLocation()
self.v.location.global_relative_frame = LocationGlobalRelative(30.13241, 10.112135, 0.0)
self.controller.recordLocation()
self.v.location.global_relative_frame = LocationGlobalRelative(-14.654861, 108.4645, 32.6545)
self.controller.recordLocation()
self.assertTrue( len(self.controller.waypoints) == 3 )
def testRecordingSameLocations(self):
""" Test recording the same locations """
self.v.location.global_relative_frame = LocationGlobalRelative(37.242124, -122.12841, 15.3)
self.controller.recordLocation()
self.controller.recordLocation()
self.controller.recordLocation()
self.assertTrue( len(self.controller.waypoints) == 1 )
def testCallSetButtonMappings(self):
""" Make sure setButtonMappings is called when recording a location """
self.controller.setButtonMappings = Mock()
self.v.location.global_relative_frame = LocationGlobalRelative(37.242124, -122.12841, 15.3)
self.controller.recordLocation()
self.controller.setButtonMappings.assert_called_with()
self.controller.setButtonMappings = Mock()
self.v.location.global_relative_frame = LocationGlobalRelative(30.13241, 10.112135, 0.0)
self.controller.recordLocation()
self.controller.setButtonMappings.assert_called_with()
class TestHandleRCsNoWaypoints(unittest.TestCase):
def testRCsMax(self):
""" Test HandleRCs no waypoints """
mgr = Mock(spec = ["sendPacket", "remapper"])
mgr.buttonManager = Mock()
mgr.getParam = Mock(return_value=500.0)
self.mock_vehicle = mock.create_autospec(Vehicle)
self.controller = CableCamShot(self.mock_vehicle, mgr)
channels = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
self.controller.handleRCs(channels)
class TestHandleRCs(unittest.TestCase):
def setUp(self):
mgr = Mock(spec = ["sendPacket", "remapper", "rcMgr", "appMgr"])
mgr.currentShot = shots.APP_SHOT_CABLECAM
mgr.buttonManager = Mock()
mgr.getParam = Mock(return_value=500.0)
self.mock_vehicle = mock.create_autospec(Vehicle)
self.controller = CableCamShot(self.mock_vehicle, mgr)
self.mock_vehicle.location.global_relative_frame = LocationGlobalRelative(37.242124, -122.12841, 15.3)
self.controller.recordLocation()
self.mock_vehicle.location.global_relative_frame = LocationGlobalRelative(30.13241, 10.112135, 0.0)
self.controller.recordLocation()
self.controller.yawPitchOffsetter = Mock()
self.controller.yawPitchOffsetter.yawOffset = 2.0
def testRCsMax(self):
""" Test RCs Max """
channels = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
self.controller.handleRCs(channels)
def testRCsMin(self):
""" Test RCs Min """
channels = [-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0]
self.controller.handleRCs(channels)
def testRCsZero(self):
""" Test RCs Max """
channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.controller.handleRCs(channels)
def testRCs2(self):
""" Test RCs Max """
channels = [0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2]
self.controller.handleRCs(channels)
def testIsNearTarget(self):
''' Test that pause is activated when we reach a target '''
channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.controller.handleRCs(channels)
self.controller.pathHandler.isNearTarget = Mock(return_value = True)
self.controller.pathHandler.pause = Mock()
self.controller.handleRCs(channels)
self.controller.pathHandler.pause.assert_called_with()
class TestHandleRCsSetupTargeting(unittest.TestCase):
def setUp(self):
mgr = Mock(spec = ["sendPacket", "remapper", "appMgr"])
mgr.currentShot = shots.APP_SHOT_CABLECAM
mgr.buttonManager = Mock()
mgr.getParam = Mock(return_value=500.0)
self.mock_vehicle = mock.create_autospec(Vehicle)
self.controller = CableCamShot(self.mock_vehicle, mgr)
self.mock_vehicle.location.global_relative_frame = LocationGlobalRelative(37.242124, -122.12841, 15.3)
self.controller.recordLocation()
self.mock_vehicle.location.global_relative_frame = LocationGlobalRelative(30.13241, 10.112135, 0.0)
self.controller.recordLocation()
self.controller.setupTargeting = Mock()
def testSetupTargetingCalled(self):
""" setupTargeting should be called when we get into guided """
self.mock_vehicle.mode.name = "GUIDED"
channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.controller.handleRCs(channels)
self.controller.setupTargeting.assert_called_with()
self.assertTrue( self.controller.haveSetupTargeting )
def testSetupTargetingNotCalled(self):
""" Don't call this if we're not yet in guided """
self.mock_vehicle.mode.name = "LOITER"
channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.controller.handleRCs(channels)
self.assertFalse( self.controller.setupTargeting.called )
self.assertFalse( self.controller.haveSetupTargeting )
def testSetupTargetingAlready(self):
""" Already set up targeting """
self.mock_vehicle.mode.name = "GUIDED"
channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.controller.haveSetupTargeting = True
self.controller.handleRCs(channels)
self.assertFalse( self.controller.setupTargeting.called )
class TestHandleRCsInterpolation(unittest.TestCase):
def setUp(self):
mgr = Mock(spec = ["sendPacket", "remapper"])
mgr.buttonManager = Mock()
mgr.getParam = Mock(return_value=500.0)
self.mock_vehicle = mock.create_autospec(Vehicle)
self.controller = CableCamShot(self.mock_vehicle, mgr)
self.controller.pathHandler = Mock()
self.controller.pathHandler.MoveTowardsEndpt = Mock(return_value = (0.0, True))
self.controller.yawPitchOffsetter = Mock()
self.controller.waypoints = [2, 2]
self.controller.InterpolateCamera = Mock()
self.controller.updateAppOptions = Mock()
self.controller.handleFreePitchYaw = Mock()
def testInterpolateCam(self):
""" InterpolateCamera should be called if the option is on """
channels = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
self.controller.handleRCs(channels)
self.controller.yawPitchOffsetter.Update.assert_called_with(channels)
self.controller.InterpolateCamera.assert_called_with(False)
self.assertFalse( self.controller.handleFreePitchYaw.called )
def testFreePitchYaw(self):
""" If the option is off, should call HandleFreePitchYaw """
channels = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
self.controller.camInterpolation = 0
self.controller.haveSetupTargeting = True
self.controller.handleRCs(channels)
self.controller.yawPitchOffsetter.Update.assert_called_with(channels)
self.controller.handleFreePitchYaw.assert_called_with()
self.assertFalse( self.controller.InterpolateCamera.called )
class TestSetButtonMappings(unittest.TestCase):
def setUp(self):
self.v = mock.create_autospec(Vehicle)
self.v.location.global_relative_frame = LocationGlobalRelative(30.13241, 10.112135, 0.0)
self.mockMgr = Mock()
self.mockMgr.buttonManager = Mock()
self.mockMgr.getParam = Mock(return_value=500.0)
self.controller = CableCamShot(self.v, self.mockMgr)
def testSetButtonMappingsNoWaypoints(self):
""" Testing setting button mappings when we have no recorded points """
self.controller.setButtonMappings()
calls = [call(btn_msg.ButtonA, shots.APP_SHOT_CABLECAM, btn_msg.ARTOO_BITMASK_ENABLED, "Record Point\0"), call(btn_msg.ButtonB, shots.APP_SHOT_CABLECAM, 0, "\0")]
self.mockMgr.buttonManager.setArtooButton.assert_has_calls(calls, any_order = False)
def testSetButtonMappings1Waypoint(self):
""" Testing setting button mappings when we have one recorded point """
self.controller.recordLocation()
self.controller.setButtonMappings()
calls = [call(btn_msg.ButtonA, shots.APP_SHOT_CABLECAM, 0, "\0"), call(btn_msg.ButtonB, shots.APP_SHOT_CABLECAM, btn_msg.ARTOO_BITMASK_ENABLED, "Record Point\0")]
self.mockMgr.buttonManager.setArtooButton.assert_has_calls(calls, any_order = False)
def testSetButtonMappings(self):
""" Testing setting button mappings when we have locked on """
self.controller.recordLocation()
self.v.location.global_relative_frame = LocationGlobalRelative(50.13241, 10.112135, 0.0)
self.controller.recordLocation()
self.controller.setButtonMappings()
calls = [call(btn_msg.ButtonA, shots.APP_SHOT_CABLECAM, 0, "\0"), call(btn_msg.ButtonB, shots.APP_SHOT_CABLECAM, 0, "\0")]
self.mockMgr.buttonManager.setArtooButton.assert_has_calls(calls, any_order = False)
class TestHandleButton(unittest.TestCase):
def setUp(self):
self.v = mock.create_autospec(Vehicle)
self.v.location.global_relative_frame = LocationGlobalRelative(48.6548694, 10.0, 0.5)
self.v.mount_status = [-80]
self.mockMgr = Mock()
self.mockMgr.getParam = Mock(return_value=500.0)
self.mockMgr.buttonManager = Mock()
self.controller = CableCamShot(self.v, self.mockMgr)
self.controller.updateAppOptions = Mock()
def TestHandleAButtonRecordPoint(self):
""" This should record a cable endpoint """
self.controller.waypoints = []
self.controller.recordLocation = Mock()
self.controller.handleButton( btn_msg.ButtonA, btn_msg.Press )
self.controller.recordLocation.assert_called_with()
def TestHandleBButtonRecordPoint(self):
""" With one point, button B should record a point """
self.controller.waypoints = [5]
self.controller.recordLocation = Mock()
self.controller.handleButton( btn_msg.ButtonB, btn_msg.Press )
self.controller.recordLocation.assert_called_with()
def TestHandleAButtonDontRecordPoint(self):
""" Already have two points, don't record a point """
self.controller.waypoints = [1, 2]
self.controller.recordLocation = Mock()
self.controller.handleButton( btn_msg.ButtonA, btn_msg.Press )
self.assertFalse( self.controller.recordLocation.called )
def TestPauseCruise(self):
""" pause button pauses cruising """
self.controller.pathHandler = Mock()
self.controller.pathHandler.cruiseSpeed = 8.0
self.controller.updateAppOptions = Mock()
self.controller.pathHandler.isPaused = Mock(return_value = False)
self.controller.handleButton( btn_msg.ButtonLoiter, btn_msg.Press )
self.controller.pathHandler.togglePause.assert_called_with()
self.controller.updateAppOptions.assert_called_with()
def TestResumeCruise(self):
'''pause button resume cruising if already in pause'''
self.controller.pathHandler = Mock()
self.controller.pathHandler.cruiseSpeed = 0.0
self.controller.handleButton( btn_msg.ButtonLoiter, btn_msg.Press )
self.controller.pathHandler.togglePause.assert_called_with()
self.controller.updateAppOptions.assert_called_with()
def TestNotifyPauseLoiter(self):
self.controller.pathHandler = None
self.controller.handleButton(btn_msg.ButtonLoiter, btn_msg.Press)
self.controller.shotmgr.notifyPause.assert_called_with(False)
def TestNotifyPauseGuided(self):
self.controller.pathHandler = Mock()
self.controller.handleButton(btn_msg.ButtonLoiter, btn_msg.Press)
self.controller.shotmgr.notifyPause.assert_called_with(True)
class TestInterpolateCamera(unittest.TestCase):
def setUp(self):
self.v = mock.create_autospec(Vehicle)
self.v.location.global_relative_frame = LocationGlobalRelative(-48.5468695, 5.68464, 10.5)
self.v.mount_status = [-80]
self.mockMgr = Mock()
self.mockMgr.buttonManager = Mock()
self.mockMgr.getParam = Mock(return_value=500.0)
self.controller = CableCamShot(self.v, self.mockMgr)
loc2 = location_helpers.newLocationFromAzimuthAndDistance(self.v.location.global_relative_frame, 23.4, 25.0)
self.startYaw = 12.4
self.startPitch = -16.7
waypt1 = cable_cam.Waypoint( loc2, self.startYaw, self.startPitch )
self.controller.waypoints.append( waypt1 )
self.endYaw = 175.4
self.endPitch = -83.4
waypt2 = cable_cam.Waypoint( self.v.location.global_relative_frame, self.endYaw, self.endPitch )
self.controller.waypoints.append( waypt2 )
self.controller.deadReckoningTicks = 0
self.controller.accel = 0.0
self.controller.totalDistance = location_helpers.getDistanceFromPoints3d(
self.v.location.global_relative_frame, loc2)
self.v.message_factory = Mock()
# turn off dead reckoning
self.v.groundspeed = 0.0
self.controller.desiredSpeed = 0.0
def TestInterp0(self):
""" Test when we're at 0 on the cable """
self.controller.lastPerc = 0.0
with patch('location_helpers.getDistanceFromPoints3d', return_value=0.0):
self.controller.InterpolateCamera( True )
self.v.message_factory.mount_control_encode.assert_called_with(
0, 1, # target system, target component
self.startPitch * 100, # pitch
0.0, # roll
1239.9999999999977, # yaw
0 # save position
)
def TestInterp100(self):
""" Test when we're at 100 on the cable """
self.controller.lastPerc = 1.0
with patch('location_helpers.getDistanceFromPoints3d', return_value=self.controller.totalDistance):
self.controller.InterpolateCamera( True )
self.v.message_factory.mount_control_encode.assert_called_with(
0, 1, # target system, target component
self.endPitch * 100, # pitch
0.0, # roll
self.endYaw * 100, # yaw
0 # save position
)
def TestInterp44(self):
""" Test when we're at 44 on the cable """
self.controller.lastPerc = 0.44
self.controller.yawDirection = 1
with patch('location_helpers.getDistanceFromPoints3d', return_value=self.controller.totalDistance * 0.44):
self.controller.InterpolateCamera( True )
pitch = ( ( self.startPitch * 0.56 ) + ( self.endPitch * 0.44) ) * 100
yaw = ( ( self.startYaw * 0.56 ) + ( self.endYaw * 0.44) ) * 100
self.v.message_factory.mount_control_encode.assert_called_with(
0, 1, # target system, target component
pitch, # pitch
0.0, # roll
yaw, # yaw
0 # save position
)
def TestInterp23NoGimbal(self):
""" Test when we're at 23\% on the cable, no gimbal """
self.controller.lastPerc = 0.23
self.controller.yawDirection = 1
self.v.mount_status = [None]
with patch('location_helpers.getDistanceFromPoints3d', return_value=self.controller.totalDistance * 0.23):
self.controller.InterpolateCamera( True )
yaw = ( ( self.startYaw * 0.77 ) + ( self.endYaw * 0.23) )
self.assertFalse( self.v.message_factory.mount_control_encode.called )
self.v.message_factory.command_long_encode.assert_called_with(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
0, #confirmation
yaw, # param 1 - target angle
cable_cam.YAW_SPEED, # param 2 - yaw speed
1, # param 3 - direction
0.0, # relative offset
0, 0, 0 # params 5-7 (unused)
)
class TestHandlePacket(unittest.TestCase):
def setUp(self):
self.v = mock.create_autospec(Vehicle)
self.v.location.global_relative_frame = LocationGlobalRelative(30.13241, 10.112135, 0.0)
self.mockMgr = Mock()
self.mockMgr.buttonManager = Mock()
self.mockMgr.getParam = Mock(return_value=500.0)
self.controller = CableCamShot(self.v, self.mockMgr)
self.controller.pathHandler = Mock()
self.controller.setupTargeting = Mock()
def testHandleCamInterpolationOn(self):
""" Should call setup targeting """
self.controller.camInterpolation = 0
options = struct.pack('<HHf', 1, 1, 2.3)
self.controller.handlePacket(app_packet.SOLO_CABLE_CAM_OPTIONS, 8, options)
self.controller.setupTargeting.assert_called_with()
def testHandleCamInterpolationSame(self):
""" Don't call setup targeting if we have that value already """
self.controller.camInterpolation = 1
options = struct.pack('<HHf', 1, 1, 2.3)
self.controller.handlePacket(app_packet.SOLO_CABLE_CAM_OPTIONS, 8, options)
self.assertFalse( self.controller.setupTargeting.called )
def testSetYawDir(self):
""" Should set our yaw direction """
options = struct.pack('<HHf', 1, 1, 2.3)
self.controller.handlePacket(app_packet.SOLO_CABLE_CAM_OPTIONS, 8, options)
self.assertEqual( self.controller.yawDirection, 1 )
def testSetCruiseSpeed(self):
""" Should set our cruise speed """
options = struct.pack('<HHf', 1, 1, 2.299999952316284)
self.controller.handlePacket(app_packet.SOLO_CABLE_CAM_OPTIONS, 8, options)
self.controller.pathHandler.setCruiseSpeed.assert_called_with( 2.299999952316284 )
def testNoPathHandler(self):
""" Should early out """
self.controller.camInterpolation = 0
options = struct.pack('<HHf', 1, 1, 2.3)
self.controller.pathHandler = None
self.controller.handlePacket(app_packet.SOLO_CABLE_CAM_OPTIONS, 8, options)
self.assertFalse( self.controller.setupTargeting.called )
class TestSetupTargeting(unittest.TestCase):
def setUp(self):
self.v = mock.create_autospec(Vehicle)
self.v.location.global_relative_frame = LocationGlobalRelative(30.13241, 10.112135, 0.0)
self.mockMgr = Mock()
self.mockMgr.currentShot = shots.APP_SHOT_CABLECAM
self.mockMgr.getParam = Mock(return_value=500.0)
self.mockMgr.buttonManager = Mock()
self.controller = CableCamShot(self.v, self.mockMgr)
self.mockMgr.remapper = Mock()
self.controller.yawPitchOffsetter = Mock()
self.controller.yawPitchOffsetter.enableNudge = Mock()
self.controller.yawPitchOffsetter.disableNudge = Mock()
def testInterpolationOn(self):
""" If we turn on camera interpolation """
self.controller.camInterpolation = 1
self.controller.setupTargeting()
self.v.message_factory.mount_configure_encode.assert_called_with(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, #mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
self.mockMgr.rcMgr.enableRemapping.assert_called_with( True )
self.controller.yawPitchOffsetter.enableNudge.assert_called_with()
def testInterpolationOff(self):
""" If we turn off camera interpolation """
self.controller.camInterpolation = 0
self.v.mount_status = [ -15.6, 27.8, 0.0 ]
self.v.attitude.yaw = math.radians(27.8)
self.controller.setupTargeting()
self.v.message_factory.mount_configure_encode.assert_called_with(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, #mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
self.mockMgr.rcMgr.enableRemapping.assert_called_with( True )
self.controller.yawPitchOffsetter.disableNudge.assert_called_with(-15.6, 27.8)
def testInterpolationOffNoGimbal(self):
""" If we turn off camera interpolation without a gimbal """
self.controller.camInterpolation = 0
self.v.mount_status = [ None, None, 0.0 ]
self.v.attitude.yaw = 1.3337
self.controller.setupTargeting()
self.v.message_factory.mount_configure_encode.assert_called_with(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, #mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
self.mockMgr.rcMgr.enableRemapping.assert_called_with( True )
self.controller.yawPitchOffsetter.disableNudge.assert_called_with(0.0, math.degrees(1.3337))
class TestHandleFreePitchYaw(unittest.TestCase):
def setUp(self):
mgr = Mock(spec = ["sendPacket", "remapper"])
mgr.buttonManager = Mock()
mgr.getParam = Mock(return_value=500.0)
self.v = mock.create_autospec(Vehicle)
self.controller = CableCamShot(self.v, mgr)
def testWithGimbal(self):
""" With a gimbal, use mount_control to control pitch/yaw """
self.v.mount_status = [20.0, 0.0, 1.3]
self.controller.yawPitchOffsetter = Mock()
yaw = float( 1.111333 )
pitch = float (2.55556666 )
self.controller.yawPitchOffsetter.yawOffset = yaw
self.controller.yawPitchOffsetter.pitchOffset = pitch
self.controller.handleFreePitchYaw()
self.v.message_factory.mount_control_encode.assert_called_with(
0, 1, # target system, target component
pitch * 100, # pitch
0.0, # roll
yaw * 100, # yaw
0 # save position
)
def testNoGimbal(self):
""" Without a gimbal, we only use condition_yaw to control """
# no gimbal
self.v.mount_status = [None, None, None]
self.controller.yawPitchOffsetter = Mock()
yaw = float( -27.283475 )
pitch = float ( 14.4444 )
self.controller.yawPitchOffsetter.yawOffset = yaw
self.controller.yawPitchOffsetter.pitchOffset = pitch
self.controller.yawPitchOffsetter.yawDir = 1
self.controller.handleFreePitchYaw()
self.v.message_factory.command_long_encode.assert_called_with(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
0, #confirmation
yaw, # param 1 - target angle
cable_cam.YAW_SPEED, # param 2 - yaw speed
self.controller.yawPitchOffsetter.yawDir, # param 3 - direction
0.0, # relative offset
0, 0, 0 # params 5-7 (unused)
)

577
shotmanager/Test/TestCableController.py Normal file
View File

@ -0,0 +1,577 @@
# TestCableController.py
# shotmanager
#
# Unit tests for the cable controller.
#
# Created by Will Silva on 1/22/2015.
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import cableController
from cableController import *
import multipoint
from multipoint import *
from dronekit import LocationGlobalRelative
import unittest
import mock
from mock import call
from mock import Mock
from mock import MagicMock
from mock import patch
class TestPublicInterface(unittest.TestCase):
def setUp(self):
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
maxSpeed = MAX_SPEED
minSpeed = MIN_CRUISE_SPEED
tanAccelLim = TANGENT_ACCEL_LIMIT
normAccelLim = NORM_ACCEL_LIMIT
smoothStopP = 0.7
maxAlt = 50
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
self.controller.killCurvatureMapThread()
self.controller.curvatureMapThread.join()
def testReachedTarget(self):
'''Tests that reachedTarget works'''
retVal = self.controller.reachedTarget()
self.assertEqual(retVal, True)
def testSetTargetP(self):
'''Test that setTargetP works'''
self.controller.setTargetP(.3)
self.assertEqual(self.controller.targetP, .3)
def testTrackSpeed(self):
'''Test that trackSpeed works'''
self.controller.trackSpeed(4)
self.assertEqual(self.controller.desiredSpeed, 4)
@mock.patch('cableController.constrain', return_value = 3)
def testUpdate(self, constrain):
'''Test that update works'''
#Mock _traverse
self.controller._traverse = Mock()
self.controller.update(UPDATE_TIME)
self.assertEqual(self.controller.desiredSpeed, 0)
self.assertEqual(self.controller.speed, 3)
self.controller._traverse.assert_called_with(UPDATE_TIME)
def testSetCurrentP(self):
'''Test that setCurrentP works'''
self.controller.setCurrentP(0.3)
self.assertEqual(self.controller.currentP, 0.3)
def testKillCurvatureMapThread(self):
'''Test that poisonPill is set to True'''
self.controller.killCurvatureMapThread()
self.assertEqual(self.controller.poisonPill, True)
class Test_computeCurvatureMap(unittest.TestCase):
def setUp(self):
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
maxSpeed = MAX_SPEED
minSpeed = MIN_CRUISE_SPEED
tanAccelLim = TANGENT_ACCEL_LIMIT
normAccelLim = NORM_ACCEL_LIMIT
smoothStopP = 0.7
maxAlt = 50
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
self.controller.killCurvatureMapThread() #send thread poison pill
self.controller.curvatureMapThread.join() #wait for thread to die
self.controller.curvatureMapSpeedLimits[0] = None
def testPositiveSpeed(self):
'''Test when self.speed is positive'''
self.controller.speed = 3
self.controller._computeCurvatureMapSpeedLimit = Mock(side_effect=[False,False,True])
self.controller.poisonPill = True
self.controller._computeCurvatureMap()
def testNegativeSpeed(self):
'''Test when self.speed is negative'''
self.controller.speed = -3
self.controller._computeCurvatureMapSpeedLimit = Mock(side_effect=[False,False,True])
self.controller.poisonPill = True
self.controller._computeCurvatureMap()
def testZeroSpeed(self):
'''Test when self.speed is zero'''
self.controller.speed = 0
self.controller._computeCurvatureMapSpeedLimit = Mock(side_effect=[False,False,True])
self.controller.poisonPill = True
self.controller._computeCurvatureMap()
class Test_computeCurvatureMapSpeedLimit(unittest.TestCase):
def setUp(self):
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
maxSpeed = MAX_SPEED
minSpeed = MIN_CRUISE_SPEED
tanAccelLim = TANGENT_ACCEL_LIMIT
normAccelLim = NORM_ACCEL_LIMIT
smoothStopP = 0.7
maxAlt = 50
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
self.controller.killCurvatureMapThread() #send thread poison pill
self.controller.curvatureMapThread.join() #wait for thread to die
self.controller.curvatureMapSpeedLimits[0] = None
def testSpeedLimitNotNone(self):
'''Test when speed limit is already computed'''
self.controller.curvatureMapSpeedLimits[0] = 3
retVal = self.controller._computeCurvatureMapSpeedLimit(0)
self.assertEqual(retVal, False)
def testCurrentMapSegJointIsNone(self):
'''Test when non dimensional index for the joint has not been computed yet'''
self.controller.curvatureMapJointsNonDimensional[0] = None
self.controller._computeCurvatureMapSpeedLimit(0)
assert self.controller.curvatureMapJointsNonDimensional[0] is not None
def testCurrentMapSegJointPlusOneIsNone(self):
'''Test when non dimensional index for the joint has not been computed yet'''
self.controller.curvatureMapJointsNonDimensional[1] = None
self.controller._computeCurvatureMapSpeedLimit(0)
assert self.controller.curvatureMapJointsNonDimensional[1] is not None
def testAltitudeLimit(self):
'''Tests that altitude limit breach sets maxAltExceeded flag'''
self.controller.posZLimit = 10 # meters
self.controller.spline.position = Mock(return_value = Vector3(0,0,-11)) # meters
self.controller._computeCurvatureMapSpeedLimit(0)
self.assertEqual(self.controller.maxAltExceeded, True)
def testAltitudeLimitDisabled(self):
'''Tests that altitude limit breach does not set maxAltExceeded flag if altitude limit is disabled'''
self.controller.posZLimit = None
self.controller.spline.position = Mock(return_value = Vector3(0,0,-11)) # meters
self.controller._computeCurvatureMapSpeedLimit(0)
self.assertEqual(self.controller.maxAltExceeded, False)
def testNonZeroMaxCurvature(self):
'''Test that a speed limit is calculated based on non-zero curvature'''
self.controller.spline.curvature = Mock(return_value = 3.)
self.controller._computeCurvatureMapSpeedLimit(0)
self.assertEqual(self.controller.curvatureMapSpeedLimits[0], math.sqrt(NORM_ACCEL_LIMIT/3.))
def testMinSpeedLimit(self):
'''Test that a speed limit is capped at minimum for high curvature'''
self.controller.spline.curvature = Mock(return_value = 300.)
self.controller._computeCurvatureMapSpeedLimit(0)
self.assertEqual(self.controller.curvatureMapSpeedLimits[0], MIN_CRUISE_SPEED)
def testZeroCurvature(self):
'''Test that speed limit is set to MAX_SPEED if max curvature is zero'''
self.controller.spline.curvature = Mock(return_value = 0.)
self.controller._computeCurvatureMapSpeedLimit(0)
self.assertEqual(self.controller.curvatureMapSpeedLimits[0], MAX_SPEED)
class Test_getCurvatureMapSpeedLimit(unittest.TestCase):
def setUp(self):
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
maxSpeed = MAX_SPEED
minSpeed = MIN_CRUISE_SPEED
tanAccelLim = TANGENT_ACCEL_LIMIT
normAccelLim = NORM_ACCEL_LIMIT
smoothStopP = 0.7
maxAlt = 50
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
self.controller.killCurvatureMapThread() #send thread poison pill
self.controller.curvatureMapThread.join() #wait for thread to die
def testSegmentLessThanZero(self):
'''Test that 0. is returned if segment requested is less than zero'''
retVal = self.controller._getCurvatureMapSpeedLimit(-1)
self.assertEqual(retVal, 0.)
def testSegmentGreaterThanNumOfSegments(self):
'''Test that 0. is returned if segment requested is greater than available number of map segments'''
self.controller.curvatureMapNumSegments = 3
retVal = self.controller._getCurvatureMapSpeedLimit(4)
self.assertEqual(retVal, 0.)
class Test_traverse(unittest.TestCase):
def setUp(self):
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
maxSpeed = MAX_SPEED
minSpeed = MIN_CRUISE_SPEED
tanAccelLim = TANGENT_ACCEL_LIMIT
normAccelLim = NORM_ACCEL_LIMIT
smoothStopP = 0.7
maxAlt = 50
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
self.controller.killCurvatureMapThread() #send thread poison pill
self.controller.curvatureMapThread.join() #wait for thread to die
def testCurrentUGreaterThanOneNotLastSegment(self):
'''Test that we advance segments if currentU is greater than 1 and it's not the last segment'''
self.controller.currentSeg = 0
self.controller.currentU = 1.0
self.controller.speed = MAX_SPEED
self.controller._traverse(UPDATE_TIME)
self.assertEqual(self.controller.currentSeg, 1)
self.assertEqual(self.controller.currentU , 0.)
def testCurrentUGreaterThanOneLastSegment(self):
'''Test that we do NOT advance segments if currentU is greater than 1 and it IS the last segment'''
self.controller.currentSeg = 2
self.controller.currentU = 1.0
self.controller.speed = MAX_SPEED
self.controller._traverse(UPDATE_TIME)
self.assertEqual(self.controller.currentSeg, 2)
self.assertEqual(self.controller.currentU , 1.)
def testCurrentULessThanZeroNotFirstSegment(self):
'''Test that we regress segments if currentU is less than 0 and it's not the first segment'''
self.controller.currentSeg = 2
self.controller.currentU = 0.0
self.controller.speed = -MAX_SPEED
self.controller._traverse(UPDATE_TIME)
self.assertEqual(self.controller.currentSeg, 1)
self.assertEqual(self.controller.currentU , 1.)
def testCurrentULessThanZeroFirstSegment(self):
'''Test that we do NOT regress segments if currentU is less than 0 and it IS the first segment'''
self.controller.currentSeg = 0
self.controller.currentU = 0.0
self.controller.speed = -MAX_SPEED
self.controller._traverse(UPDATE_TIME)
self.assertEqual(self.controller.currentSeg, 0)
self.assertEqual(self.controller.currentU , 0.)
class Test_constrainSpeed(unittest.TestCase):
def setUp(self):
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
maxSpeed = MAX_SPEED
minSpeed = MIN_CRUISE_SPEED
tanAccelLim = TANGENT_ACCEL_LIMIT
normAccelLim = NORM_ACCEL_LIMIT
smoothStopP = 0.7
maxAlt = 50
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
self.controller.killCurvatureMapThread() #send thread poison pill
self.controller.curvatureMapThread.join() #wait for thread to die
def testMaxSpeedGreaterThanSpeedLimitGreaterThanSpeedGreaterThanZero(self):
'''Test when 0 < speed < speed limit < max speed'''
self.controller._getPosSpeedLimit = Mock(return_value = 6.)
retVal = self.controller._constrainSpeed(4.)
self.assertEqual(retVal, 4.)
def testMaxSpeedGreaterThanSpeedGreaterThanSpeedLimitGreaterThanZero(self):
'''Test when 0 < speed limit < speed < max speed'''
self.controller._getPosSpeedLimit = Mock(return_value = 3.)
retVal = self.controller._constrainSpeed(4.)
self.assertEqual(retVal, 3.)
def testSpeedGreaterThanMaxSpeedGreaterThanSpeedLimitGreaterThanZero(self):
'''Test when 0 < speed limit < max speed < speed'''
self.controller._getPosSpeedLimit = Mock(return_value = 3.)
retVal = self.controller._constrainSpeed(9.)
self.assertEqual(retVal, 3.)
def testSpeedGreaterThanSpeedLimitGreaterThanMaxSpeedGreaterThanZero(self):
'''Test when 0 < max speed < speed limit < speed'''
self.controller._getPosSpeedLimit = Mock(return_value = 10.)
retVal = self.controller._constrainSpeed(9.)
self.assertEqual(retVal, MAX_SPEED)
def testSpeedLessThanSpeedLimitLessThanMaxSpeedLessThanZero(self):
'''Test when speed < speed limit < max speed < 0'''
self.controller._getNegSpeedLimit = Mock(return_value = -9.)
retVal = self.controller._constrainSpeed(-10.)
self.assertEqual(retVal, -MAX_SPEED)
def testSpeedLimitLessThanSpeedLessThanMaxSpeedLessThanZero(self):
'''Test when speed limit < speed < max speed < 0'''
self.controller._getNegSpeedLimit = Mock(return_value = -9.)
retVal = self.controller._constrainSpeed(-8.5)
self.assertEqual(retVal, -MAX_SPEED)
def testSpeedLimitLessThanMaxSpeedLessThanSpeedLessThanZero(self):
'''Test when speed limit < max speed < speed < 0'''
self.controller._getNegSpeedLimit = Mock(return_value = -9.)
retVal = self.controller._constrainSpeed(-4.)
self.assertEqual(retVal, -4.)
def testMaxSpeedLessThanSpeedLessThanSpeedLimitLessThanZero(self):
'''Test when max speed < speed < speedLimit < 0'''
self.controller._getNegSpeedLimit = Mock(return_value = -4.)
retVal = self.controller._constrainSpeed(-5.)
self.assertEqual(retVal, -4.)
class Test_speedCurve(unittest.TestCase):
def setUp(self):
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
maxSpeed = MAX_SPEED
minSpeed = MIN_CRUISE_SPEED
tanAccelLim = TANGENT_ACCEL_LIMIT
normAccelLim = NORM_ACCEL_LIMIT
self.smoothStopP = 0.7
maxAlt = 50
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, self.smoothStopP, maxAlt)
self.controller.killCurvatureMapThread() #send thread poison pill
self.controller.curvatureMapThread.join() #wait for thread to die
def testSpeedGreaterThanLinearVelocity(self):
'''Test that if speed is greater than linear_velocity then return sqrt curve speed profile'''
lVelocity = TANGENT_ACCEL_LIMIT / self.smoothStopP
lDist = lVelocity / self.smoothStopP
speed = lVelocity + 1 #m/s
dist = 10 #meters
expectedVal = math.sqrt(2. * TANGENT_ACCEL_LIMIT * (speed**2/(2.*TANGENT_ACCEL_LIMIT) + dist))
retVal = self.controller._speedCurve(dist, speed)
self.assertEqual(retVal, expectedVal)
def testSpeedLessThanLinearVelocityAndP2GreaterThanLinearDist(self):
'''Test that if speed is less than linear_velocity and P2 is greater than linear distance then return linear speed profile'''
lVelocity = TANGENT_ACCEL_LIMIT / self.smoothStopP
lDist = lVelocity / self.smoothStopP
speed = lVelocity - 1 #m/s
p1 = speed / self.smoothStopP
p2 = lDist + 1
dist = p2 - p1 # meters
expectedVal = math.sqrt(2. * TANGENT_ACCEL_LIMIT * (p2 - 0.5*lDist))
retVal = self.controller._speedCurve(dist, speed)
self.assertEqual(retVal, expectedVal)
def testSpeedLessThanLinearVelocityAndP2LessThanLinearDist(self):
'''Test that if speed is less than linear_velocity and P2 is less than linear distance then return linear speed profile'''
lVelocity = TANGENT_ACCEL_LIMIT / self.smoothStopP
lDist = lVelocity / self.smoothStopP
speed = lVelocity - 1 #m/s
p1 = speed / self.smoothStopP
p2 = lDist - 1
dist = p2 - p1 # meters
expectedVal = p2 * self.smoothStopP
retVal = self.controller._speedCurve(dist, speed)
self.assertEqual(retVal, expectedVal)
class Test_maxLookAheadDist(unittest.TestCase):
def setUp(self):
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
maxSpeed = MAX_SPEED
minSpeed = MIN_CRUISE_SPEED
tanAccelLim = TANGENT_ACCEL_LIMIT
normAccelLim = NORM_ACCEL_LIMIT
self.smoothStopP = 0.7
maxAlt = 50
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, self.smoothStopP, maxAlt)
self.controller.killCurvatureMapThread() #send thread poison pill
self.controller.curvatureMapThread.join() #wait for thread to die
def testAbsSpeedGreaterThanLinearVelocity(self):
'''Test if absolute value of speed is greater than linear velocity then return distance required to stop at constant deceleration'''
lVelocity = TANGENT_ACCEL_LIMIT / self.smoothStopP
lDist = lVelocity / self.smoothStopP
self.controller.speed = lVelocity + 1
retVal = self.controller._maxLookAheadDist()
self.assertEqual(retVal, 0.5 * abs(self.controller.speed)**2 / TANGENT_ACCEL_LIMIT + 0.5*lDist)
def testAbsSpeedLessThanLinearVelocity(self):
'''Test if absolute value of speed is less than linear velocity then return distance required to stop at constant deceleration'''
lVelocity = TANGENT_ACCEL_LIMIT / self.smoothStopP
lDist = lVelocity / self.smoothStopP
self.controller.speed = lVelocity - 1
retVal = self.controller._maxLookAheadDist()
self.assertEqual(retVal, abs(self.controller.speed)/self.smoothStopP)
class Test_getCurvatureMapSegment(unittest.TestCase):
def setUp(self):
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
maxSpeed = MAX_SPEED
minSpeed = MIN_CRUISE_SPEED
tanAccelLim = TANGENT_ACCEL_LIMIT
normAccelLim = NORM_ACCEL_LIMIT
smoothStopP = 0.7
maxAlt = 50
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
self.controller.killCurvatureMapThread() #send thread poison pill
self.controller.curvatureMapThread.join() #wait for thread to die
def testPIsOneHalf(self):
'''Test if P = 0.5 for 100 segment map'''
self.controller.curvatureMapNumSegments = 100 # segments
self.controller.curvatureMapSegLengthP = 1./100 # p per map segment
retVal = self.controller._getCurvatureMapSegment(0.5)
self.assertEqual(retVal,50)
class Test_getDistToCurvatureMapSegmentBegin(unittest.TestCase):
def setUp(self):
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
maxSpeed = MAX_SPEED
minSpeed = MIN_CRUISE_SPEED
tanAccelLim = TANGENT_ACCEL_LIMIT
normAccelLim = NORM_ACCEL_LIMIT
smoothStopP = 0.7
maxAlt = 50
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
self.controller.killCurvatureMapThread() #send thread poison pill
self.controller.curvatureMapThread.join() #wait for thread to die
def testP1IsOneHalf(self):
'''Test if P1 = 0.5, P2 = 0.6, 100 meter cable'''
self.controller.spline.totalArcLength = 100 #meters
self.controller.curvatureMapJointsP[2] = 0.6 #p
retVal = self.controller._getDistToCurvatureMapSegmentBegin(0.5, 2)
self.assertAlmostEqual(retVal, 10)
class Test_getDistToCurvatureMapSegmentEnd(unittest.TestCase):
def setUp(self):
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
maxSpeed = MAX_SPEED
minSpeed = MIN_CRUISE_SPEED
tanAccelLim = TANGENT_ACCEL_LIMIT
normAccelLim = NORM_ACCEL_LIMIT
smoothStopP = 0.7
maxAlt = 50
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
self.controller.killCurvatureMapThread() #send thread poison pill
self.controller.curvatureMapThread.join() #wait for thread to die
def testP1IsOneHalf(self):
'''Test if P1 = 0.5, P2 = 0.4, 100 meter cable'''
self.controller.spline.totalArcLength = 100 #meters
self.controller.curvatureMapJointsP[2] = 0.4 #p
retVal = self.controller._getDistToCurvatureMapSegmentBegin(0.5, 2)
self.assertAlmostEqual(retVal, 10)
class Test_getPosSpeedLimit(unittest.TestCase):
def setUp(self):
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
maxSpeed = MAX_SPEED
minSpeed = MIN_CRUISE_SPEED
tanAccelLim = TANGENT_ACCEL_LIMIT
normAccelLim = NORM_ACCEL_LIMIT
smoothStopP = 0.7
maxAlt = 50
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
self.controller.killCurvatureMapThread() #send thread poison pill
self.controller.curvatureMapThread.join() #wait for thread to die
def testTargetPGreaterThanCurrentP(self):
'''Test if targetP is greater than currentP then speedLimit is compared with map segment speedlimit and speedCurve'''
self.controller.targetP = .5
self.controller.currentP = .3
self.controller._speedCurve = Mock(side_effect = [2.,1.])
retVal = self.controller._getPosSpeedLimit(self.controller.currentP)
self.assertEqual(retVal, 1.)
class Test_getNegSpeedLimit(unittest.TestCase):
def setUp(self):
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
maxSpeed = MAX_SPEED
minSpeed = MIN_CRUISE_SPEED
tanAccelLim = TANGENT_ACCEL_LIMIT
normAccelLim = NORM_ACCEL_LIMIT
smoothStopP = 0.7
maxAlt = 50
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
self.controller.killCurvatureMapThread() #send thread poison pill
self.controller.curvatureMapThread.join() #wait for thread to die
def testTargetPLessThanCurrentP(self):
'''Test if targetP is less than currentP then speedLimit is compared with map segment speedlimit and speedCurve'''
self.controller.targetP = .3
self.controller.currentP = .5
self.controller._speedCurve = Mock(side_effect = [2.,1.])
retVal = self.controller._getNegSpeedLimit(self.controller.currentP)
self.assertEqual(retVal, -1.)

65
shotmanager/Test/TestCamera.py Normal file
View File

@ -0,0 +1,65 @@
# Unit tests for camera
import math
import mock
import os
from os import sys, path
import unittest
from dronekit import LocationGlobalRelative
sys.path.append(os.path.realpath('..'))
import camera
class Attitude():
def __init__( self, yaw = 0.0 ):
self.yaw = yaw
# mock vehicle for tests
class Vehicle():
def __init__( self, yaw = 0.0, pitch = None ):
self.attitude = Attitude( yaw )
self.mount_status = [ pitch, math.degrees( yaw ), 0.0 ]
class TestCameraYaw(unittest.TestCase):
def testCameraGetYawZeroNoGimbal(self):
""" retrieve yaw 0 from camera class (no gimbal) """
v = Vehicle( yaw = 0.0 )
self.assertEqual( camera.getYaw(v), 0.0 )
def testCameraGetYaw90NoGimbal(self):
""" retrieve yaw 90 from camera class (no gimbal) """
v = Vehicle( yaw = math.radians(90.0) )
self.assertEqual( camera.getYaw(v), 90.0 )
def testCameraGetYawZeroWithGimbal(self):
""" retrieve yaw 0 from camera class (gimbal) """
v = Vehicle( yaw = 0.0, pitch = 35.2 )
v.attitude.yaw = math.radians(13.0)
#self.assertEqual( camera.getYaw(v), 0.0 )
# for now, we are using vehicle yaw in all cases
self.assertEqual( camera.getYaw(v), 13.0 )
def testCameraGetYaw90WithGimbal(self):
""" retrieve yaw 90 from camera class (gimbal) """
v = Vehicle( yaw = math.radians(90.0), pitch = 78.3 )
v.attitude.yaw = math.radians(13.0)
#self.assertEqual( camera.getYaw(v), 90.0 )
# for now, we are using vehicle yaw in all cases
self.assertEqual( camera.getYaw(v), 13.0 )
class TestCameraPitch(unittest.TestCase):
def testCameraGetPitchNoGimbal(self):
""" retrieve pitch from camera class (no gimbal) """
v = Vehicle()
self.assertEqual( camera.getPitch(v), 0.0 )
def testCameraGetPitch45(self):
""" retrieve 45 pitch from camera class """
v = Vehicle( pitch = 45.0 )
self.assertEqual( camera.getPitch(v), 45.0 )
def testCameraGetPitch70(self):
""" retrieve 70 pitch from camera class """
v = Vehicle( pitch = 70.0 )
self.assertEqual( camera.getPitch(v), 70.0 )

313
shotmanager/Test/TestCatmullRom.py Normal file
View File

@ -0,0 +1,313 @@
# TestCatmullRom.py
# shotmanager
#
# Unit tests for the CatmullRom class in catmullRom.py
#
# Created by Will Silva on 11/22/2015.
# Copyright (c) 2015 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import unittest
from catmullRom import CatmullRom
from vector3 import Vector3
class TestConstructor(unittest.TestCase):
def testMinNumofPoints2(self):
'''Tests that catmull raises an exception if less than one waypoint is passed to it'''
Pts = [Vector3(0.0, 0.0, 0.0)] * 3
self.assertRaises(ValueError, CatmullRom, Pts)
class TestCatmullRom(unittest.TestCase):
def setUp(self):
self.spline = CatmullRom(
[Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)])
def testUpdateSplineCoefficientsError(self):
'''Tests that an error is raised if we call updateSplineCoefficients with an out of range segment'''
self.assertRaises(ValueError, self.spline.updateSplineCoefficients, 3)
def testPosition(self):
'''For an evenly spaced straight-line spline, test that position is traversed'''
seg = 0
u = 0
q = self.spline.position(seg, u)
self.assertEqual(q, Vector3(1.0, 0.0, 0.0))
u = 0.5
q = self.spline.position(seg, u)
self.assertEqual(q, Vector3(1.5, 0.0, 0.0))
u = 1.0
q = self.spline.position(seg, u)
self.assertEqual(q, Vector3(2.0, 0.0, 0.0))
def testVelocity(self):
'''For an evenly spaced straight-line spline, test that velocity is constant'''
seg = 0
u = 0
dq = self.spline.velocity(seg, u)
self.assertEqual(dq, Vector3(1.0, 0.0, 0.0))
u = 0.5
dq = self.spline.velocity(seg, u)
self.assertEqual(dq, Vector3(1.0, 0.0, 0.0))
u = 1.0
dq = self.spline.velocity(seg, u)
self.assertEqual(dq, Vector3(1.0, 0.0, 0.0))
def testAcceleration(self):
'''For an evenly spaced straight-line spline, test that acceleration is zero'''
seg = 0
u = 0
ddq = self.spline.acceleration(seg, u)
self.assertEqual(ddq, Vector3(0.0, 0.0, 0.0))
u = 0.5
ddq = self.spline.acceleration(seg, u)
self.assertEqual(ddq, Vector3(0.0, 0.0, 0.0))
u = 1.0
ddq = self.spline.acceleration(seg, u)
self.assertEqual(ddq, Vector3(0.0, 0.0, 0.0))
def testCurvature(self):
'''For an evenly spaced straight-line spline, test that curvature is zero'''
seg = 0
u = 0
curv = self.spline.curvature(seg, u)
self.assertEqual(curv, 0.0)
u = 0.5
curv = self.spline.curvature(seg, u)
self.assertEqual(curv, 0.0)
u = 1.0
curv = self.spline.curvature(seg, u)
self.assertEqual(curv, 0.0)
class TestArcLength(unittest.TestCase):
def setUp(self):
self.spline = CatmullRom(
[Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)])
def testStandardInput(self):
'''Calculate arc length of the segment and verify that it's close to 1 (within 7 decimal places)'''
seg = 0
u1 = 0
u2 = 1
length = self.spline.arcLength(seg, u1, u2)
self.assertAlmostEqual(1, length, 7)
def testU2LessU1(self):
'''Tests case where u2 < u1'''
seg = 0
u2 = 0.5
u1 = 0.75
length = self.spline.arcLength(seg, u1, u2)
self.assertEqual(0, length)
def testU1OutOfBounds(self):
'''Test when u1 is not between 0-1'''
seg = 0
u1 = -0.2
u2 = 0.5
length = self.spline.arcLength(seg, u1, u2)
self.assertAlmostEqual(0.5, length)
def testU2OutOfBounds(self):
'''Test when u2 is not between 0-1'''
seg = 0
u1 = 0.5
u2 = 1.5
length = self.spline.arcLength(seg, u1, u2)
self.assertAlmostEqual(0.5, length)
def testU1andU2OutOfBounds(self):
'''Test when u1 and u2 are not between 0-1'''
seg = 0
u1 = 1.1
u2 = 1.2
length = self.spline.arcLength(seg, u1, u2)
self.assertAlmostEqual(0, length)
u1 = -0.7
u2 = -0.5
length = self.spline.arcLength(seg, u1, u2)
self.assertAlmostEqual(0, length)
def testAgainstBruteForceIntegration(self):
'''Make sure our numerical integrator is doing a decent job at estimating arc length'''
self.spline = CatmullRom(
[Vector3(0, 0, 0), Vector3(1, 5, 0), Vector3(2, -3, 0), Vector3(3, 0, 0)])
seg = 0
u1 = 0
u2 = 1
gaussResult = self.spline.arcLength(seg, u1, u2)
# Let's use Brute Force
n = 1000
dt = 1.0 / n
L = 0
t = 0
for i in range(0, n):
L += self.spline.velocity(seg, t).length() * dt
t += dt
# within a millimeter
self.assertAlmostEqual(gaussResult, L, 3)
class TestParameterByDistance(unittest.TestCase):
def setUp(self):
self.spline = CatmullRom(
[Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)])
def testStandardInput(self):
'''Calculate parameter u that is s meters ahead of u1 (within 7 decimal places'''
seg = 0
u1 = 0.25
s = 0.25 # meters
uLive = self.spline.findParameterByDistance(seg, u1, s)
# test with 300 newton iterations
uAccurate = self.spline.findParameterByDistance(seg, u1, s, 300)
self.assertAlmostEqual(uLive, uAccurate, 7)
def testSTooLong(self):
'''Test when s (in meters) extends beyond the segment'''
seg = 0
u1 = 0.5
s = 0.75
u = self.spline.findParameterByDistance(seg, u1, s)
self.assertEqual(1.0, u)
def testSNegative(self):
'''Test when s is negative (doesn't make sense for this algorithm)'''
seg = 0
u1 = 0.5
s = -0.75
u = self.spline.findParameterByDistance(seg, u1, s)
self.assertEqual(u1, u)
class TestArclengthToNonDimensional(unittest.TestCase):
def setUp(self):
# 1 meter long spline
self.spline = CatmullRom(
[Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)])
def testPTooBig(self):
'''Test when p is not between 0-1'''
p = 1.5 # should default to 1.0
dp = .1 # should be .1 m/s
seg, u, v = self.spline.arclengthToNonDimensional(p, dp)
self.assertEqual(seg, 0)
self.assertEqual(u, 1.0)
self.assertAlmostEqual(v, 0.1)
def testPTooSmall(self):
'''Test when p is not between 0-1'''
p = -1.5 # should default to 0
dp = .1 # should be .1 m/s
seg, u, v = self.spline.arclengthToNonDimensional(p, dp)
self.assertEqual(seg, 0)
self.assertEqual(u, 0)
self.assertAlmostEqual(v, 0.1)
def testConversionFromArclength(self):
'''Test conversion in an ideal 1 meter spline'''
p = 0.5 # should be halfway through spline distance (0.5 meters)
dp = .1 # should be .1 m/s
seg, u, v = self.spline.arclengthToNonDimensional(p, dp)
self.assertEqual(seg, 0)
self.assertEqual(u, 0.5)
self.assertAlmostEqual(v, 0.1)
def testConversionFromArclength2Seg(self):
'''Test conversion on a 2 segment spline'''
self.spline = CatmullRom([Vector3(0, 0, 0), Vector3(
1, 0, 0), Vector3(2, 0, 0), Vector3(4, 0, 0), Vector3(5, 0, 0)])
# x x-x--x x
p = 0.5 # should be halfway through spline distance (1.5 meters)
dp = .1
seg, u, v = self.spline.arclengthToNonDimensional(p, dp)
self.assertEqual(seg, 1)
self.assertAlmostEqual(u, 0.27272727)
self.assertAlmostEqual(v, 0.3)
class TestNonDimensionalToArclength(unittest.TestCase):
def setUp(self):
# 1 meter long spline
self.spline = CatmullRom(
[Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)])
def testSegTooBig(self):
'''Test when segment doesn't exist (too high)'''
seg = 1 # for a 4 point spline, we only have 1 segment with index 0 x x----x x
u = 0.5
v = 1
p, dp = self.spline.nonDimensionalToArclength(seg, u, v)
self.assertEqual(p, 0.5)
self.assertAlmostEqual(dp, 1.0)
def testSegTooSmall(self):
'''Test when segment doesn't exist (no negative segments allowed)'''
seg = - \
1 # for a 4 point spline, we only have 1 segment with index 0 x x----x x
u = 0.5
v = 1
p, dp = self.spline.nonDimensionalToArclength(seg, u, v)
self.assertEqual(p, 0.5)
self.assertAlmostEqual(dp, 1.0)
def testUTooBig(self):
'''Test when U is not between 0-1'''
seg = 0
u = 1.5
v = 1
p, dp = self.spline.nonDimensionalToArclength(seg, u, v)
self.assertEqual(p, 1.0)
self.assertAlmostEqual(dp, 1.0)
def testUTooSmall(self):
'''Test when U is not between 0-1'''
seg = 0
u = -1
v = 1
p, dp = self.spline.nonDimensionalToArclength(seg, u, v)
self.assertEqual(p, 0.0)
self.assertAlmostEqual(dp, 1.0)
def testConversionFromNonDimensional(self):
'''Test conversion on an ideal 1 meter spline'''
seg = 0
u = .75
v = 2
p, dp = self.spline.nonDimensionalToArclength(seg, u, v)
self.assertEqual(p, 0.75)
self.assertAlmostEqual(dp, 2.0)
def testConversionFromNonDimensional2Seg(self):
'''Test conversion on a 2 segment spline'''
self.spline = CatmullRom([Vector3(0, 0, 0), Vector3(
1, 0, 0), Vector3(2, 0, 0), Vector3(4, 0, 0), Vector3(5, 0, 0)])
# x x-x--x x
seg = 1
u = .5
v = 2
p, dp = self.spline.nonDimensionalToArclength(seg, u, v)
self.assertAlmostEqual(p, 0.66666666)
self.assertAlmostEqual(dp, 0.66666666)

330
shotmanager/Test/TestFlyController.py Normal file
View File

@ -0,0 +1,330 @@
# TestFlyController.py
# shotmanager
#
# Unit tests for the orbit controller.
#
# Created by Will Silva and Eric Liao on 1/19/2015.
# Modified by Nick Speal on 3/16/2016
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import flyController
from flyController import *
from dronekit import LocationGlobalRelative
import unittest
import mock
from mock import call
from mock import Mock
from mock import MagicMock
from mock import patch
class TestSetOptions(unittest.TestCase):
def setUp(self):
origin = LocationGlobalRelative(37.873168,-122.302062, 0) # lat,lon,alt
self.startxOffset = 20 # meters
self.startyOffset = 15 # meters
self.startzOffset = 10 # meters
self.startHeading = 0 # deg
self.startLocation = location_helpers.addVectorToLocation(origin,Vector3(self.startxOffset, self.startyOffset, self.startzOffset))
#Run the controller constructor
self.controller = flyController.FlyController(origin, self.startxOffset, self.startyOffset, self.startzOffset, self.startHeading)
def testSetBothOptions(self):
'''Tests setting both options'''
self.controller.setOptions(maxClimbRate = 3, maxAlt = 75)
self.assertEqual(self.controller.maxClimbRate, 3)
self.assertEqual(self.controller.maxAlt, 75)
def testSetOptionsNoAltLimit(self):
'''Tests setting both options, with disabled altitude limit'''
self.controller.setOptions(maxClimbRate = 3, maxAlt = None)
self.assertEqual(self.controller.maxClimbRate, 3)
self.assertEqual(self.controller.maxAlt, None)
class TestMove(unittest.TestCase):
def setUp(self):
origin = LocationGlobalRelative(37.873168,-122.302062, 0) # lat,lon,alt
self.startxOffset = 20 # meters
self.startyOffset = 15 # meters
self.startzOffset = 130 # meters
self.startHeading = 0 # deg
self.startLocation = location_helpers.addVectorToLocation(origin,Vector3(self.startxOffset, self.startyOffset, self.startzOffset))
#Run the controller constructor
self.controller = flyController.FlyController(origin, self.startxOffset, self.startyOffset, self.startzOffset, self.startHeading)
self.controller._approach = Mock(return_value = Vector3())
self.controller._strafe = Mock(return_value = Vector3())
self.controller._climb = Mock(return_value = Vector3())
#Set options
self.controller.maxClimbRate = 5 # m/s
self.controller.maxAlt = 100 #m
#Arbirary sticks
throttle = 0.3
roll = -0.4
pitch = 0.5
yaw = -0.6
raw_paddle = 0.7
self.channels = [throttle, roll, pitch, yaw, 0.0, 0.0, 0.0, raw_paddle]
self.controller.move(self.channels, newHeading = 0, newOrigin = LocationGlobalRelative(37.873168,-122.302062, 20))
def testCallApproach(self):
'''Tests that _approach() is called'''
self.controller._approach.assert_called_with(-0.5)
def testCallStrafe(self):
'''Tests that _strafe() is called'''
self.controller._strafe.assert_called_with(-0.4)
def testCallClimb(self):
'''Tests that _climb() is called'''
self.controller._climb.assert_called_with(0.7)
def testAltitudeLimit(self):
'''Test that maximum altitude is enforced when climbing'''
self.assertEqual(self.controller.offset.z, self.controller.maxAlt)
def testNeworigin(self):
'''Tests that a new origin is set when passed via optional argument'''
self.assertEqual(self.controller.origin.alt, 20)
@mock.patch('location_helpers.addVectorToLocation')
def testCallAddVectorToLocation(self, location_helpers_addVectorToLocation):
'''Tests that location_helpers.addVectorToLocation() is called'''
self.controller.move(self.channels)
location_helpers_addVectorToLocation.assert_called_with(mock.ANY, mock.ANY)
class TestApproach(unittest.TestCase):
def setUp(self):
origin = LocationGlobalRelative(37.873168,-122.302062, 0) # lat,lon,alt
self.startxOffset = 20 # meters
self.startyOffset = 15 # meters
self.startzOffset = 10 # meters
self.startHeading = 0 # deg
self.startLocation = location_helpers.addVectorToLocation(origin,Vector3(self.startxOffset, self.startyOffset, self.startzOffset))
#Run the controller constructor
self.controller = flyController.FlyController(origin, self.startxOffset, self.startyOffset, self.startzOffset, self.startHeading)
# set options
self.controller.setOptions(maxClimbRate=1.5 ,maxAlt=45)
def testPitchForwards(self):
'''Stick/pitch forwards to move forwards'''
approachVel = self.controller._approach(1.0)
expectedXVel = math.cos(self.controller.heading) * self.controller.approachSpeed
expectedYVel = math.sin(self.controller.heading) * self.controller.approachSpeed
self.assertEqual(self.controller.approachSpeed, ACCEL_PER_TICK)
self.assertEqual(approachVel, Vector3(expectedXVel, expectedYVel, 0))
def testPitchBackwards(self):
'''Stick/pitch backwards to move backwards'''
approachVel = self.controller._approach(-1.0)
expectedXVel = math.cos(self.controller.heading) * self.controller.approachSpeed
expectedYVel = math.sin(self.controller.heading) * self.controller.approachSpeed
self.assertEqual(self.controller.approachSpeed, -ACCEL_PER_TICK)
self.assertEqual(approachVel, Vector3(expectedXVel, expectedYVel, 0))
def testApproachSpeedDoNotExceed(self):
'''Test that the incremented speed doesn't exceed approachSpeed'''
self.controller.approachSpeed = 3.99
self.controller._approach(0.5) #desired approach speed is 4 but an increment will put it over 4
self.assertEqual(self.controller.approachSpeed,4) #make sure we're limited to 4
def testApproachSpeedDoNotFallBelow(self):
'''Test that the incremented speed doesn't fall below approachSpeed'''
self.controller.approachSpeed = -3.99
self.controller._approach(-0.5) #desired approach speed is 4 but an increment will put it under 4
self.assertEqual(self.controller.approachSpeed,-4) #make sure we're limited to 4
class TestStrafe(unittest.TestCase):
def setUp(self):
origin = LocationGlobalRelative(37.873168,-122.302062, 0) # lat,lon,alt
self.startxOffset = 20 # meters
self.startyOffset = 15 # meters
self.startzOffset = 10 # meters
self.startHeading = 0 # deg
self.startLocation = location_helpers.addVectorToLocation(origin,Vector3(self.startxOffset, self.startyOffset, self.startzOffset))
#Run the controller constructor
self.controller = flyController.FlyController(origin, self.startxOffset, self.startyOffset, self.startzOffset, self.startHeading)
# set options
self.controller.setOptions(maxClimbRate=1.5 ,maxAlt=45)
def testStrafeRight(self):
'''Stick/roll right to move right'''
strafeVel = self.controller._strafe(1.0)
expectedXVel = math.cos(self.controller.heading + math.pi/2.) * self.controller.strafeSpeed
expectedYVel = math.sin(self.controller.heading + math.pi/2.) * self.controller.strafeSpeed
self.assertEqual(self.controller.strafeSpeed, ACCEL_PER_TICK)
self.assertEqual(strafeVel, Vector3(expectedXVel, expectedYVel, 0))
def testStrafeLeft(self):
'''Stick/roll left to move left'''
strafeVel = self.controller._strafe(-1.0)
expectedXVel = math.cos(self.controller.heading + math.pi/2.) * self.controller.strafeSpeed
expectedYVel = math.sin(self.controller.heading + math.pi/2.) * self.controller.strafeSpeed
self.assertEqual(self.controller.strafeSpeed, -ACCEL_PER_TICK)
self.assertEqual(strafeVel, Vector3(expectedXVel, expectedYVel, 0))
def testApproachSpeedDoNotExceed(self):
'''Test that the incremented speed doesn't exceed approachSpeed'''
self.controller.strafeSpeed = 3.99
self.controller._strafe(0.5) #desired approach speed is 4 but an increment will put it over 4
self.assertEqual(self.controller.strafeSpeed,4) #make sure we're limited to 4
def testApproachSpeedDoNotFallBelow(self):
'''Test that the incremented speed doesn't fall below approachSpeed'''
self.controller.strafeSpeed = -3.99
self.controller._strafe(-0.5) #desired approach speed is 4 but an increment will put it under 4
self.assertEqual(self.controller.strafeSpeed,-4) #make sure we're limited to 4
class TestClimb(unittest.TestCase):
def setUp(self):
origin = LocationGlobalRelative(37.873168,-122.302062, 0) # lat,lon,alt
self.startxOffset = 20 # meters
self.startyOffset = 15 # meters
self.startzOffset = 10 # meters
self.startHeading = 0 # deg
self.startLocation = location_helpers.addVectorToLocation(origin,Vector3(self.startxOffset, self.startyOffset, self.startzOffset))
#Run the controller constructor
self.controller = flyController.FlyController(origin, self.startxOffset, self.startyOffset, self.startzOffset, self.startHeading)
# set options
self.controller.setOptions(maxClimbRate=1.5 ,maxAlt=45)
def testClimbUp(self):
'''Test climbing up with stick up'''
climbVel = self.controller._climb(1.0)
self.assertEqual(climbVel, Vector3(0,0,1.0*self.controller.maxClimbRate))
def testClimbDown(self):
'''Test descending with stick down'''
climbVel = self.controller._climb(-1.0)
self.assertEqual(climbVel, Vector3(0,0,-1.0*self.controller.maxClimbRate))
class TestMaxApproachSpeed(unittest.TestCase):
def setUp(self):
origin = LocationGlobalRelative(37.873168,-122.302062, 0) # lat,lon,alt
self.startxOffset = 20 # meters
self.startyOffset = 15 # meters
self.startzOffset = 10 # meters
self.startHeading = 0 # deg
self.startLocation = location_helpers.addVectorToLocation(origin,Vector3(self.startxOffset, self.startyOffset, self.startzOffset))
#Run the controller constructor
self.controller = flyController.FlyController(origin, self.startxOffset, self.startyOffset, self.startzOffset, self.startHeading)
# set options
self.controller.setOptions(maxClimbRate=1.5 ,maxAlt=45)
def testZeroStrafeSpeed(self):
'''Test that approachSpeed gets all of MAX_SPEED if strafeSpeed is zero'''
strafeSpeed = 0.0
maxSpeed = self.controller._maxApproachSpeed(strafeSpeed)
self.assertEqual(maxSpeed, MAX_SPEED)
def testHalfStrafeSpeed(self):
'''Test that approachSpeed is equal to strafeSpeed if strafeSpeed is sqrt(MAX_SPEED*4)'''
strafeSpeed = math.sqrt(MAX_SPEED*4)
maxSpeed = self.controller._maxApproachSpeed(strafeSpeed)
self.assertAlmostEqual(maxSpeed, strafeSpeed)
def testAllStrafeSpeed(self):
'''Test that approachSpeed is zero if strafeSpeed is MAX_SPEED'''
strafeSpeed = MAX_SPEED
maxSpeed = self.controller._maxApproachSpeed(strafeSpeed)
self.assertEqual(maxSpeed, 0.0)
class TestMaxStrafeSpeed(unittest.TestCase):
def setUp(self):
origin = LocationGlobalRelative(37.873168,-122.302062, 0) # lat,lon,alt
self.startxOffset = 20 # meters
self.startyOffset = 15 # meters
self.startzOffset = 10 # meters
self.startHeading = 0 # deg
self.startLocation = location_helpers.addVectorToLocation(origin,Vector3(self.startxOffset, self.startyOffset, self.startzOffset))
#Run the controller constructor
self.controller = flyController.FlyController(origin, self.startxOffset, self.startyOffset, self.startzOffset, self.startHeading)
# set options
self.controller.setOptions(maxClimbRate=1.5 ,maxAlt=45)
def testZeroApproachSpeed(self):
'''Test that strafeSpeed gets all of MAX_SPEED if approachSpeed is zero'''
approachSpeed = 0.0
maxSpeed = self.controller._maxStrafeSpeed(approachSpeed)
self.assertEqual(maxSpeed, MAX_SPEED)
def testHalfApproachSpeed(self):
'''Test that strafeSpeed is equal to approachSpeed if approachSpeed is sqrt(MAX_SPEED*4)'''
approachSpeed = math.sqrt(MAX_SPEED*4)
maxSpeed = self.controller._maxStrafeSpeed(approachSpeed)
self.assertAlmostEqual(maxSpeed, approachSpeed)
def testAllApproachSpeed(self):
'''Test that strafeSpeed is zero if approachSpeed is MAX_SPEED'''
approachSpeed = MAX_SPEED
maxSpeed = self.controller._maxStrafeSpeed(approachSpeed)
self.assertEqual(maxSpeed, 0.0)

872
shotmanager/Test/TestFollow.py Normal file
View File

@ -0,0 +1,872 @@
# TestFollow.py
# shotmanager
#
# Unit tests for the follow smart shot.
#
# Created by Will Silva and Eric Liao on 1/19/2015.
# Mostly overhauled by Nick Speal on 3/16/2016
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import follow
from follow import *
import shotManager
from shotManager import ShotManager
import unittest
import mock
from mock import call
from mock import Mock
from mock import MagicMock
from mock import patch
class TestFollowShotInit(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
vehicle.attitude.yaw = math.radians(30)
vehicle.mount_status[0] = -45
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
shotmgr.getParam.return_value = 0 # so mock doesn't do lazy binds
#Run the shot constructor
self.shot = follow.FollowShot(vehicle, shotmgr)
def testInit(self):
'''Test that the shot initialized properly'''
# vehicle object should be created (not None)
assert self.shot.vehicle is not None
# shotManager object should be created (not None)
assert self.shot.shotmgr is not None
# pathHandler should be None
assert self.shot.pathHandler is not None
# pathHandler should be None
assert self.shot.pathController is None
# filtered roi should be None
self.assertEqual(self.shot.filteredROI, None)
self.assertEqual(self.shot.rawROI, None)
# rawROI queue should be created
assert self.shot.rawROIQueue is not None
# filteredROI queue should be created
assert self.shot.filteredROIQueue is not None
# roiVelocity should be initialized to None
self.assertEqual(self.shot.roiVelocity, None)
# init shot state
self.assertEqual(self.shot.followState, 0)
self.assertEqual(self.shot.followPreference, 0)
# initialize curTranslateVel to an empty Vector3
self.assertEqual(self.shot.translateVel,Vector3())
# shotmgr.getParam should be called thrice
# once for maxClimbRate and once for maxAlt and once for FENCE_ENABLE
calls = [call("PILOT_VELZ_MAX", DEFAULT_PILOT_VELZ_MAX_VALUE), call("FENCE_ALT_MAX", DEFAULT_FENCE_ALT_MAX), call("FENCE_ENABLE", DEFAULT_FENCE_ENABLE)]
self.shot.shotmgr.getParam.assert_has_calls(calls)
# initialize shot in altitudeOffset
self.assertEqual(self.shot.followControllerAltOffset, 0)
# initialize shot with ROIAltitudeOffset 0
self.assertEqual(self.shot.ROIAltitudeOffset, 0)
# enter Guided
self.assertEqual(self.shot.vehicle.mode.name, "GUIDED")
class TestHandleRCs(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
shotmgr.getParam.return_value = 0 # so mock doesn't do lazy binds
#Run the shot constructor
self.shot = follow.FollowShot(vehicle, shotmgr)
# Mock the functions
self.shot.checkSocket = Mock()
self.shot.filterROI = Mock()
# Mock Attributes
self.shot.rawROI = LocationGlobalRelative(37.873168,-122.302062, 0)
self.shot.filteredROI = LocationGlobalRelative(37.873168,-122.302062, 0)
self.shot.roiVelocity = Vector3(0,0,0)
self.shot.camYaw = 0.0
# Mock the pointing functions
self.shot.handleFreeLookPointing = Mock()
self.shot.handleFollowPointing = Mock()
self.shot.handleLookAtMePointing = Mock()
#Mock the pathHandler object
self.shot.pathHandler = mock.create_autospec(pathHandler.PathHandler)
self.shot.pathHandler.cruiseSpeed = 0
#Mock the pathController object
self.shot.pathController = mock.create_autospec(OrbitController)
self.shot.pathController.move.return_value = (LocationGlobalRelative(37.873168,-122.302062, 0),Vector3(1,1,1))
#Neutral sticks
throttle = 0.0
roll = 0.0
pitch = 0.0
yaw = 0.0
self.channels = [throttle, roll, pitch, yaw, 0.0, 0.0, 0.0, 0.0]
def testCallcheckSocket(self):
'''Test that we call checkSocket()'''
self.shot.handleRCs(self.channels)
self.shot.checkSocket.assert_called_with()
def testNoRawROI(self):
'''If raw ROI is not set then do NOT continue'''
self.shot.rawROI = None
self.shot.handleRCs(self.channels)
assert not self.shot.filterROI.called
def testCallFilterROI(self):
'''Test that we call filteRROI()'''
self.shot.handleRCs(self.channels)
self.shot.filterROI.assert_called_with()
def testFollowWaitReturns(self):
'''If followState == FOLLOW_WAIT, do NOT continue'''
self.shot.followState = FOLLOW_WAIT
self.shot.handleRCs(self.channels)
# assert not self.shot.vehicle.send_mavlink.called # TODO - CAN"T FIGURE OUT WHY THIS IS FAILING
def testLookAtMePathControllerCalled(self):
'''Test that correct path controller gets called'''
self.shot.followState = FOLLOW_LOOKAT
self.shot.pathController = mock.create_autospec(FlyController)
self.shot.pathController.move.return_value = (LocationGlobalRelative(37.873168,-122.302062, 0),Vector3(1,1,1))
self.shot.handleRCs(self.channels)
self.shot.pathController.move.assert_called_with(self.channels)
def testFreeLookPathControllerCalled(self):
'''Test that correct path controller gets called'''
self.shot.followState = FOLLOW_FREELOOK
self.shot.pathController = mock.create_autospec(FlyController)
self.shot.pathController.move.return_value = (LocationGlobalRelative(37.873168,-122.302062, 0),Vector3(1,1,1))
self.shot.camPitch = 45.0
self.shot.handleRCs(self.channels)
self.shot.pathController.move.assert_called_with(self.channels, newHeading = self.shot.camYaw, newOrigin = self.shot.filteredROI, roiVel=self.shot.roiVelocity)
def testOrbitPathControllerCalled(self):
'''Test that correct path controller gets called'''
self.shot.followState = FOLLOW_ORBIT
# pathController created in setup
self.shot.pathHandler.cruiseSpeed = 0.0
self.shot.handleRCs(self.channels)
self.shot.pathController.move.assert_called_with(self.channels, cruiseSpeed = self.shot.pathHandler.cruiseSpeed, newroi = self.shot.filteredROI, roiVel=self.shot.roiVelocity)
def testLeashPathControllerCalled(self):
'''Test that correct path controller gets called'''
self.shot.followState = FOLLOW_LEASH
self.shot.pathController = mock.create_autospec(LeashController)
self.shot.pathController.move.return_value = (LocationGlobalRelative(37.873168,-122.302062, 0),Vector3(1,1,1))
self.shot.handleRCs(self.channels)
self.shot.pathController.move.assert_called_with(self.channels, newroi = self.shot.filteredROI, roiVel=self.shot.roiVelocity)
def testSettingAltitudeLimit(self):
self.shot.maxAlt = 100.0
self.shot.handleRCs(self.channels)
def testFreelookPointing(self):
self.followState = FOLLOW_FREELOOK
self.shot.manualPitch = Mock()
self.shot.manualYaw = Mock()
self.shot.handleFreeLookPointing = Mock()
self.shot.handleRCs(self.channels)
# self.shot.manualPitch.assert_called_with(self.channels) # TODO - CAN"T FIGURE OUT WHY THIS IS FAILING
# self.shot.manualYaw.assert_called_with(self.channels) # TODO - CAN"T FIGURE OUT WHY THIS IS FAILING
# self.shot.handleFreeLookPointing.assert_called_with(mock.ANY) # TODO - CAN"T FIGURE OUT WHY THIS IS FAILING
def testNonFreelookPointing(self):
# may need mock
self.followState = FOLLOW_ORBIT
self.updateaROIAltOffset = Mock()
self.shot.handleRCs(self.channels)
#self.updateaROIAltOffset.assert_called_with(self.channels[RAW_PADDLE]) # TODO - CAN"T FIGURE OUT WHY THIS IS FAILING
def testMavlinkMessageDidSend(self):
self.shot.handleRCs(self.channels)
#Test that the expected message got encoded:
expectedControlMask = 0b0000110111000000 # pos-vel mask
# TODO - CAN"T FIGURE OUT WHY THIS IS FAILING:
# self.shot.vehicle.message_factory.set_position_target_global_int_encode.assert_called_with(
# 0, # time_boot_ms (not used)
# 0, 1, # target system, target component
# mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame
# expectedControlMask, # type_mask
# int(37.873168*1E7), int(-122.302062*1E7), 0, # x, y, z positions (ignored)
# 1, 1, 1, # x, y, z velocity in m/s
# 0, 0, 0, # x, y, z acceleration (not used)
# 0, 0) # yaw, yaw_rate (not used)
self.shot.vehicle.send_mavlink.assert_called_with(mock.ANY)
class TestUpdateROIAltOffset(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = follow.FollowShot(vehicle, shotmgr)
self.shot.pathHandler = mock.create_autospec(pathHandler.PathHandler)
self.shot.pathHandler.cruiseSpeed = 0
#Mock the pathController object
self.shot.followState = FOLLOW_ORBIT
self.shot.pathController = mock.create_autospec(OrbitController)
self.shot.ROIAltitudeOffset = 0.0
self.shot.ROIAltitudeOffset = 0.0
def testWithinDeadZone(self):
self.shot.updateROIAltOffset(0.01)
self.assertEqual(self.shot.ROIAltitudeOffset, 0.0)
def testNotWithinDeadZone(self):
self.shot.updateROIAltOffset(0.9)
self.assertNotEqual(self.shot.ROIAltitudeOffset, 0.0)
class TestHandleButton(unittest.TestCase):
'''TODO Nick - after UI shakes out'''
pass
class TestSetButtonMappings(unittest.TestCase):
'''TODO Nick - after UI shakes out'''
pass
class TestHandleOptions(unittest.TestCase):
'''Handles follow options from app. TODO Nick - after UI shakes out'''
pass
class TestInitState(unittest.TestCase):
ARBITRARY_HEADING = 34.8
DISTANCE = 97.5
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = follow.FollowShot(vehicle, shotmgr)
#Mock Methods
self.shot.updateMountStatus = Mock()
self.shot.initLookAtMeController = Mock()
self.shot.initOrbitController = Mock()
self.shot.initLeashController = Mock()
self.shot.initFreeLookController = Mock()
self.shot.updateMountStatus = Mock()
self.shot.updateAppOptions = Mock()
self.shot.setButtonMappings = Mock()
#Setup Attributes
self.shot.followState = FOLLOW_WAIT
self.shot.rawROI = location_helpers.newLocationFromAzimuthAndDistance(self.shot.vehicle.location.global_relative_frame, self.ARBITRARY_HEADING, self.DISTANCE)
def testNoROINoInit(self):
self.shot.rawROI = None
self.shot.initState(FOLLOW_ORBIT)
assert not self.shot.updateMountStatus.called
def testLookatControllerDidInit(self):
self.shot.initState(FOLLOW_LOOKAT)
assert self.shot.initLookAtMeController.called
def testOrbitControllerDidInit(self):
self.shot.initState(FOLLOW_ORBIT)
assert self.shot.initOrbitController.called
def testLeashControllerDidInit(self):
self.shot.initState(FOLLOW_LEASH)
assert self.shot.initLeashController.called
def testFreeLookControllerDidInit(self):
self.shot.initState(FOLLOW_FREELOOK)
assert self.shot.initFreeLookController.called
def testUpdateFunctionsCalled(self):
self.shot.initState(FOLLOW_ORBIT)
assert self.shot.updateMountStatus.called
assert self.shot.updateAppOptions.called
assert self.shot.setButtonMappings.called
class TestUpdateAppOptions(unittest.TestCase):
'''TODO Nick - after UI shakes out'''
pass
class TestSetupSocket(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = follow.FollowShot(vehicle, shotmgr)
@mock.patch('socket.socket')
def testSocketCreation(self, socket_socket):
'''Test that the socket is created'''
self.shot.setupSocket()
socket_socket.assert_called_with(socket.AF_INET, socket.SOCK_DGRAM)
@mock.patch('socket.socket', return_value = Mock())
def testSocketOptions(self, socket_socket):
'''Test that socket options are set'''
self.shot.setupSocket()
self.shot.socket.setsockopt.assert_called_with(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
@mock.patch('socket.socket', return_value = Mock())
def testSocketBlocking(self, socket_socket):
'''Test that socket blocking is set to 0'''
self.shot.setupSocket()
self.shot.socket.setblocking.assert_called_with(0)
@mock.patch('socket.socket', return_value = Mock())
def testSocketBind(self, socket_socket):
'''Test that socket tries to bind'''
self.shot.setupSocket()
self.shot.socket.bind.assert_called_with(("",FOLLOW_PORT))
@mock.patch('socket.socket', return_value = Mock())
def testSocketTimeout(self, socket_socket):
'''Test that socket sets a timeout'''
self.shot.setupSocket()
self.shot.socket.settimeout.assert_called_with(SOCKET_TIMEOUT)
class TestCheckSocket(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = follow.FollowShot(vehicle, shotmgr)
sampleData = struct.pack('<IIddf', app_packet.SOLO_MESSAGE_LOCATION, 20, 37.873168,-122.302062, 0)
self.shot.socket.recvfrom = Mock(side_effect = [(sampleData,'127.0.0.1'), socket.timeout])
def testRecvFrom(self):
'''Test recvFrom functionality'''
self.shot.checkSocket()
self.shot.socket.recvfrom.assert_called_with(28)
@mock.patch('monotonic.monotonic', return_value = 333)
def testWhenROIisNone(self, monotonic_monotonic):
'''Test first data unpack'''
self.shot.rawROI = None
self.shot.checkSocket()
self.assertEqual(self.shot.roiDeltaTime, None)
self.assertEqual(self.shot.previousROItime, 333)
self.assertEqual(self.shot.rawROI.lat, 37.873168)
self.assertEqual(self.shot.rawROI.lon, -122.302062)
self.assertEqual(self.shot.rawROI.alt, 0)
@mock.patch('monotonic.monotonic', return_value = 333)
def testNewData(self, monotonic_monotonic):
'''Test subsequent data unpack'''
self.shot.previousROItime = 222
self.shot.rawROI = LocationGlobalRelative(37.873168,-122.302062, 0)
self.shot.checkSocket()
self.assertEqual(self.shot.roiDeltaTime, 111)
self.assertEqual(self.shot.previousROItime, 333)
self.assertEqual(self.shot.rawROI.lat, 37.873168)
self.assertEqual(self.shot.rawROI.lon, -122.302062)
self.assertEqual(self.shot.rawROI.alt, 0)
class TestFilterROI(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
shotmgr.buttonManager = Mock()
#Run the shot constructor
self.shot = follow.FollowShot(vehicle, shotmgr)
# rawROI
self.ROI = LocationGlobalRelative(37.873168,-122.302062, 0) #sample ROI, used throughout
self.shot.rawROI = self.ROI
# mock methods
self.shot.initState = Mock()
location_helpers.calcYawPitchFromLocations = Mock()
location_helpers.calcYawPitchFromLocations.return_value = (0.0,0.0)
#roiDeltaTime
self.shot.roiDeltaTime = 0.04 # 25Hz (guess)
#init vars
self.shot.roiVelocity = Vector3()
def testFilteredROIQueueGreaterThanZero(self):
'''Test that beginFollow is called if roiVelocity is None and the filteredROIQueue is long enough'''
self.shot.filterROI()
self.assertEqual(self.shot.filteredROI, self.shot.rawROI)
self.assertEqual(self.shot.roiVelocity, Vector3(0,0,0))
assert self.shot.initState.called
def testNoQueue(self):
'''run through the method with empty queues (First item will be added in this queue)'''
# Filter gets skipped b/c queues are empty
# initialization block executes because queue now has 1 item
# end of method executes
self.shot.roiDeltaTime = None
self.shot.rawROIQueue = collections.deque(maxlen=MIN_RAW_ROI_QUEUE_LENGTH)
self.shot.filteredROIQueue = collections.deque(maxlen=MIN_FILT_ROI_QUEUE_LENGTH)
self.shot.filterROI()
self.assertEqual(self.shot.filteredROI, self.shot.rawROI)
assert not location_helpers.calcYawPitchFromLocations.called
def testFilterFullQueue(self):
'''run through the method with a pre-filled queue'''
# filter executes because queues are full
# init is skipped. len(queue) > 1
# else block executes. len(queue) > 1 AND roiDeltaTime exists.
for i in range(5):
self.shot.rawROIQueue.append(self.ROI)
self.shot.filteredROIQueue.append(self.ROI)
self.shot.filterROI()
assert location_helpers.calcYawPitchFromLocations.called
def testAccelerationLimitVariations_x(self):
'''For different combinations of roiVeloctiy, translateVel, and components X,Y,Z: Verify that the code is executed and that the values are as expected.'''
self.shot.roiVelocity.x = 0.0 # gets overwritten in filterROI() anyway
# (much) Greater than case
self.shot.translateVel.x = -1.0
self.shot.filterROI()
assert self.shot.translateVel.x == -1.0 + ACCEL_PER_TICK
# equal case
self.shot.translateVel.x = 0.0
self.shot.filterROI()
assert self.shot.translateVel.x == self.shot.roiVelocity.x
# (much) Less than case
self.shot.translateVel.x = 1.0
self.shot.filterROI()
assert self.shot.translateVel.x == 1.0 - ACCEL_PER_TICK
def testAccelerationLimitVariations_y(self):
'''For different combinations of roiVeloctiy, translateVel, and components X,Y,Z: Verify that the code is executed and that the values are as expected.'''
self.shot.roiVelocity.y = 0.0 # gets overwritten in filterROI() anyway
# (much) Greater than case
self.shot.translateVel.y = -1.0
self.shot.filterROI()
assert self.shot.translateVel.y == -1.0 + ACCEL_PER_TICK
# equal case
self.shot.translateVel.y = 0.0
self.shot.filterROI()
assert self.shot.translateVel.x == self.shot.roiVelocity.y
# (much) Less than case
self.shot.translateVel.y = 1.0
self.shot.filterROI()
assert self.shot.translateVel.y == 1.0 - ACCEL_PER_TICK
def testAccelerationLimitVariations_z(self):
'''For different combinations of roiVeloctiy, translateVel, and components X,Y,Z: Verify that the code is executed and that the values are as expected.'''
self.shot.roiVelocity.z = 0.0 # gets overwritten in filterROI() anyway
# (much) Greater than case
self.shot.translateVel.z = -1.0
self.shot.filterROI()
assert self.shot.translateVel.z == -1.0 + ACCEL_PER_TICK
# equal case
self.shot.translateVel.z = 0.0
self.shot.filterROI()
assert self.shot.translateVel.z == self.shot.roiVelocity.z
# (much) Less than case
self.shot.translateVel.z = 1.0
self.shot.filterROI()
assert self.shot.translateVel.z == 1.0 - ACCEL_PER_TICK
def testQueueLengthsAreNotTooLong(self):
'''make sure (at steady state) the length of the queue is correct.'''
# should be defined as
# MIN_RAW_ROI_QUEUE_LENGTH = 5, MIN_FILT_ROI_QUEUE_LENGTH = 4
for i in range(10):
self.shot.rawROIQueue.append(self.ROI)
self.shot.filteredROIQueue.append(self.ROI)
self.shot.filterROI()
self.assertEqual(len(self.shot.rawROIQueue),5)
self.assertEqual(len(self.shot.filteredROIQueue),4)
class TestInitControllersParent(unittest.TestCase):
'''
Parent class to enable testing of multiple methods
'''
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = follow.FollowShot(vehicle, shotmgr)
# mock methods
location_helpers.getDistanceFromPoints = Mock()
location_helpers.calcAzimuthFromPoints = Mock()
location_helpers.getDistanceFromPoints.return_value = 10.0
location_helpers.calcAzimuthFromPoints.return_value = 0.0
class TestInitOrbitController(TestInitControllersParent):
''' ORBIT '''
def setup(self):
super(TestInitOrbitController,self).setup()
controller = mock.create_autospec(OrbitController)
def didExecute_initOrbitController(self):
self.shot.initOrbitController()
assert location_helpers.getDistanceFromPoints.called
assert location_helpers.calcAzimuthFromPoints.called
assert controller.setOptions.called
class TestInitLeashController(TestInitControllersParent):
''' LEASH '''
def setup(self):
super(TestInitLeashController,self).setup()
controller = mock.create_autospec(LeashController)
def didExecute_initLeashController(self):
self.shot.initLeashController()
assert location_helpers.getDistanceFromPoints.called
assert location_helpers.calcAzimuthFromPoints.called
assert controller.setOptions.called
class TestInitFreeLookController(TestInitControllersParent):
''' FREE LOOK '''
def setup(self):
super(TestInitFreeLookController,self).setup()
location_helpers.getVectorFromPoints = Mock()
location_helpers.getVectorFromPoints.return_value = Vector3(5,5,5)
controller = mock.create_autospec(FlyController)
def didExecute_initFreeLookController(self):
self.shot.initFreeLookController()
assert location_helpers.getDistanceFromPoints.called
assert location_helpers.calcAzimuthFromPoints.called
assert controller.setOptions.called
# Didn't check if getYaw, getPitch were called. It's probably fine
class TestInitLookAtMeController(TestInitControllersParent):
''' LOOK AT ME '''
def setup(self):
super(TestInitLookAtMeController,self).setup()
controller = mock.create_autospec(LookAtController)
self.shot.pathHandler.pause = Mock()
def didExecute_initLookAtMeController(self):
self.shot.initLookAtMeController()
assert self.shot.pathHandler.pause.called
assert controller.setOptions.called
class TestUpdateMountStatus(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = follow.FollowShot(vehicle, shotmgr)
def testFreeLookMessage(self):
self.shot.followState = FOLLOW_FREELOOK
self.shot.updateMountStatus()
self.shot.vehicle.message_factory.mount_configure_encode.assert_called_with(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, #mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
assert self.shot.vehicle.send_mavlink.called
def testNonFreeLookMessage(self):
self.shot.followState = FOLLOW_ORBIT
self.shot.updateMountStatus()
self.shot.vehicle.message_factory.mount_configure_encode.assert_called_with(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, #mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
assert self.shot.vehicle.send_mavlink.called
class TestManualPitch(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = follow.FollowShot(vehicle, shotmgr)
#Neutral sticks
throttle = 0.0
roll = 0.0
pitch = 0.0
yaw = 0.0
self.channels = [throttle, roll, pitch, yaw, 0.0, 0.0, 0.0, 0.0]
def testPositiveValue(self):
self.shot.camPitch = 1.0
self.shot.manualPitch(self.channels[THROTTLE])
assert self.shot.camPitch == 0.0
def testZeroValue(self):
self.shot.camPitch = 0.0
self.shot.manualPitch(self.channels[THROTTLE])
assert self.shot.camPitch == 0.0
def testSlightlyNegativeValue(self):
self.shot.camPitch = -1.0
self.shot.manualPitch(self.channels[THROTTLE])
assert self.shot.camPitch == -1.0
def testNegative90Value(self):
self.shot.camPitch = -90.0
self.shot.manualPitch(self.channels[THROTTLE])
assert self.shot.camPitch == -90.0
def testHugeNegativeValue(self):
self.shot.camPitch = -100.0
self.shot.manualPitch(self.channels[THROTTLE])
assert self.shot.camPitch == -90.0
class TestManualYaw(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = follow.FollowShot(vehicle, shotmgr)
self.shot.camYaw = 0 #init to zero. Can be permuted below.
#Neutral sticks, unless permuted in the methods below
throttle = 0.0
roll = 0.0
pitch = 0.0
yaw = 0.0
self.channels = [throttle, roll, pitch, yaw, 0.0, 0.0, 0.0, 0.0]
def testZeroValue(self):
''' Make sure Function returns if no yaw command'''
self.channels[YAW] = 0.0
self.shot.camYaw = -999 #Dummy value to make sure it is unchanged in the function
self.shot.manualYaw(self.channels)
assert self.shot.camYaw == -999
def testPositiveYawPositiveDirectionOfSpin(self):
self.channels[YAW] = 0.5
self.shot.manualYaw(self.channels)
assert self.shot.camDir == 1
def testNegativeYawNegativeDirectionOfSpin(self):
self.channels[YAW] = -0.5
self.shot.manualYaw(self.channels)
assert self.shot.camDir == -1
def testLargeValue(self):
self.channels[YAW] = -0.1
self.shot.camYaw = 380
self.shot.manualYaw(self.channels)
assert self.shot.camYaw <= 20
def testNegativeValue(self):
self.channels[YAW] = 0.1
self.shot.camYaw = -50
self.shot.manualYaw(self.channels)
assert self.shot.camYaw >= 310
# TODO - Needed to test edge cases of 0 and 360? Can camYaw be both 0 and 360 on the dot? Hard to test.
class TestHandleFreeLookPointing(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = follow.FollowShot(vehicle, shotmgr)
self.shot.camYaw = float( 1.111333 )
self.shot.camPitch = float (2.55556666 )
self.shot.camDir = 1
def testWithGimbal(self):
''' With a gimbal, use mount_control to control pitch/yaw '''
self.shot.vehicle.mount_status = [20.0, 0.0, 1.3]
self.shot.handleFreeLookPointing()
self.shot.vehicle.message_factory.mount_control_encode.assert_called_with(
0, 1, # target system, target component
self.shot.camPitch * 100, # pitch
0.0, # roll
self.shot.camYaw * 100, # yaw
0 # save position
)
def testNoGimbal(self):
''' Without a gimbal, we only use condition_yaw to control '''
# no gimbal
self.shot.vehicle.mount_status = [None, None, None]
yawDir = 1
self.shot.handleFreeLookPointing()
self.shot.vehicle.message_factory.command_long_encode.assert_called_with(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
0, #confirmation
self.shot.camYaw, # param 1 - target angle
follow.YAW_SPEED, # param 2 - yaw speed
yawDir, # param 3 - direction
0.0, # relative offset
0, 0, 0 # params 5-7 (unused)
)
class testHandleLookAtMePointing(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = follow.FollowShot(vehicle, shotmgr)
#roi
self.tempROI = LocationGlobalRelative(37.873168,-122.302062, 0)
def didExecute_handleLookAtPointing(self):
self.vehicle.message_factory.command_int_encode.assert_called_with(
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.tempROI.lat*1.E7,
self.tempROI.lon*1.E7,
self.tempROI.alt + self.shot.ROIAltitudeOffset #offset for ROI
)
assert self.shot.vehicle.send_mavlink.called

632
shotmanager/Test/TestGoProManager.py Normal file
View File

@ -0,0 +1,632 @@
# Unit tests for GoProManager
import mock
from mock import call
from mock import Mock
from mock import patch
import os
from os import sys, path
import struct
import monotonic
import unittest
from pymavlink import mavutil
sys.path.append(os.path.realpath('..'))
import app_packet
import GoProManager
from GoProConstants import *
class TestStatusCallback(unittest.TestCase):
def setUp(self):
shotmgr = Mock()
self.v = Mock()
self.v.message_factory = Mock()
shotmgr.vehicle = self.v
self.mgr = GoProManager.GoProManager(shotmgr)
self.mgr.sendGoProRequest = Mock()
self.mgr.sendState = Mock()
def testStartup(self):
""" GoPro status on startup should be GOPRO_HEARTBEAT_STATUS_DISCONNECTED """
self.assertEqual( self.mgr.status, mavutil.mavlink.GOPRO_HEARTBEAT_STATUS_DISCONNECTED)
self.assertFalse( self.mgr.sendState.called )
def testConnected(self):
""" Test connected status """
message = Mock(spec=["status", "capture_mode", "flags"])
message.status = mavutil.mavlink.GOPRO_HEARTBEAT_STATUS_CONNECTED
message.capture_mode = mavutil.mavlink.GOPRO_CAPTURE_MODE_VIDEO
message.flags = 0
#message = (mavutil.mavlink.GOPRO_HEARTBEAT_STATUS_CONNECTED, mavutil.mavlink.GOPRO_CAPTURE_MODE_VIDEO, 0)
self.mgr.state_callback('vehicle','name', message)
self.assertEqual( self.mgr.status, mavutil.mavlink.GOPRO_HEARTBEAT_STATUS_CONNECTED)
self.mgr.sendState.assert_called_with()
# when we connect, we should fetch status info (check last request called)
self.mgr.sendGoProRequest.assert_called_with(mavutil.mavlink.GOPRO_COMMAND_PROTUNE_EXPOSURE)
class TestGetResponseCallback(unittest.TestCase):
def setUp(self):
shotmgr = Mock()
self.v = Mock()
shotmgr.vehicle = self.v
self.mgr = GoProManager.GoProManager(shotmgr)
self.mgr.processMsgQueue = Mock()
self.mgr.sendState = Mock()
def testCaptureResponse(self):
""" Test that capture response works """
message = (mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, mavutil.mavlink.GOPRO_REQUEST_SUCCESS, (CAPTURE_MODE_BURST, 0, 0, 0))
self.mgr.get_response_callback('vehicle','name', message)
self.assertEqual( self.mgr.captureMode, CAPTURE_MODE_BURST)
self.mgr.processMsgQueue.assert_called_with()
self.mgr.sendState.assert_called_with()
def testBatteryResponse(self):
""" Test that battery response works """
message = (mavutil.mavlink.GOPRO_COMMAND_BATTERY, mavutil.mavlink.GOPRO_REQUEST_SUCCESS, (72, 0, 0, 0))
self.mgr.get_response_callback('vehicle','name', message)
self.assertEqual( self.mgr.battery, 72)
self.mgr.processMsgQueue.assert_called_with()
def testModelResponse(self):
""" Test that model response works """
message = (mavutil.mavlink.GOPRO_COMMAND_MODEL, mavutil.mavlink.GOPRO_REQUEST_SUCCESS, (MODEL_HERO3PLUS_BLACK, 0, 0, 0))
self.mgr.get_response_callback('vehicle','name', message)
self.assertEqual( self.mgr.model, MODEL_HERO3PLUS_BLACK)
self.mgr.processMsgQueue.assert_called_with()
self.mgr.sendState.assert_called_with()
def testVideoSettingsResponse(self):
""" Test that the video settings response works """
message = (mavutil.mavlink.GOPRO_COMMAND_VIDEO_SETTINGS, mavutil.mavlink.GOPRO_REQUEST_SUCCESS, (mavutil.mavlink.GOPRO_RESOLUTION_1080p, mavutil.mavlink.GOPRO_FRAME_RATE_60, mavutil.mavlink.GOPRO_FIELD_OF_VIEW_WIDE, VIDEO_FORMAT_NTSC))
self.mgr.get_response_callback('vehicle','name', message)
self.assertEqual(self.mgr.videoResolution, mavutil.mavlink.GOPRO_RESOLUTION_1080p)
self.assertEqual(self.mgr.videoFrameRate, mavutil.mavlink.GOPRO_FRAME_RATE_60)
self.assertEqual(self.mgr.videoFieldOfView, mavutil.mavlink.GOPRO_FIELD_OF_VIEW_WIDE)
self.assertEqual(self.mgr.videoFormat, VIDEO_FORMAT_NTSC)
self.mgr.processMsgQueue.assert_called_with()
self.mgr.sendState.assert_called_with()
class TestSetResponseCallback(unittest.TestCase):
def setUp(self):
shotmgr = Mock()
self.v = Mock()
shotmgr.vehicle = self.v
self.mgr = GoProManager.GoProManager(shotmgr)
self.mgr.processMsgQueue = Mock()
def testPowerOnResponse(self):
""" Test that power on response works """
message = (mavutil.mavlink.GOPRO_COMMAND_POWER, mavutil.mavlink.GOPRO_REQUEST_SUCCESS)
self.mgr.set_response_callback('vehicle','name', message)
self.mgr.processMsgQueue.assert_called_with()
def testCaptureModeResponse(self):
""" Test that capture mode response works """
message = (mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, mavutil.mavlink.GOPRO_REQUEST_SUCCESS)
self.mgr.set_response_callback('vehicle','name', message)
self.mgr.processMsgQueue.assert_called_with()
def testShutterResponse(self):
""" Test that shutter response works """
message = (mavutil.mavlink.GOPRO_COMMAND_SHUTTER, mavutil.mavlink.GOPRO_REQUEST_SUCCESS)
self.mgr.set_response_callback('vehicle','name', message)
self.mgr.processMsgQueue.assert_called_with()
def testVideoSettingsResponse(self):
""" Test that video settings response works """
message = (mavutil.mavlink.GOPRO_COMMAND_VIDEO_SETTINGS, mavutil.mavlink.GOPRO_REQUEST_SUCCESS)
self.mgr.set_response_callback('vehicle','name', message)
self.mgr.processMsgQueue.assert_called_with()
def testVideoLowLightResponse(self):
""" Test that video protune response works """
message = (mavutil.mavlink.GOPRO_COMMAND_LOW_LIGHT, mavutil.mavlink.GOPRO_REQUEST_SUCCESS)
self.mgr.set_response_callback('vehicle','name', message)
self.mgr.processMsgQueue.assert_called_with()
def testPhotoResolutionResponse(self):
""" Test that photo resolution response works """
message = (mavutil.mavlink.GOPRO_COMMAND_PHOTO_RESOLUTION, mavutil.mavlink.GOPRO_REQUEST_SUCCESS)
self.mgr.set_response_callback('vehicle','name', message)
self.mgr.processMsgQueue.assert_called_with()
def testPhotoBurstRateResponse(self):
""" Test that photo burst rate response works """
message = (mavutil.mavlink.GOPRO_COMMAND_PHOTO_BURST_RATE, mavutil.mavlink.GOPRO_REQUEST_SUCCESS)
self.mgr.set_response_callback('vehicle','name', message)
self.mgr.processMsgQueue.assert_called_with()
def testVideoProtuneResponse(self):
""" Test that video protune response works """
message = (mavutil.mavlink.GOPRO_COMMAND_PROTUNE, mavutil.mavlink.GOPRO_REQUEST_SUCCESS)
self.mgr.set_response_callback('vehicle','name', message)
self.mgr.processMsgQueue.assert_called_with()
def testVideoProtuneWhiteBalanceResponse(self):
""" Test that video protune white balance response works """
message = (mavutil.mavlink.GOPRO_COMMAND_PROTUNE_WHITE_BALANCE, mavutil.mavlink.GOPRO_REQUEST_SUCCESS)
self.mgr.set_response_callback('vehicle','name', message)
self.mgr.processMsgQueue.assert_called_with()
def testVideoProtuneColorResponse(self):
""" Test that video protune color response works """
message = (mavutil.mavlink.GOPRO_COMMAND_PROTUNE_COLOUR, mavutil.mavlink.GOPRO_REQUEST_SUCCESS)
self.mgr.set_response_callback('vehicle','name', message)
self.mgr.processMsgQueue.assert_called_with()
def testVideoProtuneGainResponse(self):
""" Test that video protune gain response works """
message = (mavutil.mavlink.GOPRO_COMMAND_PROTUNE_GAIN, mavutil.mavlink.GOPRO_REQUEST_SUCCESS)
self.mgr.set_response_callback('vehicle','name', message)
self.mgr.processMsgQueue.assert_called_with()
def testVideoProtuneSharpnessResponse(self):
""" Test that video protune sharpness response works """
message = (mavutil.mavlink.GOPRO_COMMAND_PROTUNE_SHARPNESS, mavutil.mavlink.GOPRO_REQUEST_SUCCESS)
self.mgr.set_response_callback('vehicle','name', message)
self.mgr.processMsgQueue.assert_called_with()
def testVideoProtuneExposureResponse(self):
""" Test that video protune exposure response works """
message = (mavutil.mavlink.GOPRO_COMMAND_PROTUNE_EXPOSURE, mavutil.mavlink.GOPRO_REQUEST_SUCCESS)
self.mgr.set_response_callback('vehicle','name', message)
self.mgr.processMsgQueue.assert_called_with()
def testFailedResponse(self):
""" Test that failed request sends state back to client """
self.mgr.sendState = Mock()
message = (mavutil.mavlink.GOPRO_COMMAND_VIDEO_SETTINGS, mavutil.mavlink.GOPRO_REQUEST_FAILED)
self.mgr.set_response_callback('vehicle','name', message)
self.mgr.sendState.assert_called_with()
self.mgr.processMsgQueue.assert_called_with()
class TestSendGoProRequest(unittest.TestCase):
def setUp(self):
shotmgr = Mock()
self.v = Mock()
self.v.message_factory = Mock()
shotmgr.vehicle = self.v
self.mgr = GoProManager.GoProManager(shotmgr)
self.mgr.queueMsg = Mock()
def testSendBattery(self):
""" Test sending a request to the Gopro for its battery life """
self.v.message_factory.gopro_get_request_encode.return_value = 7
self.mgr.sendGoProRequest(mavutil.mavlink.GOPRO_COMMAND_BATTERY)
self.v.message_factory.gopro_get_request_encode.assert_called_with(0, mavutil.mavlink.MAV_COMP_ID_GIMBAL,
mavutil.mavlink.GOPRO_COMMAND_BATTERY)
self.mgr.queueMsg.assert_called_with(7)
def testInvalidRequest(self):
""" Test sending an invalid request """
self.mgr.sendGoProRequest(141)
self.assertFalse(self.v.message_factory.gopro_set_request_encode.called)
class TestSendGoProCommand(unittest.TestCase):
def setUp(self):
shotmgr = Mock()
self.v = Mock()
self.v.message_factory = Mock()
shotmgr.vehicle = self.v
self.mgr = GoProManager.GoProManager(shotmgr)
self.mgr.queueMsg = Mock()
def testSendModeChange(self):
""" If we send a mode change command, we need to follow it up with a mode request to make sure it worked """
self.mgr.sendGoProRequest = Mock()
self.v.message_factory.gopro_set_request_encode.return_value = 3
self.mgr.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, (1, 0, 0, 0))
self.v.message_factory.gopro_set_request_encode.assert_called_with(0, mavutil.mavlink.MAV_COMP_ID_GIMBAL,
mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, (1, 0, 0, 0))
self.mgr.queueMsg.assert_called_with(3)
def testSendShutterTo1(self):
""" Test sending a shutter command with value 1 """
self.v.message_factory.gopro_set_request_encode.return_value = 3
self.mgr.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_SHUTTER, (1, 0, 0, 0))
self.v.message_factory.gopro_set_request_encode.assert_called_with(0, mavutil.mavlink.MAV_COMP_ID_GIMBAL,
mavutil.mavlink.GOPRO_COMMAND_SHUTTER, (1, 0, 0, 0))
self.mgr.queueMsg.assert_called_with(3)
def testSendVideoSettingsChange(self):
""" Test sending a video settings command """
self.mgr.sendGoProRequest = Mock()
self.v.message_factory.gopro_set_request_encode.return_value = 3
self.mgr.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_VIDEO_SETTINGS, (0, 3, 7, 1))
self.v.message_factory.gopro_set_request_encode.assert_called_with(0, mavutil.mavlink.MAV_COMP_ID_GIMBAL,
mavutil.mavlink.GOPRO_COMMAND_VIDEO_SETTINGS, (0, 3, 7, 1))
self.mgr.queueMsg.assert_called_with(3)
self.mgr.sendGoProRequest.assert_called_with(mavutil.mavlink.GOPRO_COMMAND_VIDEO_SETTINGS)
def testSendVideoLowLight(self):
""" Test sending a video low light command """
self.mgr.sendGoProRequest = Mock()
self.v.message_factory.gopro_set_request_encode.return_value = 3
self.mgr.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_LOW_LIGHT, (1, 0, 0, 0))
self.v.message_factory.gopro_set_request_encode.assert_called_with(0, mavutil.mavlink.MAV_COMP_ID_GIMBAL,
mavutil.mavlink.GOPRO_COMMAND_LOW_LIGHT, (1, 0, 0, 0))
self.mgr.queueMsg.assert_called_with(3)
self.mgr.sendGoProRequest.assert_called_with(mavutil.mavlink.GOPRO_COMMAND_LOW_LIGHT)
def testSendPhotoResolution(self):
""" Test sending a photo resolution command """
self.mgr.sendGoProRequest = Mock()
self.v.message_factory.gopro_set_request_encode.return_value = 3
self.mgr.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_PHOTO_RESOLUTION, (1, 0, 0, 0))
self.v.message_factory.gopro_set_request_encode.assert_called_with(0, mavutil.mavlink.MAV_COMP_ID_GIMBAL,
mavutil.mavlink.GOPRO_COMMAND_PHOTO_RESOLUTION, (1, 0, 0, 0))
self.mgr.queueMsg.assert_called_with(3)
self.mgr.sendGoProRequest.assert_called_with(mavutil.mavlink.GOPRO_COMMAND_PHOTO_RESOLUTION)
def testSendPhotoBurstRate(self):
""" Test sending a photo burst rate command """
self.mgr.sendGoProRequest = Mock()
self.mgr.sendGoProRequest = Mock()
self.v.message_factory.gopro_set_request_encode.return_value = 3
self.mgr.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_PHOTO_BURST_RATE, (1, 0, 0, 0))
self.v.message_factory.gopro_set_request_encode.assert_called_with(0, mavutil.mavlink.MAV_COMP_ID_GIMBAL,
mavutil.mavlink.GOPRO_COMMAND_PHOTO_BURST_RATE, (1, 0, 0, 0))
self.mgr.queueMsg.assert_called_with(3)
self.mgr.sendGoProRequest.assert_called_with(mavutil.mavlink.GOPRO_COMMAND_PHOTO_BURST_RATE)
def testSendVideoProtune(self):
""" Test sending a video protune command """
self.mgr.sendGoProRequest = Mock()
self.v.message_factory.gopro_set_request_encode.return_value = 3
self.mgr.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_PROTUNE, (1, 0, 0, 0))
self.v.message_factory.gopro_set_request_encode.assert_called_with(0, mavutil.mavlink.MAV_COMP_ID_GIMBAL,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE, (1, 0, 0, 0))
self.mgr.queueMsg.assert_called_with(3)
self.mgr.sendGoProRequest.assert_called_with(mavutil.mavlink.GOPRO_COMMAND_PROTUNE)
def testSendVideoProtuneWhiteBalance(self):
""" Test sending a video protune white balance command """
self.mgr.sendGoProRequest = Mock()
self.v.message_factory.gopro_set_request_encode.return_value = 3
self.mgr.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_PROTUNE_WHITE_BALANCE, (1, 0, 0, 0))
self.v.message_factory.gopro_set_request_encode.assert_called_with(0, mavutil.mavlink.MAV_COMP_ID_GIMBAL,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_WHITE_BALANCE, (1, 0, 0, 0))
self.mgr.queueMsg.assert_called_with(3)
self.mgr.sendGoProRequest.assert_called_with(mavutil.mavlink.GOPRO_COMMAND_PROTUNE_WHITE_BALANCE)
def testSendVideoProtuneColor(self):
""" Test sending a video protune color command """
self.mgr.sendGoProRequest = Mock()
self.v.message_factory.gopro_set_request_encode.return_value = 3
self.mgr.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_PROTUNE_COLOUR, (1, 0, 0, 0))
self.v.message_factory.gopro_set_request_encode.assert_called_with(0, mavutil.mavlink.MAV_COMP_ID_GIMBAL,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_COLOUR, (1, 0, 0, 0))
self.mgr.queueMsg.assert_called_with(3)
self.mgr.sendGoProRequest.assert_called_with(mavutil.mavlink.GOPRO_COMMAND_PROTUNE_COLOUR)
def testSendVideoProtuneGain(self):
""" Test sending a video protune gain command """
self.mgr.sendGoProRequest = Mock()
self.v.message_factory.gopro_set_request_encode.return_value = 3
self.mgr.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_PROTUNE_GAIN, (1, 0, 0, 0))
self.v.message_factory.gopro_set_request_encode.assert_called_with(0, mavutil.mavlink.MAV_COMP_ID_GIMBAL,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_GAIN, (1, 0, 0, 0))
self.mgr.queueMsg.assert_called_with(3)
self.mgr.sendGoProRequest.assert_called_with(mavutil.mavlink.GOPRO_COMMAND_PROTUNE_GAIN)
def testSendVideoProtuneSharpness(self):
""" Test sending a video protune sharpness command """
self.mgr.sendGoProRequest = Mock()
self.v.message_factory.gopro_set_request_encode.return_value = 3
self.mgr.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_PROTUNE_SHARPNESS, (1, 0, 0, 0))
self.v.message_factory.gopro_set_request_encode.assert_called_with(0, mavutil.mavlink.MAV_COMP_ID_GIMBAL,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_SHARPNESS, (1, 0, 0, 0))
self.mgr.queueMsg.assert_called_with(3)
self.mgr.sendGoProRequest.assert_called_with(mavutil.mavlink.GOPRO_COMMAND_PROTUNE_SHARPNESS)
def testSendVideoProtuneExposure(self):
""" Test sending a video protune exposure command """
self.mgr.sendGoProRequest = Mock()
self.v.message_factory.gopro_set_request_encode.return_value = 3
self.mgr.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_PROTUNE_EXPOSURE, (1, 0, 0, 0))
self.v.message_factory.gopro_set_request_encode.assert_called_with(0, mavutil.mavlink.MAV_COMP_ID_GIMBAL,
mavutil.mavlink.GOPRO_COMMAND_PROTUNE_EXPOSURE, (1, 0, 0, 0))
self.mgr.queueMsg.assert_called_with(3)
self.mgr.sendGoProRequest.assert_called_with(mavutil.mavlink.GOPRO_COMMAND_PROTUNE_EXPOSURE)
def testInvalidCommand(self):
""" Test sending an invalid command """
self.mgr.sendGoProCommand(140, (1, 0, 0, 0))
self.assertFalse(self.v.message_factory.gopro_set_request_encode.called)
class TestHandleRecordCommand(unittest.TestCase):
def setUp(self):
shotmgr = Mock()
self.v = Mock()
self.v.message_factory = Mock()
shotmgr.vehicle = self.v
self.mgr = GoProManager.GoProManager(shotmgr)
self.mgr.status = mavutil.mavlink.GOPRO_HEARTBEAT_STATUS_CONNECTED
self.mgr.sendGoProCommand = Mock()
self.mgr.sendState = Mock()
def testStartRecord(self):
""" Test starting video recording """
self.mgr.handleRecordCommand( CAPTURE_MODE_VIDEO, RECORD_COMMAND_START )
self.mgr.sendGoProCommand.assert_called_with(mavutil.mavlink.GOPRO_COMMAND_SHUTTER, (1, 0, 0, 0))
def testStartRecordAlreadyRecording(self):
""" Test starting video recording while already recording """
self.mgr.isRecording = True
self.mgr.captureMode = CAPTURE_MODE_VIDEO
self.mgr.handleRecordCommand( CAPTURE_MODE_VIDEO, RECORD_COMMAND_START )
self.assertTrue(self.mgr.isRecording)
assert not self.mgr.sendGoProCommand.called
assert not self.mgr.sendState.called
def testSwitchToStillsAndTakeAStill(self):
""" Should switch modes to still and take a still """
self.mgr.captureMode = CAPTURE_MODE_VIDEO
self.mgr.handleRecordCommand( CAPTURE_MODE_PHOTO, RECORD_COMMAND_START )
self.assertFalse(self.mgr.isRecording)
call1 = call(mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, (CAPTURE_MODE_PHOTO, 0, 0, 0))
call2 = call(mavutil.mavlink.GOPRO_COMMAND_SHUTTER, (1, 0, 0, 0))
self.mgr.sendGoProCommand.assert_has_calls( [call1, call2] )
def testToggleStartRecord(self):
""" If we're not recording, start recording """
self.mgr.captureMode = CAPTURE_MODE_VIDEO
self.mgr.handleRecordCommand( CAPTURE_MODE_VIDEO, RECORD_COMMAND_TOGGLE )
self.mgr.sendGoProCommand.assert_called_with(mavutil.mavlink.GOPRO_COMMAND_SHUTTER, (1, 0, 0, 0))
def testToggleStopRecord(self):
""" If we're recording, stop recording """
self.mgr.captureMode = CAPTURE_MODE_VIDEO
self.mgr.isRecording = True
self.mgr.handleRecordCommand( CAPTURE_MODE_VIDEO, RECORD_COMMAND_TOGGLE )
self.mgr.sendGoProCommand.assert_called_with(mavutil.mavlink.GOPRO_COMMAND_SHUTTER, (0, 0, 0, 0))
def testStartBurstShot(self):
'''If GoPro is in burst shot mode then record button takes a burst shot'''
self.mgr.captureMode = CAPTURE_MODE_BURST
self.mgr.handleRecordCommand( CAPTURE_MODE_BURST, RECORD_COMMAND_START )
self.mgr.sendGoProCommand.assert_called_with(mavutil.mavlink.GOPRO_COMMAND_SHUTTER, (1, 0, 0, 0))
def testWrongMode(self):
""" Don't do anything if we're not in the right mode """
self.mgr.status = mavutil.mavlink.GOPRO_HEARTBEAT_STATUS_DISCONNECTED
self.mgr.handleRecordCommand( CAPTURE_MODE_VIDEO, RECORD_COMMAND_TOGGLE )
self.assertFalse(self.mgr.sendGoProCommand.called)
class TestQueueMsg(unittest.TestCase):
def setUp(self):
shotmgr = Mock()
self.v = Mock()
shotmgr.vehicle = self.v
self.mgr = GoProManager.GoProManager(shotmgr)
def testQueueMsg(self):
""" Test queuing up a message """
self.mgr.isGoproBusy = True
self.mgr.lastRequestSent = monotonic.monotonic()
self.mgr.queueMsg(4)
self.assertFalse( self.mgr.msgQueue.empty() )
self.assertTrue(self.mgr.isGoproBusy)
def testQueueMultiMsg(self):
""" Test queuing up multiple messages """
for i in range(10):
self.mgr.queueMsg(i)
self.assertEqual( self.mgr.msgQueue.qsize(), 9)
def testQueueSend(self):
""" if the queue is not busy, we start sending a message instead of queuing up """
self.mgr.queueMsg(37)
self.assertTrue( self.mgr.msgQueue.empty() )
self.v.send_mavlink.assert_called_with(37)
def testQueueFlushQueue(self):
""" Test queue is flushed with outstanding message sent more than 2 seconds ago """
self.mgr.sendState = Mock()
self.mgr.isGoproBusy = True
self.mgr.lastRequestSent = monotonic.monotonic()
self.mgr.queueMsg(1)
self.mgr.queueMsg(2)
self.assertEqual(self.mgr.msgQueue.qsize(), 2)
self.mgr.lastRequestSent = monotonic.monotonic() - 3.0
self.mgr.queueMsg(3)
self.assertTrue(self.mgr.msgQueue.empty)
self.mgr.sendState.assert_called_with()
class TestProcessMsgQueue(unittest.TestCase):
def setUp(self):
shotmgr = Mock()
self.v = Mock()
shotmgr.vehicle = self.v
self.mgr = GoProManager.GoProManager(shotmgr)
def testSendNextMessage(self):
""" send the next message in the queue """
self.mgr.isGoproBusy = True
self.mgr.lastRequestSent = monotonic.monotonic()
self.mgr.queueMsg(3)
self.mgr.queueMsg(2)
self.mgr.queueMsg(1)
self.mgr.processMsgQueue()
self.v.send_mavlink.assert_called_with(3)
self.assertEqual( self.mgr.msgQueue.qsize(), 2)
def testQueueisEmpty(self):
""" if our queue is empty, set ourselves to not busy """
self.mgr.isGoproBusy = True
self.mgr.processMsgQueue()
self.assertFalse( self.mgr.isGoproBusy )
class TestHandlePacket(unittest.TestCase):
def setUp(self):
shotmgr = Mock()
self.mgr = GoProManager.GoProManager(shotmgr)
def testEnableGoPro(self):
""" Should enable gopro commands """
pkt = struct.pack('<I', 1)
self.mgr.setGoProEnabled = Mock()
self.mgr.handlePacket(app_packet.GOPRO_SET_ENABLED, pkt)
self.mgr.setGoProEnabled.assert_called_with(True)
def testSetRequest(self):
""" Handle a set request """
self.mgr.sendGoProCommand = Mock()
value = struct.pack('<HH', 8, 22)
self.mgr.handlePacket(app_packet.GOPRO_SET_REQUEST, value)
self.mgr.sendGoProCommand.assert_called_with( 8, (22, 0, 0, 0) )
def testRecord(self):
""" Handle a record packet """
self.mgr.handleRecordCommand = Mock()
self.mgr.captureMode = CAPTURE_MODE_VIDEO
value = struct.pack('<I', 1)
self.mgr.handlePacket(app_packet.GOPRO_RECORD, value)
self.mgr.handleRecordCommand.assert_called_with( CAPTURE_MODE_VIDEO, 1 )
def testStateRequest(self):
""" Should send state on request """
pkt = struct.pack('<')
self.mgr.sendState = Mock()
self.mgr.handlePacket(app_packet.GOPRO_REQUEST_STATE, pkt)
self.mgr.sendState.assert_called_with()
def testSetExtendedRequest(self):
""" Handle extended payload settings request """
self.mgr.sendGoProCommand = Mock()
value = struct.pack('<HBBBB', 5, 0, 3, 7, 1)
self.mgr.handlePacket(app_packet.GOPRO_SET_EXTENDED_REQUEST, value)
self.mgr.sendGoProCommand.assert_called_with(5, (0, 3, 7, 1))
class TestSendState(unittest.TestCase):
def setUp(self):
shotmgr = Mock()
self.mgr = GoProManager.GoProManager(shotmgr)
def testSendState(self):
""" Send state sends the current gopro state to the app """
self.mgr.enabled = 1
self.mgr.model = MODEL_HERO3PLUS_BLACK
self.mgr.status = STATUS_GOPRO_CONNECTED
self.mgr.isRecording = False
self.mgr.captureMode = CAPTURE_MODE_VIDEO
self.mgr.videoFormat = VIDEO_FORMAT_NTSC
self.mgr.videoResolution = 3
self.mgr.videoFrameRate = 1
self.mgr.videoFieldOfView = 2
self.mgr.videoLowLight = True
self.mgr.photoResolution = 1
self.mgr.photoBurstRate = 2
self.mgr.videoProtune = True
self.mgr.videoProtuneWhiteBalance = 2
self.mgr.videoProtuneColor = 1
self.mgr.videoProtuneGain = 3
self.mgr.videoProtuneSharpness = 2
self.mgr.videoProtuneExposure = 1
# Send old spec version
# 2 unsigned shorts for a header, 26 unsigned bytes, then 5 unsigned shorts
pkt1 = struct.pack('<IIBBBBBBBBBBBBBBBBBBBBBBBBBBHHHHH', app_packet.GOPRO_V1_STATE, 36, \
GOPRO_V1_SPEC_VERSION,
self.mgr.model,
self.mgr.status,
self.mgr.isRecording,
self.mgr.captureMode,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0
)
# send new spec version
# 2 unsigned shorts for a header, 26 unsigned bytes, then 5 unsigned shorts
pkt2 = struct.pack('<IIBBBBBBBBBBBBBBBBBBBBBBBBBBHHHHH', app_packet.GOPRO_V2_STATE, 36, \
GOPRO_V2_SPEC_VERSION,
self.mgr.model,
self.mgr.status,
self.mgr.isRecording,
self.mgr.captureMode,
self.mgr.videoFormat,
self.mgr.videoResolution,
self.mgr.videoFrameRate,
self.mgr.videoFieldOfView,
self.mgr.videoLowLight,
self.mgr.photoResolution,
self.mgr.photoBurstRate,
self.mgr.videoProtune,
self.mgr.videoProtuneWhiteBalance,
self.mgr.videoProtuneColor,
self.mgr.videoProtuneGain,
self.mgr.videoProtuneSharpness,
self.mgr.videoProtuneExposure,
self.mgr.enabled,
0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0
)
self.mgr.sendState()
call1 = call(pkt1)
call2 = call(pkt2)
self.mgr.shotMgr.appMgr.sendPacket.assert_has_calls([call1, call2])
class TestSetGimbalEnabledParam(unittest.TestCase):
def setUp(self):
shotmgr = Mock()
self.mgr = GoProManager.GoProManager(shotmgr)
def testSetEnabled(self):
""" Tell gimbal to turn on gopro commands """
self.mgr.enabled = True
self.mgr.setGimbalEnabledParam()
self.mgr.shotMgr.vehicle.message_factory.param_set_encode.assert_called_with(0, mavutil.mavlink.MAV_COMP_ID_GIMBAL, # target system, target component
"GMB_GP_CTRL", 1.0, mavutil.mavlink.MAV_PARAM_TYPE_REAL32 )
def testSetDisabled(self):
""" Tell gimbal to turn off gopro commands """
self.mgr.enabled = False
self.mgr.setGimbalEnabledParam()
self.mgr.shotMgr.vehicle.message_factory.param_set_encode.assert_called_with(0, mavutil.mavlink.MAV_COMP_ID_GIMBAL, # target system, target component
"GMB_GP_CTRL", 0.0, mavutil.mavlink.MAV_PARAM_TYPE_REAL32 )
class TestSetGoProEnabled(unittest.TestCase):
def setUp(self):
shotmgr = Mock()
self.mgr = GoProManager.GoProManager(shotmgr)
mock = patch('settings.writeSetting')
self.addCleanup(mock.stop)
self.mockWrite = mock.start()
self.mgr.setGimbalEnabledParam = Mock()
def testSetEnabled(self):
""" Tell gimbal to turn on gopro commands """
self.mgr.setGoProEnabled(True)
self.assertTrue(self.mgr.enabled)
self.mockWrite.assert_called_with("GoProEnabled", "1")
self.mgr.setGimbalEnabledParam.assert_called_with()
def testSetDisabled(self):
""" Tell gimbal to turn off gopro commands """
self.mgr.setGoProEnabled(False)
self.assertFalse(self.mgr.enabled)
self.mockWrite.assert_called_with("GoProEnabled", "0")
self.mgr.setGimbalEnabledParam.assert_called_with()

112
shotmanager/Test/TestLeashController.py Normal file
View File

@ -0,0 +1,112 @@
# TestLeashController.py
# shotmanager
#
# Unit tests for the orbit-based Leash controller.
#
# Created by Nick Speal on 3/16/2016.
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import leashController
from leashController import *
from dronekit import LocationGlobalRelative
import unittest
import mock
from mock import call
from mock import Mock
from mock import MagicMock
from mock import patch
class testInit(unittest.TestCase):
def setUp(self):
roi = LocationGlobalRelative(37.873168,-122.302062, 1) # lat,lon,alt
self.startRadius = 20 # meters
self.startAzimuth = 15 # deg
self.startAltitude = 10 # meters
self.startLocation = location_helpers.newLocationFromAzimuthAndDistance(roi,self.startAzimuth,self.startRadius)
self.startLocation.alt = self.startAltitude
#Run the controller constructor
self.controller = leashController.LeashController(roi, self.startRadius, self.startAzimuth, self.startAltitude)
def testInheritanceDidCall(self):
# TODO
# want to test if the orbitController class actually gets inherited. Not sure how exactly.
pass
class testMove(unittest.TestCase):
def setUp(self):
self.oldROI = LocationGlobalRelative(37.873168,-122.302062, 1) # lat,lon,alt
self.startRadius = 20 # meters
self.startAzimuth = 15 # deg
self.startAltitude = 10 # meters
self.startLocation = location_helpers.newLocationFromAzimuthAndDistance(self.oldROI,self.startAzimuth,self.startRadius)
self.startLocation.alt = self.startAltitude
#Run the controller constructor
self.controller = leashController.LeashController(self.oldROI, self.startRadius, self.startAzimuth, self.startAltitude)
#Arbirary sticks
throttle = 0.3
roll = -0.4
pitch = 0.5
yaw = -0.6
raw_paddle = 0.7
self.channels = [throttle, roll, pitch, yaw, 0.0, 0.0, 0.0, raw_paddle]
#Attributes
self.controller.maxClimbRate = 5.
#methods
self.controller._approach = Mock(return_value = Vector3())
self.controller._climb = Mock(return_value = Vector3())
def testRadiusReset(self):
self.controller.approachSpeed = 1
self.controller.distance = self.startRadius/2.
newROI = location_helpers.newLocationFromAzimuthAndDistance(self.startLocation,self.startAzimuth,self.controller.distance)
self.controller.move(self.channels, newroi = newROI)
#No way to assert execution of the if statement..
def testVelocityUpdateMethodsAreCalled(self):
self.controller.move(self.channels, newroi = self.oldROI)
assert self.controller._approach.called
assert self.controller._climb.called
def testStopCaseWithStaticROI(self):
self.controller.approachSpeed = 0
newROI = self.oldROI # Same as original roi
self.controller.move(self.channels, newroi = newROI)
def testStopCaseWithApproachSpeed(self):
newROI = location_helpers.newLocationFromAzimuthAndDistance(self.oldROI,0.0, 50)
self.controller.approachSpeed = 1
self.controller.move(self.channels, newroi = newROI)
def testDontStop(self):
newROI = location_helpers.newLocationFromAzimuthAndDistance(self.oldROI,0.0, 50)
self.controller.approachSpeed = 0
self.controller.move(self.channels, newroi = newROI)

230
shotmanager/Test/TestLocationHelpers.py Normal file
View File

@ -0,0 +1,230 @@
# Unit tests for location_helpers
import math
import mock
import os
from os import sys, path
import unittest
from dronekit import LocationGlobalRelative
sys.path.append(os.path.realpath('..'))
import location_helpers
from vector3 import Vector3
# for distances and angles
ERROR = 0.1
# for lat and lon errors
ERROR_LOC = 0.000001
class TestDistanceFromPoints(unittest.TestCase):
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 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 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 )
class TestDistanceFromPoints3d(unittest.TestCase):
def test3dDistAltDiff(self):
""" Only difference between these points is altitude """
alt1 = 17.23463
alt2 = 40.4564
loc = LocationGlobalRelative(83.234632, -42.823752, alt1)
loc2 = LocationGlobalRelative(83.234632, -42.823752, alt2)
diff = alt2 - alt1
dist = location_helpers.getDistanceFromPoints3d( loc, loc2 )
delta = diff - dist
self.assertTrue( abs( delta ) < ERROR )
def testSomeKnown3dDist(self):
""" Known 3d distance apart """
alt1 = 64.234
alt2 = 12.2345
loc = LocationGlobalRelative(83.45234, 9.3452346, alt1)
loc2 = LocationGlobalRelative(83.4523, 9.3452344, alt2)
dist = location_helpers.getDistanceFromPoints3d( loc, loc2 )
delta = dist - 52.1
self.assertTrue( abs( delta ) < ERROR )
class TestNewLocationFromAzimuthAndDistance(unittest.TestCase):
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 )
class TestCalcAzimuthFromPoints(unittest.TestCase):
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 )
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 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 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 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 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 )
class TestGetVectorFromPoints(unittest.TestCase):
def testAltUp(self):
""" Test a vector straight up """
alt1 = 30.28374
alt2 = 11.234865
loc = LocationGlobalRelative(23.5445, -12.3333, alt1)
loc2 = LocationGlobalRelative(23.5445, -12.3333, alt2)
diff = alt2 - alt1
vec = location_helpers.getVectorFromPoints(loc, loc2)
self.assertTrue( vec.x == 0.0 )
self.assertTrue( vec.y == 0.0 )
self.assertTrue( vec.z == diff )
def testLength(self):
""" Test a vector's length """
loc = LocationGlobalRelative(-56.34563, -10.23463246, 11.1235235)
loc2 = LocationGlobalRelative(-56.34561, -10.2346328, 16.3453)
vec = location_helpers.getVectorFromPoints(loc, loc2)
length = vec.normalize()
dist = location_helpers.getDistanceFromPoints3d(loc, loc2)
self.assertTrue( abs(length - dist) < ERROR )
def testKnown(self):
""" Test against Google Earth Data """
loc = LocationGlobalRelative( 21.5, -77.8, 0)
loc2 = LocationGlobalRelative( 21.51, -77.79, 0)
vec = location_helpers.getVectorFromPoints(loc, loc2)
self.assertTrue( abs( vec.x - 1111.950000 ) < ERROR_LOC )
self.assertTrue( abs( vec.y - 1034.577815 ) < ERROR_LOC )
def testKnown2(self):
""" Test against Google Earth Data """
loc = LocationGlobalRelative( 83.5, 9.2, 0)
loc2 = LocationGlobalRelative( 83.51, 9.21, 0)
vec = location_helpers.getVectorFromPoints(loc, loc2)
self.assertTrue( abs( vec.x - 1111.950000 ) < ERROR_LOC )
self.assertTrue( abs( vec.y - 125.876314 ) < ERROR_LOC )
class TestAddVectorToLocation(unittest.TestCase):
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 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 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 )
class TestSpotLock(unittest.TestCase):
def testSpotLockN(self):
""" Test spot loc North """
loc = LocationGlobalRelative(34.5, -20.8, 100)
pitch = -45
yaw = 0
newloc = location_helpers.getSpotLock(loc, pitch, yaw)
self.assertTrue( abs( newloc.lat - 34.500899 ) < ERROR_LOC )
self.assertTrue( abs( newloc.lon - -20.8 ) < ERROR_LOC )
self.assertTrue( newloc.alt == 0.0 )
def testSpotLockSE(self):
""" Test spot loc SE """
loc = LocationGlobalRelative(34.5, -20.8, 100)
pitch = -70
yaw = 135
newloc = location_helpers.getSpotLock(loc, pitch, yaw)
self.assertTrue( abs( newloc.lat - 34.499769 ) < ERROR_LOC )
self.assertTrue( abs( newloc.lon - -20.799719 ) < ERROR_LOC )
self.assertTrue( newloc.alt == 0.0 )
def testSpotLockW(self):
""" Test spot loc W at Shallow angle threshold """
loc = LocationGlobalRelative(34.5, -20.8, 100)
pitch = -6
yaw = 270
newloc = location_helpers.getSpotLock(loc, pitch, yaw)
self.assertTrue( abs( newloc.lat - 34.5 ) < ERROR_LOC )
self.assertTrue( abs( newloc.lon - -20.810382 ) < ERROR_LOC )
self.assertTrue( newloc.alt == 0.0 )

124
shotmanager/Test/TestLookAtController.py Normal file
View File

@ -0,0 +1,124 @@
# TestLookAtController.py
# shotmanager
#
# Unit tests for the orbit controller.
#
# Created by Nick Speal on 3/16/2016.
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import lookAtController
from lookAtController import *
from dronekit import LocationGlobalRelative
import unittest
import mock
from mock import call
from mock import Mock
from mock import MagicMock
from mock import patch
class TestSetOptions(unittest.TestCase):
def setUp(self):
origin = LocationGlobalRelative(37.873168,-122.302062, 0) # lat,lon,alt
'''Create a mock vehicle object'''
vehicle = mock.create_autospec(Vehicle)
controllerAlt = 5
#Run the controller constructor
self.controller = lookAtController.LookAtController(vehicle, controllerAlt)
def testSetBothOptions(self):
'''Tests setting both options'''
self.controller.setOptions(maxClimbRate = 3, maxAlt = 75)
self.assertEqual(self.controller.maxClimbRate, 3)
self.assertEqual(self.controller.maxAlt, 75)
def testSetOptionsNoAltLimit(self):
'''Tests setting both options, with disabled altitude limit'''
self.controller.setOptions(maxClimbRate = 3, maxAlt = None)
self.assertEqual(self.controller.maxClimbRate, 3)
self.assertEqual(self.controller.maxAlt, None)
class TestMove(unittest.TestCase):
def setUp(self):
'''Create a mock vehicle object'''
vehicle = mock.create_autospec(Vehicle)
controllerAlt = 105
#Run the controller constructor
self.controller = lookAtController.LookAtController(vehicle, controllerAlt)
self.controller._climb = Mock(return_value = Vector3())
#Set options
self.controller.maxClimbRate = 5 # m/s
self.controller.maxAlt = 100 #m
#Arbirary sticks
throttle = 0.3
roll = -0.4
pitch = 0.5
yaw = -0.6
raw_paddle = 0.7
self.channels = [throttle, roll, pitch, yaw, 0.0, 0.0, 0.0, raw_paddle]
self.controller.move(self.channels)
def testCallClimb(self):
'''Tests that _climb() is called'''
self.controller._climb.assert_called_with(0.3)
class TestClimb(unittest.TestCase):
def setUp(self):
'''Create a mock vehicle object'''
vehicle = mock.create_autospec(Vehicle)
controllerAlt = 5
#Run the controller constructor
self.controller = lookAtController.LookAtController(vehicle, controllerAlt)
# set options
self.controller.setOptions(maxClimbRate=1.5 ,maxAlt=45)
def testClimbUp(self):
'''Test climbing up with stick up'''
self.controller.altitude = 100
climbVel = self.controller._climb(1.0)
self.assertEqual(climbVel, Vector3(0,0,1.0*self.controller.maxClimbRate))
def testClimbDown(self):
'''Test descending with stick down'''
self.controller.altitude = 100
climbVel = self.controller._climb(-1.0)
self.assertEqual(climbVel, Vector3(0,0,-1.0*self.controller.maxClimbRate))
def testAltitudeLimit(self):
'''Test that maximum altitude is enforced when climbing'''
self.controller.altitude = 105
climbVel = self.controller._climb(-1.0)
self.assertEqual(self.controller.altitude, self.controller.maxAlt)

41
shotmanager/Test/TestModes.py Normal file
View File

@ -0,0 +1,41 @@
# Unit tests for modes
import mock
import os
from os import sys, path
import unittest
from dronekit import Vehicle
sys.path.append(os.path.realpath('..'))
import modes
class TestModeLookup(unittest.TestCase):
def setUp(self):
self.v = mock.create_autospec(Vehicle)
self.v._mode_mapping = { "ALT_HOLD" : 2,
"TEST_ME" : 317 }
def testLookupAltHold(self):
""" Look up 'ALT_HOLD' """
i = modes.getAPMModeIndexFromName( 'ALT_HOLD', self.v )
self.assertEqual( i, 2 )
def testLookupTestMe(self):
""" Look up 'TEST_ME' """
i = modes.getAPMModeIndexFromName( 'TEST_ME', self.v )
self.assertEqual( i, 317 )
def testLookupModeParser(self):
""" Look up 'Mode(10)' """
i = modes.getAPMModeIndexFromName( 'Mode(10)', self.v )
self.assertEqual( i, 10 )
def testMalformedMode(self):
""" Look up 'Mode(asta' """
i = modes.getAPMModeIndexFromName( 'Mode(asta', self.v )
self.assertEqual( i, -1 )
def testLookupInvalidMode(self):
""" Look up 'Invalid' """
i = modes.getAPMModeIndexFromName( 'Invalid', self.v )
self.assertEqual( i, -1 )

1371
shotmanager/Test/TestMultipoint.py Normal file
View File

File diff suppressed because it is too large Load Diff

522
shotmanager/Test/TestOrbit.py Normal file
View File

@ -0,0 +1,522 @@
# TestOrbit.py
# shotmanager
#
# Unit tests for the orbit smart shot.
#
# Created by Will Silva and Eric Liao on 1/19/2015.
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import orbit
from orbit import *
import shotManager
from shotManager import ShotManager
import unittest
import mock
from mock import call
from mock import Mock
from mock import MagicMock
from mock import patch
class TestShotInit(unittest.TestCase):
def setUp(self):
'''Create a mock vehicle object'''
vehicle = mock.create_autospec(Vehicle)
'''Create a mock shotManager object'''
shotmgr = mock.create_autospec(ShotManager)
shotmgr.getParam.return_value = 0 # so mock doesn't do lazy binds
shotmgr.rcMgr = Mock(specs=['remapper'])
'''Run the shot constructor'''
self.shot = orbit.OrbitShot(vehicle, shotmgr)
def testInit(self):
'''Test that the shot initialized properly'''
# vehicle object should be created (not None)
self.assertNotEqual(self.shot.vehicle, None)
# shotManager object should be created (not None)
self.assertNotEqual(self.shot.shotmgr, None)
# roi should be None
self.assertEqual(self.shot.roi, None)
# pathController should be None
self.assertEqual(self.shot.pathController, None)
# ticksPaddleCentered should be initialized to infinity
self.assertEqual(self.shot.ticksPaddleCentered, float('inf'))
# pathHandler should be None
self.assertEqual(self.shot.pathHandler, None)
# shotmgr.getParam should be called thrice
# once for maxClimbRate and once for maxAlt and once for FENCE_ENABLE
calls = [call("PILOT_VELZ_MAX", DEFAULT_PILOT_VELZ_MAX_VALUE), call("FENCE_ALT_MAX", DEFAULT_FENCE_ALT_MAX), call("FENCE_ENABLE", DEFAULT_FENCE_ENABLE)]
self.shot.shotmgr.getParam.assert_has_calls(calls)
class TestHandleRCs(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.getParam.return_value = 0 # so mock doesn't do lazy binds
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = orbit.OrbitShot(vehicle, shotmgr)
#set roi
self.shot.roi = LocationGlobalRelative(37.873168,-122.302062, 0) #3DR
#Mock the pathHandler object
self.shot.pathHandler = mock.create_autospec(pathHandler.PathHandler)
self.shot.pathHandler.cruiseSpeed = 0
#Mock the pathController object
self.shot.pathController = mock.create_autospec(OrbitController)
self.shot.pathController.move.return_value = (LocationGlobalRelative(37.873168,-122.302062, 0),Vector3(0,0,0))
#Neutral sticks
throttle = 0.0
roll = 0.0
pitch = 0.0
yaw = 0.0
self.channels = [throttle, roll, pitch, yaw, 0.0, 0.0, 0.0, 0.0]
def testNoROI(self):
'''If not ROI is set then do NOT continue'''
self.shot.roi = None
self.shot.handleRCs(self.channels)
assert not self.shot.pathController.called
def testSetPositionMsg(self):
'''Test that we're sending a lat,lon,alt/x,y,z vel to APM'''
self.shot.handleRCs(self.channels)
self.shot.vehicle.message_factory.set_position_target_global_int_encode.assert_called_with(
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
37.873168*1E7, -122.302062*1E7, 0, # x, y, z positions
0, 0, 0, # x, y, z velocity in m/s
0, 0, 0, # x, y, z acceleration (not used)
0, 0) # yaw, yaw_rate (not used)
self.shot.vehicle.send_mavlink.assert_called_with(mock.ANY)
def testCallsetROIAltitude(self):
'''Tests that we're calling setROIAltitude() with correct channels'''
self.channels[5] = 0.5
self.channels[7] = 0.75
self.shot.setROIAltitude = Mock()
self.shot.handleRCs(self.channels)
self.shot.setROIAltitude.assert_called_with(0.5,0.75)
def testsetROIAltitudeMsg(self):
'''Test that we're sending an roi lat,lon,alt to APM'''
self.shot.handleRCs(self.channels)
self.shot.vehicle.message_factory.command_long_encode.assert_called_with(
0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_SET_ROI, #command
0, #confirmation
0, 0, 0, 0, #params 1-4
self.shot.roi.lat,
self.shot.roi.lon,
self.shot.roi.alt
)
self.shot.vehicle.send_mavlink.assert_called_with(mock.ANY)
class TestAddLocation(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.appMgr = Mock()
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = orbit.OrbitShot(vehicle, shotmgr)
#Mock initOrbitShot
self.shot.initOrbitShot = Mock()
@mock.patch('location_helpers.getDistanceFromPoints', return_value = 10)
@mock.patch('location_helpers.calcAzimuthFromPoints', return_value = 45)
def testCallingLocationHelperFunctions(self, location_helpers_calcAzimuthFromPoints, location_helpers_getDistanceFromPoints):
'''Make sure all location_helpers functions are called'''
self.shot.addLocation(LocationGlobalRelative(37.873168,-122.302062, 0))
location_helpers_getDistanceFromPoints.assert_called_with(self.shot.roi, self.shot.vehicle.location.global_relative_frame)
location_helpers_calcAzimuthFromPoints.assert_called_with(self.shot.roi, self.shot.vehicle.location.global_relative_frame)
@mock.patch('orbitController.OrbitController', return_value = mock.create_autospec(OrbitController))
def testCreatingPathController(self, OrbitController):
'''Test that the pathController object is instantiated'''
self.shot.addLocation(LocationGlobalRelative(37.873168,-122.302062, 0))
self.assertNotEqual(self.shot.pathController, None)
self.shot.pathController.setOptions.assert_called_with(maxAlt = mock.ANY, maxClimbRate = mock.ANY)
def testNoPreviouslysetROIAltitude(self):
'''If no roi has been set previously, then call initOrbitShot()'''
self.shot.roi = None
self.shot.initOrbitShot = Mock()
self.shot.addLocation(LocationGlobalRelative(37.873168,-122.302062, 0))
self.assertEqual(self.shot.roi.lat, 37.873168)
self.assertEqual(self.shot.roi.lon, -122.302062)
self.assertEqual(self.shot.roi.alt, 0)
self.shot.initOrbitShot.assert_called_with()
def testOverwriteROI(self):
'''If an ROI is already set, overwrite it & call initOrbitShot()'''
self.shot.roi = LocationGlobalRelative(37.873168,-122.302062, 0)
self.shot.initOrbitShot = Mock()
self.shot.addLocation(LocationGlobalRelative(37.873169,-122.302063, 5))
self.assertEqual(self.shot.roi.lat, 37.873169)
self.assertEqual(self.shot.roi.lon, -122.302063)
self.assertEqual(self.shot.roi.alt, 5)
assert self.shot.initOrbitShot.called
def testSendROIToApp(self):
'''Send the added ROI to the app as confirmation'''
self.shot.roi = LocationGlobalRelative(37.873168,-122.302062, 0)
self.shot.addLocation(LocationGlobalRelative(37.873169,-122.302063, 5))
expectedPacket = struct.pack('<IIddf', app_packet.SOLO_MESSAGE_LOCATION, 20, self.shot.roi.lat, self.shot.roi.lon, self.shot.roi.alt)
self.shot.shotmgr.appMgr.sendPacket.assert_called_with(expectedPacket)
class TestSpotLock(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = orbit.OrbitShot(vehicle, shotmgr)
#Mock addLocation()
self.shot.addLocation = Mock()
@mock.patch('location_helpers.getSpotLock', return_value = LocationGlobalRelative(37.873168,-122.302062, 0))
@mock.patch('camera.getYaw', return_value = 0)
@mock.patch('camera.getPitch', return_value = 0)
def testIfPitchIsZero(self, camera_getPitch, camera_getYaw, location_helpers_getSpotLock):
'''If camera pitch is zero then set an ROI at min angle'''
self.shot.spotLock()
location_helpers_getSpotLock.assert_called_with(self.shot.vehicle.location.global_relative_frame, orbit.SHALLOW_ANGLE_THRESHOLD,0)
@mock.patch('location_helpers.getSpotLock', return_value = LocationGlobalRelative(37.873168,-122.302062, 0))
@mock.patch('camera.getYaw', return_value = 0)
@mock.patch('camera.getPitch', return_value = orbit.SHALLOW_ANGLE_THRESHOLD + 1)
def testIfPitchGreaterThanShallowAngleThreshold(self, camera_getPitch, camera_getYaw, location_helpers_getSpotLock):
'''If camera pitch is greater than SHALLOW_ANGLE_THRESHOLD'''
self.shot.spotLock()
location_helpers_getSpotLock.assert_called_with(self.shot.vehicle.location.global_relative_frame, orbit.SHALLOW_ANGLE_THRESHOLD, camera_getYaw(self.shot.vehicle))
@mock.patch('location_helpers.getSpotLock', return_value = LocationGlobalRelative(37.873168,-122.302062, 0))
@mock.patch('camera.getYaw', return_value = 0)
@mock.patch('camera.getPitch', return_value = orbit.SHALLOW_ANGLE_THRESHOLD - 1)
def testIfPitchLessThanShallowAngleThreshold(self, camera_getPitch, camera_getYaw, location_helpers_getSpotLock):
'''If camera pitch is greater than SHALLOW_ANGLE_THRESHOLD'''
self.shot.spotLock()
location_helpers_getSpotLock.assert_called_with(self.shot.vehicle.location.global_relative_frame, orbit.SHALLOW_ANGLE_THRESHOLD-1, camera_getYaw(self.shot.vehicle))
def testCallAddLocation(self):
'''Test if addLocation is called (value not checked in this test)'''
self.shot.spotLock()
self.shot.addLocation.assert_called_with(mock.ANY)
class TestinitOrbitShot(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = orbit.OrbitShot(vehicle, shotmgr)
#Set ROI
self.shot.roi = LocationGlobalRelative(37.873168,-122.302062, 0)
#Mock setButtonMappings()
self.shot.setButtonMappings = Mock()
def testEnterGUIDEDMode(self):
'''Test that initOrbitShot() puts us into GUIDED mode'''
self.shot.initOrbitShot()
self.assertEqual(self.shot.vehicle.mode.name, "GUIDED")
def testSettingGimbalROI(self):
'''Test that we're setting the roi as the gimbal ROI'''
self.shot.initOrbitShot()
expectedMsg = self.shot.vehicle.message_factory.mount_configure_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, #mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
self.shot.vehicle.send_mavlink.assert_called_with(expectedMsg)
def testEnableRemapping(self):
'''Test that remapping is enabled'''
self.shot.initOrbitShot()
self.shot.shotmgr.rcMgr.enableRemapping.assert_called_with(True)
def testCreatePathHandlerObject(self):
'''Test that we are creating the pathHandler object'''
self.shot.initOrbitShot()
self.assertNotEqual(self.shot.pathHandler, None)
def testSetButtonMappings(self):
'''Test that we call setButtonMappings'''
self.shot.initOrbitShot()
self.shot.setButtonMappings.assert_called_with()
class TestSetButtonMappings(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.buttonManager = Mock()
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = orbit.OrbitShot(vehicle, shotmgr)
def testNoROI(self):
'''Test setButtonMappings() before an ROI is set'''
self.shot.setButtonMappings()
calls = [call(btn_msg.ButtonA, shots.APP_SHOT_ORBIT, btn_msg.ARTOO_BITMASK_ENABLED, "Begin\0"), call(btn_msg.ButtonB, shots.APP_SHOT_ORBIT, 0, "\0")]
self.shot.shotmgr.buttonManager.setArtooButton.assert_has_calls(calls)
def testROISet(self):
'''Test setButtonMappings() after an ROI is set'''
self.shot.roi = LocationGlobalRelative(37.873168,-122.302062, 0)
self.shot.setButtonMappings()
calls = [call(btn_msg.ButtonA, shots.APP_SHOT_ORBIT, 0, "\0"), call(btn_msg.ButtonB, shots.APP_SHOT_ORBIT, 0, "\0")]
self.shot.shotmgr.buttonManager.setArtooButton.assert_has_calls(calls)
class TestHandleButton(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = orbit.OrbitShot(vehicle, shotmgr)
self.shot.spotLock = Mock()
self.shot.updateAppOptions = Mock()
def testAButtonNoROI(self):
'''Test A button press without roi set'''
self.shot.handleButton(btn_msg.ButtonA, btn_msg.Press)
self.shot.spotLock.assert_called_with()
def testAButtonROISet(self):
'''Test A Button press with roi set'''
self.shot.roi = LocationGlobalRelative(37.873168,-122.302062, 0)
self.shot.handleButton(btn_msg.ButtonA, btn_msg.Press)
assert not self.shot.spotLock.called
def testPauseButtonNoPathHandler(self):
'''Test B Button press without pathHandler set'''
self.shot.handleButton(btn_msg.ButtonLoiter, btn_msg.Press)
self.shot.shotmgr.notifyPause.assert_called_with(False)
def testPauseButtonPathHandlerSet(self):
'''Test B Button press with pathHandler set'''
self.shot.pathHandler = mock.create_autospec(pathHandler.PathHandler)
self.shot.handleButton(btn_msg.ButtonLoiter, btn_msg.Press)
self.shot.pathHandler.togglePause.assert_called_with()
self.shot.updateAppOptions.assert_called_with()
self.shot.shotmgr.notifyPause.assert_called_with(True)
class TestHandlePacket(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = orbit.OrbitShot(vehicle, shotmgr)
#Mock pathHandler object
self.shot.pathHandler = mock.create_autospec(pathHandler.PathHandler)
@mock.patch('struct.unpack')
def testNoPathHandler(self, struct_unpack):
'''Test if pathHandler isn't set yet'''
self.shot.pathHandler = None
options = struct.pack('<f', 0.0)
self.shot.handlePacket(app_packet.SOLO_SHOT_OPTIONS, 4, options)
assert not struct_unpack.called
def testCruiseSpeedSet0(self):
'''Test setting cruiseSpeed to zero'''
options = struct.pack('<f', 0.0)
self.shot.handlePacket(app_packet.SOLO_SHOT_OPTIONS, 4, options)
self.shot.pathHandler.setCruiseSpeed.assert_called_with(0.0)
def testCruiseSpeedSet6(self):
'''Test setting cruiseSpeed to 6'''
options = struct.pack('<f', 6.0)
self.shot.handlePacket(app_packet.SOLO_SHOT_OPTIONS, 4, options)
self.shot.pathHandler.setCruiseSpeed.assert_called_with(6.0)
class TestUpdateAppOptions(unittest.TestCase):
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.appMgr = Mock()
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = orbit.OrbitShot(vehicle, shotmgr)
#Mock pathHandler object
self.shot.pathHandler = mock.create_autospec(pathHandler.PathHandler)
self.shot.pathHandler.cruiseSpeed = 6
def testSendToApp(self):
'''Test sending cruiseSpeed of 6 to the app'''
self.shot.updateAppOptions()
expectedPacket = struct.pack('<IIf', app_packet.SOLO_SHOT_OPTIONS, 4, 6)
self.shot.shotmgr.appMgr.sendPacket.assert_called_with(expectedPacket)
class TestsetROIAltitude(unittest.TestCase):
ARBITRARY_HEADING = 34.8
DISTANCE = 97.5
def setUp(self):
#Create a mock vehicle object
vehicle = mock.create_autospec(Vehicle)
#Create a mock shotManager object
shotmgr = mock.create_autospec(ShotManager)
shotmgr.rcMgr = Mock(specs=['remapper'])
#Run the shot constructor
self.shot = orbit.OrbitShot(vehicle, shotmgr)
#Mock pathController
self.shot.pathController = mock.create_autospec(OrbitController)
self.shot.pathController.radius = self.DISTANCE
#set vehicle location
self.shot.vehicle.location.global_relative_frame = LocationGlobalRelative(-4.897465, 111.4894, 10.0)
#set mount status
self.shot.vehicle.mount_status = [-20.0, 40.0, 30.0]
#set ROI
self.shot.roi = location_helpers.newLocationFromAzimuthAndDistance(self.shot.vehicle.location.global_relative_frame, self.ARBITRARY_HEADING, self.DISTANCE)
def testNoGimbalNoChange(self):
""" Without a gimbal, there's no need to adjust the ROI """
self.shot.vehicle.mount_status = [None, None, None]
self.shot.roi.alt = 13.0
self.shot.setROIAltitude(0.5, 0.2)
self.assertEquals( self.shot.ROIAltitudeOffset, 0 )
def testROI0Degrees(self):
""" Passing in 1.0 to setROIAltitude should result in an ROI altitude equal to the vehicle's current altitude """
self.shot.roi.alt = 0.0
self.shot.setROIAltitude(1.0, 0.5)
self.assertEquals( self.shot.ROIAltitudeOffset, self.shot.vehicle.location.global_relative_frame.alt )
def testROI90Degrees(self):
""" Passing in -1.0 to setROIAltitude should result in an ROI altitude that is MAX_ALT_DIFF below our altitude (we basically want to point straight down) """
self.shot.roi.alt = 0.0
self.shot.setROIAltitude(-1.0, -1.0)
self.assertEquals( self.shot.ROIAltitudeOffset, self.shot.vehicle.location.global_relative_frame.alt - orbit.MAX_ALT_DIFF )
def testROI45Degrees(self):
""" Passing in 0.0 to setROIAltitude should result in an ROI altitude that is DISTANCE lower than our altitude (45 degree angle) """
self.shot.roi.alt = -50.0
self.shot.setROIAltitude(0.0, 1.0)
self.assertAlmostEqual(self.shot.ROIAltitudeOffset, self.shot.vehicle.location.global_relative_frame.alt - self.DISTANCE,1)
def testROIArbitraryDegrees(self):
""" Try passing in an arbitrary number (0.213) to setROIAltitude """
self.shot.ticksPaddleCentered = 0
self.shot.roi.alt = -50.0
arbNum = 0.213
self.shot.setROIAltitude(arbNum, 1.0)
# convert arbNum in stick value to degrees
deg = 45.0 - ( arbNum * 45.0 )
altDiff = self.DISTANCE * math.tan(math.radians(deg))
self.assertAlmostEqual(self.shot.ROIAltitudeOffset, self.shot.vehicle.location.global_relative_frame.alt - altDiff,1)
def testROIArbitraryDegrees2(self):
""" Try passing in another number (-0.764) to setROIAltitude """
self.shot.ticksPaddleCentered = 0
self.shot.roi.alt = -50.0
arbNum = -0.764
self.shot.setROIAltitude(arbNum, 0.0)
# convert arbNum in stick value to degrees
deg = arbNum * -45.0 + 45.0
altDiff = self.DISTANCE * math.tan(math.radians(deg))
self.assertAlmostEqual(self.shot.ROIAltitudeOffset, self.shot.vehicle.location.global_relative_frame.alt - altDiff,1)
def testNoUserInput(self):
""" After the user has centered the gimbal paddle for enough time, we should no longer be setting the ROI """
self.shot.roi.alt = -50.0
self.shot.ticksPaddleCentered = orbit.TICKS_TO_IGNORE_PADDLE
self.shot.setROIAltitude(0.5348, 0.0)
self.assertEqual(self.shot.ROIAltitudeOffset, 0)
def testCenterPaddleUpsTicks(self):
""" Centering the paddle should increase ticksPaddleCentered """
self.shot.ticksPaddleCentered = 13
self.shot.setROIAltitude(0.222, 0.0)
self.assertEqual(self.shot.ticksPaddleCentered, 14)
def testMovingPaddleClearsTicks(self):
""" Moving the paddle should reset ticksPaddleCentered """
self.shot.ticksPaddleCentered = 13
self.shot.setROIAltitude(0.222, 0.1)
self.assertEqual(self.shot.ticksPaddleCentered, 0)

389
shotmanager/Test/TestOrbitController.py Normal file
View File

@ -0,0 +1,389 @@
# TestOrbitController.py
# shotmanager
#
# Unit tests for the orbit controller.
#
# Created by Will Silva and Eric Liao on 1/19/2015.
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import orbitController
from orbitController import *
from dronekit import LocationGlobalRelative
import unittest
import mock
from mock import call
from mock import Mock
from mock import MagicMock
from mock import patch
class TestSetOptions(unittest.TestCase):
def setUp(self):
roi = LocationGlobalRelative(37.873168,-122.302062, 0) # lat,lon,alt
self.startRadius = 20 # meters
self.startAzimuth = 15 # deg
self.startAltitude = 10 # meters
self.startLocation = location_helpers.newLocationFromAzimuthAndDistance(roi,self.startAzimuth,self.startRadius)
self.startLocation.alt = self.startAltitude
#Run the controller constructor
self.controller = orbitController.OrbitController(roi, self.startRadius, self.startAzimuth, self.startAltitude+roi.alt)
def testSetBoth(self):
'''Tests setting both options'''
self.controller.setOptions(maxClimbRate = 3, maxAlt = 75)
self.assertEqual(self.controller.maxClimbRate, 3)
self.assertEqual(self.controller.maxAlt, 75)
class TestMove(unittest.TestCase):
def setUp(self):
roi = LocationGlobalRelative(37.873168,-122.302062, 0) # lat,lon,alt
self.startRadius = 20 # meters
self.startAzimuth = 15 # deg
self.startAltitude = 10 # meters
self.startLocation = location_helpers.newLocationFromAzimuthAndDistance(roi,self.startAzimuth,self.startRadius)
self.startLocation.alt = self.startAltitude
#Run the controller constructor
self.controller = orbitController.OrbitController(roi, self.startRadius, self.startAzimuth, self.startAltitude+roi.alt)
self.controller._approach = Mock(return_value = Vector3())
self.controller._strafe = Mock(return_value = Vector3())
self.controller._climb = Mock(return_value = Vector3())
#Set options
self.controller.maxClimbRate = 5 # m/s
self.controller.maxAlt = 100 #m
#Arbirary sticks
throttle = 0.3
roll = -0.4
pitch = 0.5
yaw = -0.6
raw_paddle = 0.7
self.channels = [throttle, roll, pitch, yaw, 0.0, 0.0, 0.0, raw_paddle]
self.controller.move(self.channels, cruiseSpeed = 1, newroi = LocationGlobalRelative(37.873168,-122.302062, 20))
def testCallApproach(self):
'''Tests that _approach() is called'''
self.controller._approach.assert_called_with(-0.5)
def testCallStrafe(self):
'''Tests that _strafe() is called'''
self.controller._strafe.assert_called_with(-0.4, 1)
def testCallClimb(self):
'''Tests that _climb() is called'''
self.controller._climb.assert_called_with(0.3)
def testNewROI(self):
'''Tests that a new roi is set when passed via optional argument'''
self.assertEqual(self.controller.roi.alt, 20)
@mock.patch('location_helpers.newLocationFromAzimuthAndDistance')
def testCallNewLocationFromAzimuthAndDistance(self, location_helpers_newLocationFromAzimuthAndDistance):
'''Tests that location_helpers.newLocationFromAzimuthAndDistance() is called'''
self.controller.move(self.channels, cruiseSpeed = 1)
location_helpers_newLocationFromAzimuthAndDistance.assert_called_with(mock.ANY, mock.ANY, mock.ANY)
class TestApproach(unittest.TestCase):
def setUp(self):
roi = LocationGlobalRelative(37.873168,-122.302062, 0) # lat,lon,alt
self.startRadius = 20 # meters
self.startAzimuth = 15 # deg
self.startAltitude = 10 # meters
self.startLocation = location_helpers.newLocationFromAzimuthAndDistance(roi,self.startAzimuth,self.startRadius)
self.startLocation.alt = self.startAltitude
#Run the controller constructor
self.controller = orbitController.OrbitController(roi, self.startRadius, self.startAzimuth, self.startAltitude+roi.alt)
# set options
self.controller.setOptions(maxClimbRate=1.5 ,maxAlt=45)
def testShrinkRadius(self):
'''Stick/pitch forwards to shrink radius'''
approachVel = self.controller._approach(1.0)
expectedXVel = math.cos(math.radians(self.controller.azimuth+180)) * self.controller.approachSpeed
expectedYVel = math.sin(math.radians(self.controller.azimuth+180)) * self.controller.approachSpeed
self.assertEqual(self.controller.approachSpeed, ACCEL_PER_TICK)
self.assertEqual(self.controller.radius, self.startRadius - ACCEL_PER_TICK*UPDATE_TIME)
self.assertEqual(approachVel, Vector3(expectedXVel, expectedYVel))
def testGrowRadius(self):
'''Stick/pitch backwards to grow radius'''
approachVel = self.controller._approach(-1.0)
expectedXVel = math.cos(math.radians(self.controller.azimuth+180)) * self.controller.approachSpeed
expectedYVel = math.sin(math.radians(self.controller.azimuth+180)) * self.controller.approachSpeed
self.assertEqual(self.controller.approachSpeed, -ACCEL_PER_TICK)
self.assertEqual(self.controller.radius, self.startRadius + ACCEL_PER_TICK*UPDATE_TIME)
self.assertEqual(approachVel, Vector3(expectedXVel, expectedYVel))
def testApproachSpeedDoNotExceed(self):
'''Test that the incremented speed doesn't exceed approachSpeed'''
self.controller.approachSpeed = 3.99
self.controller._approach(0.5) #desired approach speed is 4 but an increment will put it over 4
self.assertEqual(self.controller.approachSpeed,4) #make sure we're limited to 4
def testApproachSpeedDoNotFallBelow(self):
'''Test that the incremented speed doesn't fall below approachSpeed'''
self.controller.approachSpeed = -3.99
self.controller._approach(-0.5) #desired approach speed is 4 but an increment will put it under 4
self.assertEqual(self.controller.approachSpeed,-4) #make sure we're limited to 4
def testEnforceMinRadius(self):
'''Test that our radius can't fall below the minimum radius'''
self.controller.radius = ORBIT_RAD_FOR_MIN_SPEED
self.controller._approach(1.0)
self.assertEqual(self.controller.radius,ORBIT_RAD_FOR_MIN_SPEED)
class TestStrafe(unittest.TestCase):
def setUp(self):
roi = LocationGlobalRelative(37.873168,-122.302062, 0) # lat,lon,alt
self.startRadius = 20 # meters
self.startAzimuth = 15 # deg
self.startAltitude = 10 # meters
self.startLocation = location_helpers.newLocationFromAzimuthAndDistance(roi,self.startAzimuth,self.startRadius)
self.startLocation.alt = self.startAltitude
#Run the controller constructor
self.controller = orbitController.OrbitController(roi, self.startRadius, self.startAzimuth, self.startAltitude+roi.alt)
# set options
self.controller.setOptions(maxClimbRate=1.5 ,maxAlt=45)
def testStrafeRight(self):
'''Stick/roll right to decrease azimuth'''
strafeVel = self.controller._strafe(1.0)
tangent = self.controller.azimuth - math.copysign(90,self.controller.strafeSpeed)
expectedXVel = math.cos(math.radians(tangent)) * abs(self.controller.strafeSpeed)
expectedYVel = math.sin(math.radians(tangent)) * abs(self.controller.strafeSpeed)
degreesDelta = 360. * self.controller.strafeSpeed / (2. * math.pi * self.controller.radius) * UPDATE_TIME
self.assertEqual(self.controller.strafeSpeed, ACCEL_PER_TICK)
self.assertEqual(self.controller.azimuth, self.startAzimuth - degreesDelta)
self.assertEqual(strafeVel, Vector3(expectedXVel, expectedYVel))
def testStrafeLeft(self):
'''Stick/roll left to increase azimuth'''
strafeVel = self.controller._strafe(-1.0)
tangent = self.controller.azimuth - math.copysign(90,self.controller.strafeSpeed)
expectedXVel = math.cos(math.radians(tangent)) * abs(self.controller.strafeSpeed)
expectedYVel = math.sin(math.radians(tangent)) * abs(self.controller.strafeSpeed)
degreesDelta = 360. * self.controller.strafeSpeed / (2. * math.pi * self.controller.radius) * UPDATE_TIME
self.assertEqual(self.controller.strafeSpeed, -ACCEL_PER_TICK)
self.assertEqual(self.controller.azimuth, self.startAzimuth - degreesDelta)
self.assertEqual(strafeVel, Vector3(expectedXVel, expectedYVel))
def testStrafeMaxSpeedUsingStick(self):
'''Test that the dynamic strafe speed limit (function of radius) is enforced with sticks'''
speedLimit = self.controller._maxStrafeSpeed(self.controller.radius)
self.controller.strafeSpeed = speedLimit
strafeVel = self.controller._strafe(1.0)
self.assertEqual(self.controller.strafeSpeed,speedLimit)
def testStrafeMaxSpeedUsingCruise(self):
'''Test that the dynamic strafe speed limit (function of radius) is enforced with cruise'''
speedLimit = self.controller._maxStrafeSpeed(self.controller.radius)
self.controller.strafeSpeed = speedLimit
strafeVel = self.controller._strafe(1.0, strafeSpeed = speedLimit)
self.assertEqual(self.controller.strafeSpeed,speedLimit)
def testCruiseRight(self):
'''Test cruising to the right'''
strafeVel = self.controller._strafe(0.0, strafeSpeed = 5)
tangent = self.controller.azimuth - math.copysign(90,self.controller.strafeSpeed)
expectedXVel = math.cos(math.radians(tangent)) * abs(self.controller.strafeSpeed)
expectedYVel = math.sin(math.radians(tangent)) * abs(self.controller.strafeSpeed)
degreesDelta = 360. * self.controller.strafeSpeed / (2. * math.pi * self.controller.radius) * UPDATE_TIME
self.assertEqual(self.controller.strafeSpeed, ACCEL_PER_TICK)
self.assertEqual(self.controller.azimuth, self.startAzimuth - degreesDelta)
self.assertEqual(strafeVel, Vector3(expectedXVel, expectedYVel))
def testCruiseLeft(self):
'''Test cruising to the left'''
strafeVel = self.controller._strafe(0.0, strafeSpeed = -5)
tangent = self.controller.azimuth - math.copysign(90,self.controller.strafeSpeed)
expectedXVel = math.cos(math.radians(tangent)) * abs(self.controller.strafeSpeed)
expectedYVel = math.sin(math.radians(tangent)) * abs(self.controller.strafeSpeed)
degreesDelta = 360. * self.controller.strafeSpeed / (2. * math.pi * self.controller.radius) * UPDATE_TIME
self.assertEqual(self.controller.strafeSpeed, -ACCEL_PER_TICK)
self.assertEqual(self.controller.azimuth, self.startAzimuth - degreesDelta)
self.assertEqual(strafeVel, Vector3(expectedXVel, expectedYVel))
def testCruiseSpeedUpRight(self):
'''Test that we can speed up cruiseSpeed using stick'''
self.controller.strafeSpeed = 4
strafeVel = self.controller._strafe(0.5, strafeSpeed = 4)
tangent = self.controller.azimuth - math.copysign(90,self.controller.strafeSpeed)
expectedXVel = math.cos(math.radians(tangent)) * abs(self.controller.strafeSpeed)
expectedYVel = math.sin(math.radians(tangent)) * abs(self.controller.strafeSpeed)
degreesDelta = 360. * self.controller.strafeSpeed / (2. * math.pi * self.controller.radius) * UPDATE_TIME
self.assertEqual(self.controller.strafeSpeed, 4 + ACCEL_PER_TICK)
self.assertEqual(self.controller.azimuth, self.startAzimuth - degreesDelta)
self.assertEqual(strafeVel, Vector3(expectedXVel, expectedYVel))
def testCruiseSlowDownRight(self):
'''Test that we can slow down cruiseSpeed using stick left'''
self.controller.strafeSpeed = 4
strafeVel = self.controller._strafe(-0.5, strafeSpeed = 4)
tangent = self.controller.azimuth - math.copysign(90,self.controller.strafeSpeed)
expectedXVel = math.cos(math.radians(tangent)) * abs(self.controller.strafeSpeed)
expectedYVel = math.sin(math.radians(tangent)) * abs(self.controller.strafeSpeed)
degreesDelta = 360. * self.controller.strafeSpeed / (2. * math.pi * self.controller.radius) * UPDATE_TIME
self.assertEqual(self.controller.strafeSpeed, 4 - ACCEL_PER_TICK)
self.assertEqual(self.controller.azimuth, self.startAzimuth - degreesDelta)
self.assertEqual(strafeVel, Vector3(expectedXVel, expectedYVel))
def testCruiseSpeedUpLeft(self):
'''Test that we can speed up cruiseSpeed using stick left'''
self.controller.strafeSpeed = -4
strafeVel = self.controller._strafe(-0.5, strafeSpeed = -4)
tangent = self.controller.azimuth - math.copysign(90,self.controller.strafeSpeed)
expectedXVel = math.cos(math.radians(tangent)) * abs(self.controller.strafeSpeed)
expectedYVel = math.sin(math.radians(tangent)) * abs(self.controller.strafeSpeed)
degreesDelta = 360. * self.controller.strafeSpeed / (2. * math.pi * self.controller.radius) * UPDATE_TIME
self.assertEqual(self.controller.strafeSpeed, -4 - ACCEL_PER_TICK)
self.assertEqual(self.controller.azimuth, self.startAzimuth - degreesDelta)
self.assertEqual(strafeVel, Vector3(expectedXVel, expectedYVel))
def testCruiseSlowDownLeft(self):
'''Test that we can slow down cruiseSpeed using stick right'''
self.controller.strafeSpeed = -4
strafeVel = self.controller._strafe(0.5, strafeSpeed = -4)
tangent = self.controller.azimuth - math.copysign(90,self.controller.strafeSpeed)
expectedXVel = math.cos(math.radians(tangent)) * abs(self.controller.strafeSpeed)
expectedYVel = math.sin(math.radians(tangent)) * abs(self.controller.strafeSpeed)
degreesDelta = 360. * self.controller.strafeSpeed / (2. * math.pi * self.controller.radius) * UPDATE_TIME
self.assertEqual(self.controller.strafeSpeed, -4 + ACCEL_PER_TICK)
self.assertEqual(self.controller.azimuth, self.startAzimuth - degreesDelta)
self.assertEqual(strafeVel, Vector3(expectedXVel, expectedYVel))
class TestClimb(unittest.TestCase):
def setUp(self):
roi = LocationGlobalRelative(37.873168,-122.302062, 0) # lat,lon,alt
self.startRadius = 20 # meters
self.startAzimuth = 15 # deg
self.startAltitude = 10 # meters
self.startLocation = location_helpers.newLocationFromAzimuthAndDistance(roi,self.startAzimuth,self.startRadius)
self.startLocation.alt = self.startAltitude
#Run the controller constructor
self.controller = orbitController.OrbitController(roi, self.startRadius, self.startAzimuth, self.startAltitude+roi.alt)
# set options
self.controller.setOptions(maxClimbRate=1.5 ,maxAlt=45)
def testClimbUp(self):
'''Test climbing up with stick up'''
climbVel = self.controller._climb(1.0)
self.assertEqual(self.controller.zOffset, self.startAltitude + self.controller.maxClimbRate*UPDATE_TIME)
def testClimbDown(self):
'''Test descending with stick down'''
climbVel = self.controller._climb(-1.0)
self.assertEqual(self.controller.zOffset, self.startAltitude - self.controller.maxClimbRate*UPDATE_TIME)
class TestMaxStrafeSpeed(unittest.TestCase):
def setUp(self):
roi = LocationGlobalRelative(37.873168,-122.302062, 0) # lat,lon,alt
self.startRadius = 20 # meters
self.startAzimuth = 15 # deg
self.startAltitude = 10 # meters
self.startLocation = location_helpers.newLocationFromAzimuthAndDistance(roi,self.startAzimuth,self.startRadius)
self.startLocation.alt = self.startAltitude
#Run the controller constructor
self.controller = orbitController.OrbitController(roi, self.startRadius, self.startAzimuth, self.startAltitude)
# set options
self.controller.setOptions(maxClimbRate=1.5 ,maxAlt=45)
def testBigRadius(self):
'''Test that maxSpeed is limited to ORBIT_MAX_SPEED at a large radius'''
self.controller.radius = ORBIT_RAD_FOR_MAX_SPEED + 1
maxSpeed = self.controller._maxStrafeSpeed(self.controller.radius)
self.assertEqual(maxSpeed, ORBIT_MAX_SPEED)
def testSmallRadius(self):
'''Test that maxSpeed is limited to ORBIT_MIN_SPEED at a small radius'''
self.controller.radius = ORBIT_RAD_FOR_MIN_SPEED - 1
maxSpeed = self.controller._maxStrafeSpeed(self.controller.radius)
self.assertEqual(maxSpeed, ORBIT_MIN_SPEED)
def testIntermediateRadius(self):
'''Test that an intermediate maxSpeed is adhered at an intermediate radius'''
self.controller.radius = (ORBIT_RAD_FOR_MAX_SPEED - ORBIT_RAD_FOR_MIN_SPEED) / 2.
expectedMaxSpeed = 5.14285714286 # pre-calculated
maxSpeed = self.controller._maxStrafeSpeed(self.controller.radius)
self.assertAlmostEqual(maxSpeed, expectedMaxSpeed)

213
shotmanager/Test/TestPano.py Normal file
View File

@ -0,0 +1,213 @@
# TestOrbit.py
# shotmanager
#
# Unit tests for the orbit smart shot.
#
# Created by Will Silva and Eric Liao on 1/19/2015.
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import pano
from pano import *
import shotManager
from shotManager import ShotManager
import unittest
import mock
from mock import call
from mock import Mock
from mock import MagicMock
from mock import patch
class TestShotInit(unittest.TestCase):
def setUp(self):
'''Create a mock vehicle object'''
vehicle = mock.create_autospec(Vehicle)
vehicle.attitude.yaw = math.radians(0)
'''Create a mock shotManager object'''
shotmgr = mock.create_autospec(ShotManager)
shotmgr.appMgr = Mock()
shotmgr.buttonManager = Mock()
shotmgr.goproManager = Mock()
shotmgr.rcMgr = Mock(specs=['remapper'])
shotmgr.getParam.return_value = 0 # so mock doesn't do lazy binds
'''Run the shot constructor'''
self.shot = pano.PanoShot(vehicle, shotmgr)
def testInit(self):
'''Test that the shot initialized properly'''
# vehicle object should be created (not None)
assert self.shot.vehicle is not None
# shotManager object should be created (not None)
assert self.shot.shotmgr is not None
# filtered roi should be None
self.assertEqual(self.shot.state, PANO_SETUP)
self.assertEqual(self.shot.panoType, PANO_CYLINDER)
self.assertEqual(self.shot.degSecondYaw, PANO_DEFAULT_VIDEO_YAW_RATE)
self.assertEqual(self.shot.cylinder_fov, PANO_DEFAULT_FOV)
# enter Guided
self.assertEqual(self.shot.vehicle.mode.name, "GUIDED")
def testRCSetupState(self):
self.shot.state = PANO_SETUP
#Neutral sticks
throttle = 0.0
roll = 0.0
pitch = 0.0
yaw = 0.0
self.channels = [throttle, roll, pitch, yaw, 0.0, 0.0, 0.0, 0.0]
self.shot.handleRCs(self.channels)
def testCylinder(self):
self.shot.state = PANO_SETUP
#Neutral sticks
throttle = 0.0
roll = 0.0
pitch = 0.0
yaw = 0.0
self.channels = [throttle, roll, pitch, yaw, 0.0, 0.0, 0.0, 0.0]
self.shot.initCylinder()
#print "angles %s" % str(self.shot.cylinderAngles).strip('[]')
self.assertEqual(self.shot.cylinderAngles[0], 270)
self.assertEqual(self.shot.cylinderAngles[1], 306)
self.assertEqual(self.shot.cylinderAngles[2], 342)
self.assertEqual(self.shot.cylinderAngles[3], 18)
self.assertEqual(self.shot.cylinderAngles[4], 54)
# check we are incrementing ticks correctly
self.assertEqual(self.shot.ticks, -25)
self.shot.runCylinder()
self.assertEqual(self.shot.ticks, -24)
# handle RCs runs
self.shot.state = PANO_RUN
self.shot.handleRCs(self.channels)
self.assertEqual(self.shot.ticks, -23)
self.assertEqual(self.shot.camYaw, 90)
#print "self.shot.camYaw %d", self.shot.camYaw
# Force us to grab next Yaw and verify
self.shot.ticks = PANO_MOVE_DELAY
self.shot.runCylinder()
self.assertEqual(self.shot.camYaw, 54)
# Force us to grab next Yaw and verify
self.shot.ticks = PANO_MOVE_DELAY
self.shot.runCylinder()
self.assertEqual(self.shot.camYaw, 18)
# Force us to grab next Yaw and verify
self.shot.ticks = PANO_MOVE_DELAY
self.shot.runCylinder()
self.assertEqual(self.shot.camYaw, 342)
# Force us to grab next Yaw and verify
self.shot.ticks = PANO_MOVE_DELAY
self.shot.runCylinder()
self.assertEqual(self.shot.camYaw, 306)
# Force us to grab next Yaw and verify
self.shot.ticks = PANO_MOVE_DELAY
self.shot.runCylinder()
self.assertEqual(self.shot.camYaw, 270)
# Force us to exit shot
self.assertEqual(self.shot.state, PANO_RUN)
self.shot.ticks = PANO_MOVE_DELAY
self.shot.runCylinder()
self.assertEqual(self.shot.state, PANO_EXIT)
def testSphere(self):
self.shot.state = PANO_SETUP
self.shot.panoType = PANO_SPHERE
#Neutral sticks
throttle = 0.0
roll = 0.0
pitch = 0.0
yaw = 0.0
self.channels = [throttle, roll, pitch, yaw, 0.0, 0.0, 0.0, 0.0]
self.shot.initSphere()
#print "angles %s" % str(self.shot.sphereAngles).strip('[]')
self.assertEqual(self.shot.sphereAngles[0][0], -90)
self.assertEqual(self.shot.sphereAngles[0][1], 0)
self.assertEqual(self.shot.sphereAngles[9][0], 0)
self.assertEqual(self.shot.sphereAngles[9][1], 60)
length = len(self.shot.sphereAngles)
# make sure camera is reset
self.assertEqual(length, 10)
self.assertEqual(self.shot.camYaw, 0)
self.assertEqual(self.shot.camPitch, 0)
for x in range(0, length):
#print "howdy %d" %x
tmp = self.shot.sphereAngles.pop()
# Force us to exit shot
self.shot.state = PANO_RUN
self.shot.ticks = PANO_MOVE_DELAY
self.shot.runSphere()
self.assertEqual(self.shot.state, PANO_EXIT)
def testVideo(self):
self.shot.panoType = PANO_VIDEO
# sticks
throttle = 0.0
roll = 0.0
pitch = 0.0
yaw = 1.0
self.channels = [throttle, roll, pitch, yaw, 0.0, 0.0, 0.0, 0.0]
self.shot.camYaw = 0
for x in range(0, 25):
self.shot.runVideo(self.channels)
self.assertGreaterEqual(self.shot.camYaw, 9.7)
def testPitch(self):
# sticks
throttle = -1.0
roll = 0.0
pitch = 0.0
yaw = 0.0
self.channels = [throttle, roll, pitch, yaw, 0.0, 0.0, 0.0, 0.0]
self.shot.camPitch = 0
for x in range(0, 40):
self.shot.manualPitch(self.channels)
self.assertEqual(self.shot.camPitch, -90)
throttle = 1.0
self.channels = [throttle, roll, pitch, yaw, 0.0, 0.0, 0.0, 0.0]
for x in range(0, 40):
self.shot.manualPitch(self.channels)
self.assertEqual(self.shot.camPitch, 0)

309
shotmanager/Test/TestPathHandler.py Normal file
View File

@ -0,0 +1,309 @@
# Unit tests for ROI
import mock
from mock import Mock
import os
from os import sys, path
from pymavlink import mavutil
from shotManagerConstants import *
import unittest
import random
import math
from dronekit import Vehicle, LocationGlobalRelative
sys.path.append(os.path.realpath('..'))
import pathHandler
from shotManager import ShotManager
import shots
#Random number generator seed
SEED = 94739473
#Number of tests to run
REPEAT = 10
class TestMoveTowardsEndpt(unittest.TestCase):
def setUp(self):
mgr = mock.create_autospec(ShotManager)
mgr.currentShot = shots.APP_SHOT_CABLECAM
self.loc1 = LocationGlobalRelative(45.68464156, 58.68464, 5.54684)
self.loc2 = LocationGlobalRelative(45.684641555, 58.684655, 5.54684)
v = mock.create_autospec(Vehicle)
v.message_factory = Mock()
v.commands = Mock()
self.handler = pathHandler.TwoPointPathHandler( self.loc1, self.loc2, v, mgr )
random.seed(SEED)
def testTogglePauseShouldPause(self):
'''If cruiseSpeed != 0 then pause() should be executed'''
self.handler.pause = Mock()
self.handler.cruiseSpeed = 4.0
self.handler.togglePause()
self.handler.pause.assert_called_with()
def testTogglePauseShouldResume(self):
'''If cruiseSpeed == 0 then resume() should be executed'''
self.handler.resume = Mock()
self.handler.cruiseSpeed = 0.0
self.handler.togglePause()
self.handler.resume.assert_called_with()
def testNoCruiseForwardsFullStick(self):
""" Command should reach (and not exceed) MAX_SPEED [Flying in forwards (+) direction, full stick in positive direction] """
for test in range(REPEAT): #test REPEAT times with random values based on SEED
self.handler.cruiseSpeed = 0.0 #No cruise
initialSpeed = random.uniform(0.0,MAX_SPEED) #Start at a random positive speed within acceptable limits
self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
channels = [1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0] #Full positive stick
#calculate number of ticks required to reach MAX_SPEED
deltaSpeed = MAX_SPEED-self.handler.currentSpeed
ticksRequired = int(math.ceil(deltaSpeed/pathHandler.ACCEL_PER_TICK))
for ticks in range(1,ticksRequired+1):
#run controller tick
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
#assert but handle floating point errors (limit speed to MAX_SPEED)
self.assertAlmostEqual(self.handler.currentSpeed, min(initialSpeed + ticks*pathHandler.ACCEL_PER_TICK,MAX_SPEED), places=7, msg=None, delta=None)
#make sure the command is called
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, abs(self.handler.currentSpeed), -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
#make sure we are targeted to the right loc
self.handler.vehicle.simple_goto.assert_called_with( self.loc2 )
self.assertGreaterEqual(returnSpeed, 0.0)
def testNoCruiseBackwardsFullNegStick(self):
""" Command should reach (and not exceed) -MAX_SPEED [Flying in backwards (-) direction, full stick in negative direction] """
for test in range(REPEAT):
self.handler.cruiseSpeed = 0.0 #No cruise
initialSpeed = random.uniform(-MAX_SPEED,0.0) #Start at a random speed within acceptable limits
self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
channels = [1.0, -1.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0] #Full negative stick
#calculate number of ticks required to reach -MAX_SPEED
deltaSpeed = -MAX_SPEED-self.handler.currentSpeed
ticksRequired = int(math.ceil(abs(deltaSpeed/pathHandler.ACCEL_PER_TICK)))
for ticks in range(1,ticksRequired+1):
#run controller tick
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
#assert but handle floating point errors (limit speed to -MAX_SPEED)
self.assertAlmostEqual(self.handler.currentSpeed, max(initialSpeed - ticks*pathHandler.ACCEL_PER_TICK,-MAX_SPEED), places=7, msg=None, delta=None)
#make sure the command is called
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, abs(self.handler.currentSpeed), -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
#make sure we are targeted to the right loc
self.handler.vehicle.simple_goto.assert_called_with( self.loc1 )
self.assertGreaterEqual(returnSpeed, 0.0)
def testNoCruiseSwitchDirections(self):
''' Command should decelerate to zero and accelerate in new direction [Flying in forwards (+) direction, full stick in negative direction] '''
for test in range(REPEAT):
self.handler.cruiseSpeed = 0.0 #No cruise
initialSpeed = random.uniform(0.0, MAX_SPEED) #Start at a random speed within acceptable limits
self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
channels = [1.0, -1.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0] #Full negative stick
#calculate number of decel ticks required to reach 0.0
deltaSpeed = 0.0-self.handler.currentSpeed
ticksRequired = int(math.ceil(abs(deltaSpeed/pathHandler.ACCEL_PER_TICK)))
for ticks in range(1,ticksRequired+1):
#run controller tick
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
#assert but handle floating point errors (limit speed to -MAX_SPEED)
self.assertAlmostEqual(self.handler.currentSpeed, initialSpeed - ticks*pathHandler.ACCEL_PER_TICK, places=7, msg=None, delta=None)
#make sure the command is called
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, abs(self.handler.currentSpeed), -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
#should be heading in reverse now
self.handler.vehicle.simple_goto.assert_called_with( self.loc1 )
#resync initial speed to current
initialSpeed = self.handler.currentSpeed
#redo math to reach full neg speed
deltaSpeed = -MAX_SPEED-self.handler.currentSpeed
ticksRequired = int(math.ceil(abs(deltaSpeed/pathHandler.ACCEL_PER_TICK)))
for ticks in range(1,ticksRequired+1):
#run controller tick
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
#assert but handle floating point errors (limit speed to -MAX_SPEED)
self.assertAlmostEqual(self.handler.currentSpeed, max(initialSpeed - ticks*pathHandler.ACCEL_PER_TICK,-MAX_SPEED), places=7, msg=None, delta=None)
#make sure the command is called
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, abs(self.handler.currentSpeed), -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
self.assertEqual(-MAX_SPEED,self.handler.currentSpeed)
self.assertGreaterEqual(returnSpeed, 0.0)
def testNoCruisePitchBigger(self):
""" Pitch beats roll """
channels = [1.0, 0.4, -0.8, 1.0, 1.0, 1.0, 1.0, 1.0]
self.handler.cruiseSpeed = 0.0 #No cruise
self.handler.currentSpeed = 0.0 #Start at a standstill
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, pathHandler.ACCEL_PER_TICK, -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
self.handler.vehicle.simple_goto.assert_called_with( self.loc2 )
self.assertGreaterEqual(returnSpeed, 0.0)
def testNoCruiseRollBigger(self):
""" Roll beats pitch """
channels = [1.0, -0.72, 0.33, 1.0, 1.0, 1.0, 1.0, 1.0]
self.handler.cruiseSpeed = 0.0 #No cruise
self.handler.currentSpeed = 0.0 #Start at a standstill
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, pathHandler.ACCEL_PER_TICK, -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
self.handler.vehicle.simple_goto.assert_called_with( self.loc1 )
self.assertGreaterEqual(returnSpeed, 0.0)
def testNoCruisePositiveAccelerate(self):
''' Vehicle flying in positive direction, increase stick in positive direction - should accelerate to steady state speed'''
for test in range(REPEAT):
initialSpeed = random.uniform(0.0,MAX_SPEED) #Start at a random speed within acceptable limits
self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
stickSpeed = random.uniform(initialSpeed,MAX_SPEED) #set a random stick speed above the randomly chosen initial speed
stickVal = stickSpeed/MAX_SPEED #calculate corresponding stick value
channels = [1.0, stickVal, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0]
#calculate number of accel ticks required to reach stickSpeed
deltaSpeed = stickSpeed-self.handler.currentSpeed
ticksRequired = int(math.ceil(abs(deltaSpeed/pathHandler.ACCEL_PER_TICK)))
for ticks in range(1,ticksRequired+1):
#run controller tick
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
#assert but handle floating point errors (limit to stickSpeed)
self.assertAlmostEqual(self.handler.currentSpeed, min(initialSpeed + ticks*pathHandler.ACCEL_PER_TICK,stickSpeed), places=7, msg=None, delta=None)
#make sure the command is called
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, abs(self.handler.currentSpeed), -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
self.assertEqual(stickSpeed,self.handler.currentSpeed)
self.handler.vehicle.simple_goto.assert_called_with( self.loc2 )
self.assertGreaterEqual(returnSpeed, 0.0)
def testNoCruisePositiveDecelerate(self):
''' Vehicle flying in positive direction, decrease stick in positive direction - should decelerate '''
for test in range(REPEAT):
initialSpeed = random.uniform(0.0,MAX_SPEED) #Start at a random speed within acceptable limits
self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
stickSpeed = random.uniform(0.0,initialSpeed) #set a random stick speed above the randomly chosen initial speed
stickVal = stickSpeed/MAX_SPEED #calculate corresponding stick value
channels = [1.0, stickVal, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0]
#calculate number of accel ticks required to reach stickSpeed
deltaSpeed = stickSpeed-self.handler.currentSpeed
ticksRequired = int(math.ceil(abs(deltaSpeed/pathHandler.ACCEL_PER_TICK)))
for ticks in range(1,ticksRequired+1):
#run controller tick
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
#assert but handle floating point errors (limit to stickSpeed)
self.assertAlmostEqual(self.handler.currentSpeed, max(initialSpeed - ticks*pathHandler.ACCEL_PER_TICK,stickSpeed), places=7, msg=None, delta=None)
#make sure the command is called
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, abs(self.handler.currentSpeed), -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
self.assertEqual(stickSpeed,self.handler.currentSpeed)
self.handler.vehicle.simple_goto.assert_called_with( self.loc2 )
self.assertGreaterEqual(returnSpeed, 0.0)
def testNoCruiseNegativeAccelerate(self):
''' Vehicle flying in negative direction, increase stick in negative direction - should accelerate '''
for test in range(REPEAT):
initialSpeed = random.uniform(-MAX_SPEED,0.0) #Start at a random speed within acceptable limits
self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
stickSpeed = random.uniform(-MAX_SPEED,initialSpeed) #set a random stick speed above the randomly chosen initial speed
stickVal = stickSpeed/MAX_SPEED #calculate corresponding stick value
channels = [1.0, stickVal, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0]
#calculate number of accel ticks required to reach stickSpeed
deltaSpeed = stickSpeed-self.handler.currentSpeed
ticksRequired = int(math.ceil(abs(deltaSpeed/pathHandler.ACCEL_PER_TICK)))
for ticks in range(1,ticksRequired+1):
#run controller tick
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
#assert but handle floating point errors (limit to stickSpeed)
self.assertAlmostEqual(self.handler.currentSpeed, max(initialSpeed - ticks*pathHandler.ACCEL_PER_TICK,stickSpeed), places=7, msg=None, delta=None)
#make sure the command is called
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, abs(self.handler.currentSpeed), -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
self.assertEqual(stickSpeed,self.handler.currentSpeed)
self.handler.vehicle.simple_goto.assert_called_with( self.loc1 )
self.assertGreaterEqual(returnSpeed, 0.0)
def testNoCruiseNegativeDecelerate(self):
''' Vehicle flying in negative direction, decrease stick in negative direction - should decelerate '''
for test in range(REPEAT):
initialSpeed = random.uniform(-MAX_SPEED,0.0) #Start at a random speed within acceptable limits
self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
stickSpeed = random.uniform(initialSpeed,0.0) #set a random stick speed above the randomly chosen initial speed
stickVal = stickSpeed/MAX_SPEED #calculate corresponding stick value
channels = [1.0, stickVal, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0]
#calculate number of accel ticks required to reach stickSpeed
deltaSpeed = stickSpeed-self.handler.currentSpeed
ticksRequired = int(math.ceil(abs(deltaSpeed/pathHandler.ACCEL_PER_TICK)))
for ticks in range(1,ticksRequired+1):
#run controller tick
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
#assert but handle floating point errors (limit to stickSpeed)
self.assertAlmostEqual(self.handler.currentSpeed, min(initialSpeed + ticks*pathHandler.ACCEL_PER_TICK,stickSpeed), places=7, msg=None, delta=None)
#make sure the command is called
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, abs(self.handler.currentSpeed), -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
self.assertEqual(stickSpeed,self.handler.currentSpeed)
self.handler.vehicle.simple_goto.assert_called_with( self.loc1 )
self.assertGreaterEqual(returnSpeed, 0.0)
###CRUISE TESTS###
def testCruiseSameDir(self):
""" Cruising with full same stick towards loc 2- should accelerate """
self.handler.cruiseSpeed = 4.0 #random positive cruise speed
self.handler.currentSpeed = self.handler.cruiseSpeed #start out at cruise
channels = [1.0, 0.5, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0] #Stick forward
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
#make sure the command is called
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, self.handler.cruiseSpeed + pathHandler.ACCEL_PER_TICK, -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
self.handler.vehicle.simple_goto.assert_called_with( self.loc2 )
self.assertGreaterEqual(returnSpeed, 0.0)
def testCruiseOppositeDir(self):
""" Cruising towards loc 2 with opposite stick - should decelerate to a steady state speed"""
self.handler.cruiseSpeed = 4.0 #random positive cruise speed
self.handler.currentSpeed = self.handler.cruiseSpeed #start out at cruise
channels = [1.0, -0.5, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0] #Stick backward
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
#make sure the command is called
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, self.handler.cruiseSpeed - pathHandler.ACCEL_PER_TICK, -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
self.handler.vehicle.simple_goto.assert_called_with( self.loc2 )
self.assertGreaterEqual(returnSpeed, 0.0)

185
shotmanager/Test/TestRCManager.py Normal file
View File

@ -0,0 +1,185 @@
import shotManager
import unittest
import mock
from mock import patch, Mock
import socket
from dronekit import Vehicle
import struct
import rcManager
class TestParse(unittest.TestCase):
@patch.object(socket.socket, 'bind')
def setUp(self, mock_bind):
self.mgr = shotManager.ShotManager()
v = mock.create_autospec(Vehicle)
self.mgr.Start(v)
self.sock = Mock()
self.channels = [1500, 5, 9, 8, 7, 1, 1, 1]
self.value = struct.pack('<dHHHHHHHHH', 2.3, 3, 1500, 5, 9, 8, 7, 1, 1, 1)
self.mgr.rcMgr.server.recv = Mock(return_value = self.value)
def testCacheSocketData(self):
""" incoming RC data should be written to self.channels """
# should feed return value of sock.recv into remap
self.mgr.rcMgr.parse()
self.assertEqual( self.mgr.rcMgr.channels, self.channels )
def testResetTicks(self):
""" handleRCSock should reset numTicksSinceRCUpdate """
self.mgr.numTicksSinceRCUpdate = 17
self.mgr.rcMgr.parse()
self.assertEqual( self.mgr.rcMgr.numTicksSinceRCUpdate, 0 )
def testDontCacheData(self):
""" if incoming data is malformed, do not cache it """
self.mgr.channels = 111
self.sock.recv = Mock(return_value='333')
self.mgr.rcMgr.parse()
self.assertNotEqual( self.mgr.rcMgr.channels, '333' )
class TestNormalizeRC(unittest.TestCase):
def setUp(self):
shotmgr = Mock()
self.rcManager = rcManager.rcManager(shotmgr)
sock = Mock()
def testOutOfBoundsHighDefaultMinMax(self):
""" PWM values above max should return 0.0 """
result = self.rcManager.normalizeRC(2050, rcManager.DEFAULT_RC_MIN, rcManager.DEFAULT_RC_MAX)
self.assertEqual( result, 0.0 )
def testOutOfBoundsLowDefaultMinMax(self):
""" PWM values below min should return 0.0 """
result = self.rcManager.normalizeRC(957, rcManager.DEFAULT_RC_MIN, rcManager.DEFAULT_RC_MAX)
self.assertEqual( result, 0.0 )
def test1DefaultMinMax(self):
""" pwm of 2000 should result in 1.0 """
result = self.rcManager.normalizeRC(2000, rcManager.DEFAULT_RC_MIN, rcManager.DEFAULT_RC_MAX)
self.assertEqual( result, 1.0 )
def testneg1DefaultMinMax(self):
""" pwm of 1000 should result in -1.0 """
result = self.rcManager.normalizeRC(1000, rcManager.DEFAULT_RC_MIN, rcManager.DEFAULT_RC_MAX)
self.assertEqual( result, -1.0 )
def testMidDefaultMinMax(self):
""" pwm of 1500 should result in 0.0 """
result = self.rcManager.normalizeRC(1500, rcManager.DEFAULT_RC_MIN, rcManager.DEFAULT_RC_MAX)
self.assertEqual( result, 0.0 )
def testOutOfBoundsHighCustomMinMax(self):
""" Values above a custom max should return 0.0 """
result = self.rcManager.normalizeRC(1521, 1000, 1520)
self.assertEqual( result, 0.0 )
def testOutOfBoundsLowCustomMinMax(self):
""" Values below a custom min should return 0.0 """
result = self.rcManager.normalizeRC(-1, 0, 1000)
self.assertEqual( result, 0.0 )
def test1CustomMinMax(self):
""" Value matching max should result in 1.0 """
result = self.rcManager.normalizeRC(1333, 650, 1333)
self.assertEqual( result, 1.0 )
def testneg1CustomMinMax(self):
""" Value matching min should result in -1.0 """
result = self.rcManager.normalizeRC(333, 333, 666)
self.assertEqual( result, -1.0 )
def testMidCustomMinMax(self):
""" Mid value should result in 0.0 """
result = self.rcManager.normalizeRC(1260, 1000, 1520)
self.assertEqual( result, 0.0 )
class TestRemap(unittest.TestCase):
def setUp(self):
shotmgr = Mock()
self.rcManager = rcManager.rcManager(shotmgr)
sock = Mock()
def testStickNormalization(self):
""" Remap should call normalize for each channel """
self.rcManager.channels = [1600, 1200, 1500, 1300, 2000, 1000, 1237, 1889]
normChannels = self.rcManager.remap()
self.assertEqual( normChannels[0], .2 )
self.assertEqual( normChannels[1], -0.6 )
self.assertEqual( normChannels[2], 0.0)
self.assertEqual( normChannels[3], -0.4 )
self.assertEqual( normChannels[5], -1.0 )
def testChannel6Max(self):
""" Channel 6 (index 5) is a special case and goes from 1000-1520. Input of 1520 should return 1.0"""
self.rcManager.channels = [0, 0, 0, 0, 0, 1520, 0, 0]
normChannels = self.rcManager.remap()
self.assertEqual( normChannels[5], 1.0 )
def testChannel6Mid(self):
""" Channel 6 (index 5) is a special case and goes from 1000-1520. Input of 1260 should return 0.0"""
self.rcManager.channels = [0, 0, 0, 0, 0, 1260, 0, 0]
normChannels = self.rcManager.remap()
self.assertEqual( normChannels[5], 0.0 )
def testChannel8(self):
""" Channel 8 (index 7) is a special case and goes from 0-1000 """
self.rcManager.channels = [0, 0, 0, 0, 0, 0, 0, 777]
normChannels = self.rcManager.remap()
result = (777 - 500) / 500.0
self.assertEqual( normChannels[7], result )
def testNoRemappingSticks(self):
""" If self.remappingSticks is off, we should not forward sticks to pixRC """
self.rcManager.channels = [1100, 1500, 1500, 1500, 0, 0, 0, 777]
self.rcManager.remappingSticks = False
with patch('sololink.rc_ipc.put') as mock_put:
self.rcManager.remap()
self.assertFalse( mock_put.called )
def testRemappingSticks(self):
""" If self.remappingSticks is on, we should forward sticks to pixRC, if passing in True for sendToPixRC """
self.rcManager.channels = [1100, 0, 0, 1500,0, 0, 0, 777]
self.rcManager.remappingSticks = True
self.rcManager.timestamp = 140
self.rcManager.sequence = 0
with patch('sololink.rc_ipc.put') as mock_put:
self.rcManager.remap()
mock_put.assert_called_with((140, 0, [1100, 0, 0, 1500, 0, 0, 0, 777]))
def testRemappingSticksFalse(self):
""" If self.remappingSticks is off, we should enter into Failsafe, enable remapping and send RC to sendToPixRC """
self.rcManager.channels = [1100, 1500, 1500, 1500, 0, 0, 0, 777]
self.rcManager.remappingSticks = False
with patch('sololink.rc_ipc.put') as mock_put:
self.rcManager.remap()
self.assertFalse( mock_put.called )
def testRemap(self):
defaultChannels = [0.0, 0.0, 0.0, 0.0, 0, 0.0, 0, 0.0]
self.rcManager.channels = [rcManager.DEFAULT_RC_MID,
rcManager.DEFAULT_RC_MID,
rcManager.DEFAULT_RC_MID,
rcManager.DEFAULT_RC_MID,
rcManager.DEFAULT_RC_MIN,
rcManager.CHANNEL6_MID,
rcManager.DEFAULT_RC_MID,
rcManager.CHANNEL8_MID]
retVal = self.rcManager.remap()
self.assertEqual(retVal,defaultChannels)
class TestRemapperInit(unittest.TestCase):
def testInit(self):
""" Test initialization of RC remapper """
shotmgr = Mock()
self.rcManager = rcManager.rcManager(shotmgr)
sock = Mock()
self.assertFalse(self.rcManager.remappingSticks)

95
shotmanager/Test/TestRewind.py Normal file
View File

@ -0,0 +1,95 @@
# Unit tests for ZiplineShot
import math
import mock
from mock import call
from mock import Mock
from mock import patch
import os
from os import sys, path
from pymavlink import mavutil
import struct
import unittest
from dronekit import LocationGlobalRelative, Vehicle
sys.path.append(os.path.realpath('..'))
import location_helpers
import rewind
from rewind import RewindShot
import rewindManager
from rewindManager import RewindManager
from shotManagerConstants import *
import shots
# on host systems these files are located here
from sololink import btn_msg
import app_packet
class TestRewind(unittest.TestCase):
def setUp(self):
mgr = Mock(spec = ["sendPacket", "remapper", "rcMgr", "appMgr", "rewindManager"])
mgr.currentShot = shots.APP_SHOT_ZIPLINE
mgr.buttonManager = Mock()
self.mock_vehicle = mock.create_autospec(Vehicle)
#self.controller = RewindShot(self.mock_vehicle, mgr)
#self.controller.rewindManager = RewindManager(self.mock_vehicle)
#test = RewindManager(self.mock_vehicle)
#self.mock_vehicle.location.global_relative_frame = LocationGlobalRelative(37.0, -122.0, 15.0)
def testInit(self):
''' verify init '''
#self.assertTrue( self.controller.rewindManager )
# def testHandleRCs(self):
# self.rewind = RewindManager(self.mock_vehicle)
#
# # store first location
# self.mock_vehicle.location.global_relative_frame = LocationGlobalRelative(37.0, -122.0, 15.0)
#
# # We only record locations while in air
# self.mock_vehicle.armed = True
# self.mock_vehicle.system_status = 'ACTIVE'
#
# self.controller.rewindManager = self.rewind
# # Pushes vehicle loc to buffer index 0
# self.controller.rewindManager.reset()
#
# self.mock_vehicle.location.global_relative_frame = LocationGlobalRelative(37.00001, -122.00002, 10.0)
# # force past load limiter
# self.rewind.counter = rewindManager.LOOP_LIMITER
# # store next location
# self.rewind.updateLocation()
#
# channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
#
# # Force state machine ot load next waypoint from rewind manager
# self.controller.state = rewind.LOAD_NEXT
# self.controller.handleRCs(channels)
#
# # make sure the index decrements
# self.assertEqual(self.rewind.currentIndex, 0)
#
# # make sure returned value matches
# self.assertEqual(self.controller.currentTarget.lat, 37.00001)
# self.assertEqual(self.controller.currentTarget.lon, -122.00002)
# self.assertEqual(self.controller.currentTarget.alt, 10.0)
#
# # read in the original and last location
# self.controller.state = rewind.LOAD_NEXT
# self.controller.handleRCs(channels)
#
# # Watch call to switch modes to RTL
# self.controller.shotmgr.enterMode = Mock()
#
# # make sure we trigger the exit to RTL
# self.controller.state = rewind.LOAD_NEXT
# self.controller.handleRCs(channels)
#
# # handleRCs shuold call RTL (mode 6)
# #self.controller.shotmgr.enterMode.assert_called_with(6)
#
#

62
shotmanager/Test/TestRewindManager.py Normal file
View File

@ -0,0 +1,62 @@
# Unit tests for ZiplineShot
import math
import mock
from mock import call
from mock import Mock
from mock import patch
import os
from os import sys, path
import unittest
from dronekit import LocationGlobalRelative, Vehicle
import location_helpers
import rewindManager
from rewindManager import RewindManager
from shotManager import ShotManager
class TestRedind(unittest.TestCase):
def setUp(self):
mgr = mock.create_autospec(ShotManager)
mgr.buttonManager = Mock()
self.mock_vehicle = mock.create_autospec(Vehicle)
self.rewind = RewindManager(self.mock_vehicle, mgr)
self.mock_vehicle.location.global_relative_frame = LocationGlobalRelative(37.0, -122.0, 10.0)
def testInit(self):
""" Test init """
self.assertEqual(self.rewind.bufferSize, int(math.floor(self.rewind.rewindDistance / rewindManager.RTL_STEP_DIST)))
def testReset(self):
""" Test reset """
self.rewind.resetSpline()
for num in range(1,self.rewind.bufferSize):
self.assertEqual(self.rewind.buffer[num], None)
self.assertEqual(self.rewind.bufferSize, len(self.rewind.buffer))
self.assertEqual(self.rewind.buffer[0].lat, 37.0)
self.assertEqual(self.rewind.buffer[0].lon, -122.0)
self.assertEqual(self.rewind.buffer[0].alt, 10.0)
self.assertEqual(self.rewind.buffer[1], None)
self.assertEqual(self.rewind.did_init, True)
def testUpdateLocation(self):
""" Test loc queue """
self.mock_vehicle.location.global_relative_frame = LocationGlobalRelative(37.0, -122.0, 10.0)
self.rewind.did_init = False
self.mock_vehicle.armed = True
self.mock_vehicle.system_status = 'ACTIVE'
self.rewind.resetSpline()
self.mock_vehicle.location.global_relative_frame = LocationGlobalRelative(37.00001, -122.00002, 10.0)
self.rewind.counter = 4
self.rewind.updateLocation()
self.assertEqual(self.rewind.currentIndex, 1)
self.assertEqual(self.rewind.buffer[1].lat, 37.00001)
self.assertEqual(self.rewind.buffer[1].lon, -122.00002)
self.assertEqual(self.rewind.buffer[1].alt, 10.0)

224
shotmanager/Test/TestSelfie.py Normal file
View File

@ -0,0 +1,224 @@
# Unit tests for SelfieShot
import math
import mock
from mock import call
from mock import Mock
from mock import MagicMock
import os
from os import sys, path
from pymavlink import mavutil
import struct
import unittest
from dronekit import LocationGlobalRelative, Vehicle
sys.path.append(os.path.realpath('..'))
import location_helpers
import selfie
from selfie import SelfieShot
from shotManager import ShotManager
from rcManager import rcManager
from shotManagerConstants import *
import shots
import app_packet
# on host systems these files are located here
from sololink import btn_msg
ERROR = 0.1
class Attitude():
yaw = 0.0
class TestSelfieHandleRCsNotStarted(unittest.TestCase):
def setUp(self):
mgr = mock.create_autospec(ShotManager)
mgr.buttonManager = Mock()
mgr.currentShot = shots.APP_SHOT_SELFIE
mgr.rcMgr = Mock(specs = ['remapper'])
v = mock.create_autospec(Vehicle)
v.location.global_relative_frame = LocationGlobalRelative(30.13241, 10.112135, 0.0)
self.controller = SelfieShot(v, mgr)
self.controller.altLimit = 0
def testHandleRCsNotStarted(self):
""" Without points, handleRCs shouldn't do anything """
channels = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
self.controller.handleRCs(channels)
class TestSelfieHandleRCs(unittest.TestCase):
def setUp(self):
mgr = mock.create_autospec(ShotManager)
mgr.buttonManager = Mock()
mgr.currentShot = shots.APP_SHOT_SELFIE
mgr.rcMgr = Mock(specs = ['remapper'])
mgr.appMgr = Mock()
v = mock.create_autospec(Vehicle)
v.location.global_relative_frame = LocationGlobalRelative(30.13241, 10.112135, 0.0)
self.controller = SelfieShot(v, mgr)
self.controller.altLimit = 0
self.controller.addLocation( LocationGlobalRelative(-14.654861, 108.4645, 32.6545) )
self.controller.addLocation( LocationGlobalRelative(23.654504, 40.4645, 17.6545) )
self.controller.addLocation( LocationGlobalRelative(23.661, 10.4645, 2.6545) )
def testRCsMax(self):
""" Test RCs Max """
channels = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
self.controller.handleRCs(channels)
def testRCsMin(self):
""" Test RCs Min """
channels = [-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0]
self.controller.handleRCs(channels)
def testRCsZero(self):
""" Test RCs Max """
channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.controller.handleRCs(channels)
def testRCs2(self):
""" Test RCs Max """
channels = [0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2]
self.controller.handleRCs(channels)
class TestSetButtonMappings(unittest.TestCase):
def setUp(self):
mgr = mock.create_autospec(ShotManager)
mgr.buttonManager = Mock()
mgr.currentShot = shots.APP_SHOT_SELFIE
mgr.buttonManager = Mock()
mgr.rcMgr = Mock(specs = ['remapper'])
v = mock.create_autospec(Vehicle)
v.location.global_relative_frame = LocationGlobalRelative(30.13241, 10.112135, 0.0)
self.controller = SelfieShot(v, mgr)
self.controller.altLimit = 0
def testSetButtonMappingsNoROI(self):
""" Testing setting button mappings when we haven't locked on yet """
self.controller.setButtonMappings()
calls = [call(btn_msg.ButtonA, shots.APP_SHOT_SELFIE, 0, "\0"), call(btn_msg.ButtonB, shots.APP_SHOT_SELFIE, 0, "\0")]
self.controller.shotmgr.buttonManager.setArtooButton.assert_has_calls(calls, any_order = False)
def testSetButtonMappings(self):
""" Testing setting button mappings when we have started """
self.controller.addLocation( LocationGlobalRelative(-14.654861, 108.4645, 32.6545) )
self.controller.addLocation( LocationGlobalRelative(23.654504, 40.4645, 17.6545) )
self.controller.addLocation( LocationGlobalRelative(23.661, 10.4645, 2.6545) )
self.controller.setButtonMappings()
calls = [call(btn_msg.ButtonA, shots.APP_SHOT_SELFIE, 0, "\0"), call(btn_msg.ButtonB, shots.APP_SHOT_SELFIE, 0, "\0")]
self.controller.shotmgr.buttonManager.setArtooButton.assert_has_calls(calls)
class TestHandlePacket(unittest.TestCase):
def setUp(self):
mgr = mock.create_autospec(ShotManager)
mgr.buttonManager = Mock()
mgr.currentShot = shots.APP_SHOT_SELFIE
mgr.rcMgr = Mock(specs = ['remapper'])
v = mock.create_autospec(Vehicle)
v.location.global_relative_frame = LocationGlobalRelative(30.13241, 10.112135, 0.0)
self.controller = SelfieShot(v, mgr)
self.controller.pathHandler = Mock(specs=['isPaused'])
self.controller.altLimit = 0
def testCruiseSpeedSet0(self):
options = struct.pack('<f', 0.0)
self.controller.handlePacket(app_packet.SOLO_SHOT_OPTIONS, 4, options)
self.controller.pathHandler.setCruiseSpeed.assert_called_with(0.0)
def testCruiseSpeedSet6(self):
options = struct.pack('<f', 6.0)
self.controller.handlePacket(app_packet.SOLO_SHOT_OPTIONS, 4, options)
self.controller.pathHandler.setCruiseSpeed.assert_called_with(6.0)
def testCruiseSpeedSet3(self):
options = struct.pack('<f', 3.0)
self.controller.handlePacket(app_packet.SOLO_SHOT_OPTIONS, 4, options)
self.controller.pathHandler.setCruiseSpeed.assert_called_with(3.0)
def testNoPathHandler(self):
""" Shouldn't crash if we don't have a path handler """
options = struct.pack('<f', 1.0)
self.controller.pathHandler = None
self.controller.handlePacket(app_packet.SOLO_SHOT_OPTIONS, 4, options)
class TestHandleButton(unittest.TestCase):
def setUp(self):
mgr = mock.create_autospec(ShotManager)
mgr.buttonManager = Mock()
mgr.currentShot = shots.APP_SHOT_SELFIE
mgr.rcMgr = Mock(specs = ['remapper'])
v = mock.create_autospec(Vehicle)
v.location.global_relative_frame = LocationGlobalRelative(30.13241, 10.112135, 0.0)
self.controller = SelfieShot(v, mgr)
self.controller.pathHandler = Mock(specs=['isPaused','togglePause'])
self.controller.updateAppOptions = Mock()
self.controller.altLimit = 0
def TestPauseCruise(self):
""" pause button pauses cruising """
self.controller.pathHandler.isPaused = Mock(return_value=False)
self.controller.handleButton( btn_msg.ButtonLoiter, btn_msg.Press )
self.controller.pathHandler.togglePause.assert_called_with()
self.controller.updateAppOptions.assert_called_with()
def TestResumeCruise(self):
""" pause button resumes cruising (if already paused) """
self.controller.pathHandler.isPaused = Mock(return_value=True)
self.controller.handleButton( btn_msg.ButtonLoiter, btn_msg.Press )
self.controller.pathHandler.togglePause.assert_called_with()
self.controller.updateAppOptions.assert_called_with()
def TestNotifyPauseLoiter(self):
self.controller.pathHandler = None
self.controller.handleButton(btn_msg.ButtonLoiter, btn_msg.Press)
self.controller.shotmgr.notifyPause.assert_called_with(False)
def TestNotifyPauseGuided(self):
self.controller.handleButton(btn_msg.ButtonLoiter, btn_msg.Press)
self.controller.shotmgr.notifyPause.assert_called_with(True)
class TestAddLocation(unittest.TestCase):
def setUp(self):
mgr = mock.create_autospec(ShotManager)
mgr.buttonManager = Mock()
mgr.currentShot = shots.APP_SHOT_SELFIE
mgr.rcMgr = Mock(specs = ['remapper'])
v = mock.create_autospec(Vehicle)
v.location.global_relative_frame = LocationGlobalRelative(30.13241, 10.112135, 0.0)
self.controller = SelfieShot(v, mgr)
self.controller.altLimit = 0
def testAltitudeLimit(self):
'''Makes sure 2nd selfie point is capped at the user-defined altitude limit'''
self.controller.altLimit = 25 # meters
self.controller.addLocation( LocationGlobalRelative(23.654504, 40.4645, 17.6545) )
self.controller.addLocation( LocationGlobalRelative(-14.654861, 108.4645, 32.6545) )
self.assertAlmostEqual(self.controller.altLimit,self.controller.waypoints[1].alt,1)
def testAltitudeLimitDisabled(self):
'''Makes sure 2nd selfie point is NOT capped if altitude limit is disabled'''
self.controller.altLimit = None
x2 = -14.654861
y2 = 108.4645
z2 = 32.6545
self.controller.addLocation( LocationGlobalRelative(23.654504, 40.4645, 17.6545) )
self.controller.addLocation( LocationGlobalRelative(x2, y2, z2) )
self.assertAlmostEqual(x2, self.controller.waypoints[1].lat,1)
self.assertAlmostEqual(y2, self.controller.waypoints[1].lon,1)
self.assertAlmostEqual(z2, self.controller.waypoints[1].alt,1)
def testAddLocations(self):
""" Test adding locations """
loc = LocationGlobalRelative(37.242124, -122.12841, 15.3)
self.controller.addLocation(loc)
loc = LocationGlobalRelative(30.13241, 10.112135, 0.0)
self.controller.addLocation(loc)
loc = LocationGlobalRelative(-14.654861, 108.4645, 32.6545)
self.controller.addLocation(loc)

62
shotmanager/Test/TestSettings.py Normal file
View File

@ -0,0 +1,62 @@
# Unit tests for buttonManager
import mock
from mock import call
from mock import Mock
from mock import patch
import os
from os import sys, path
import unittest
sys.path.append(os.path.realpath('..'))
import settings
class TestWriteSettingsThread(unittest.TestCase):
def setUp(self):
settings.CONFIG_FILE = "Test/shotmanager.conf"
settings.CONFIG_FILE_BACKUP = "Test/shotmanager.back"
self.lock = Mock()
settings.settingsLock = self.lock
def testLocks(self):
""" Make sure we lock/unlock """
settings.writeSettingsThread("a", "b")
self.lock.acquire.assert_called_with()
self.lock.release.assert_called_with()
def testValueSet(self):
""" Make sure we are setting the correct value """
with patch('ConfigParser.SafeConfigParser') as patchedParser:
parser = Mock()
patchedParser.return_value = parser
settings.writeSettingsThread("aaa", "bbb")
parser.read.assert_called_with("Test/shotmanager.conf")
parser.set.assert_called_with("shotManager", "aaa", "bbb")
class TestReadSetting(unittest.TestCase):
def setUp(self):
mockParser = patch('ConfigParser.SafeConfigParser')
self.addCleanup(mockParser.stop)
mock = mockParser.start()
self.parser = Mock()
settings.CONFIG_FILE = "Test/shotmanager.conf"
mock.return_value = self.parser
def testReadSetting(self):
""" Test that we attempt to read the correct thing """
self.parser.get = Mock(return_value = "foo")
value = settings.readSetting("bleh")
self.parser.get.assert_called_with("shotManager", "bleh")
self.assertEqual(value, "foo")
def testReadBadSetting(self):
""" Test that we get an exception from a failed get """
self.parser.get = Mock(return_value = "foo", side_effect=KeyError("Boo"))
try:
value = settings.readSetting("bleh")
except:
pass
else:
self.assertFalse(True)

308
shotmanager/Test/TestShotManager.py Normal file
View File

@ -0,0 +1,308 @@
# Unit tests for ShotManager
import mock
from mock import call
from mock import Mock
from mock import patch
import os
from os import sys, path
from pymavlink import mavutil
import socket
import struct
import unittest
from dronekit import Vehicle, LocationGlobalRelative, VehicleMode
sys.path.append(os.path.realpath('..'))
import app_packet
from orbit import OrbitShot
from selfie import SelfieShot
from follow import FollowShot
from multipoint import MultipointShot
from cable_cam import CableCamShot
import shotManager
from shotManagerConstants import *
import shots
# on host systems these files are located here
sys.path.append(os.path.realpath('../../../flightcode/stm32'))
from sololink import btn_msg
import __builtin__
ERROR = 0.1
class TestEKFCallback(unittest.TestCase):
@patch.object(socket.socket, 'bind')
def setUp(self, mock_bind):
self.mgr = shotManager.ShotManager()
vehicle = mock.create_autospec(Vehicle)
# Make sure EKF is not reported as ok on Startup
vehicle.ekf_ok = False
self.mgr.Start(vehicle)
def testEKFStartup(self):
""" EKF on startup should be not ok """
self.assertFalse( self.mgr.last_ekf_ok )
def testEKFUnarmedToLoiter(self):
""" Shot manager starts in loiter """
self.mgr.vehicle.ekf_ok = True
self.mgr.buttonManager = Mock()
self.mgr.ekf_callback( self.mgr.vehicle, "ekf_ok", True )
self.assertTrue( self.mgr.last_ekf_ok )
self.assertEqual( self.mgr.vehicle.mode.name, "LOITER" )
self.mgr.buttonManager.setButtonMappings.assert_called_with()
class TestArmedCallback(unittest.TestCase):
@patch.object(socket.socket, 'bind')
def setUp(self, mock_bind):
self.mgr = shotManager.ShotManager()
vehicle = mock.create_autospec(Vehicle)
self.mgr.Start(vehicle)
self.mgr.buttonManager = Mock()
self.mgr.goproManager = Mock()
self.mgr.rewindManager = Mock()
def testArmedStartup(self):
""" armed on startup should be not ok """
self.assertFalse( self.mgr.last_armed )
def testArmedCallback(self):
""" change button mappings if our armed state changes """
self.mgr.vehicle.armed = True
self.mgr.armed_callback( self.mgr.vehicle, "armed", True)
self.assertTrue( self.mgr.last_armed )
self.mgr.buttonManager.setButtonMappings.assert_called_with()
def testArmedCallbackNoChange(self):
""" Don't change button mappings if our armed state doesn't change """
self.mgr.vehicle.armed = False
self.mgr.armed_callback( self.mgr.vehicle, "armed", False )
self.assertFalse( self.mgr.last_armed )
self.assertFalse( self.mgr.buttonManager.setButtonMappings.called )
def testDisarmExitShot(self):
""" Disarming should exit a shot """
self.mgr.currentShot = shots.APP_SHOT_SELFIE
self.mgr.last_armed = True
self.mgr.armed_callback( self.mgr.vehicle, "armed", False )
self.assertTrue(self.mgr.currentShot == shots.APP_SHOT_NONE)
def testDisarmStopGoProRecording(self):
""" Disarming should stop recording on GoPro """
self.mgr.currentShot = shots.APP_SHOT_NONE
self.mgr.last_armed = True
self.mgr.armed_callback( self.mgr.vehicle, "armed", False )
self.assertTrue( self.mgr.goproManager.handleRecordCommand.called )
class TestEnterShot(unittest.TestCase):
@patch.object(socket.socket, 'bind')
def setUp(self, mock_bind):
self.mgr = shotManager.ShotManager()
vehicle = mock.create_autospec(Vehicle)
vehicle.system_status = 'ACTIVE'
self.mgr.Start(vehicle)
self.mgr.buttonManager = Mock()
self.mgr.appMgr.sendPacket = Mock()
self.mgr.rcMgr = Mock()
self.mgr.last_ekf_ok = True
self.mgr.appMgr = Mock()
self.mgr.appMgr.isAppConnected = Mock(return_value=True)
def testShouldNotenterShotOnTheGround(self):
""" Won't enter a shot from the ground """
self.mgr.vehicle.system_status = 'STANDBY'
self.mgr.currentShot = shots.APP_SHOT_NONE
self.mgr.enterShot( shots.APP_SHOT_CABLECAM )
packet = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_UNARMED)
self.mgr.appMgr.sendPacket.assert_any_call( packet )
self.assertEqual( self.mgr.currentShot, shots.APP_SHOT_NONE )
def testShouldNotEnterShotinCritical(self):
'''Won't enter a shot when system is in critical state'''
self.mgr.vehicle.system_status = 'CRITICAL'
self.mgr.currentShot = shots.APP_SHOT_NONE
self.mgr.enterShot( shots.APP_SHOT_CABLECAM )
packet = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_RTL)
self.mgr.appMgr.sendPacket.assert_any_call( packet )
self.assertEqual( self.mgr.currentShot, shots.APP_SHOT_NONE )
def testShouldNotEnterShotinEmergency(self):
'''Won't enter a shot when system is in emergency state'''
self.mgr.vehicle.system_status = 'EMERGENCY'
self.mgr.currentShot = shots.APP_SHOT_NONE
self.mgr.enterShot( shots.APP_SHOT_CABLECAM )
packet = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_RTL)
self.mgr.appMgr.sendPacket.assert_any_call( packet )
self.assertEqual( self.mgr.currentShot, shots.APP_SHOT_NONE )
def testEnterShotNoChange(self):
""" Entering a shot we're already in. No change """
self.mgr.currentShot = shots.APP_SHOT_SELFIE
self.mgr.enterShot( shots.APP_SHOT_SELFIE )
self.assertEqual( self.mgr.currentShot, shots.APP_SHOT_SELFIE )
self.assertEqual( self.mgr.curController, None )
def testEnterShotChange(self):
""" Go from no shot to selfie """
self.mgr.currentShot = shots.APP_SHOT_NONE
self.mgr.enterShot( shots.APP_SHOT_SELFIE )
self.assertEqual( self.mgr.currentShot, shots.APP_SHOT_SELFIE )
self.mgr.buttonManager.setButtonMappings.assert_called_with()
self.mgr.buttonManager.setArtooShot.assert_called_with( shots.APP_SHOT_SELFIE )
self.mgr.appMgr.broadcastShotToApp(shots.APP_SHOT_SELFIE)
self.assertTrue( isinstance(self.mgr.curController, SelfieShot) )
def testOrbit(self):
""" No shot to Orbit """
self.mgr.enterShot( shots.APP_SHOT_ORBIT )
self.assertTrue( isinstance(self.mgr.curController, OrbitShot) )
def testFollow(self):
""" No shot to Follow """
self.mgr.enterShot( shots.APP_SHOT_FOLLOW )
self.assertTrue( isinstance(self.mgr.curController, FollowShot) )
def testNoShot(self):
""" Entering no shot """
self.mgr.vehicle.message_factory = Mock()
self.mgr.getParam = Mock(return_value=500.0)
self.mgr.vehicle.mode = VehicleMode("GUIDED")
self.mgr.currentShot = shots.APP_SHOT_SELFIE
self.mgr.enterShot( shots.APP_SHOT_NONE )
self.mgr.vehicle.gimbal.release.assert_called_with()
self.mgr.rcMgr.enableRemapping.assert_called_with( False )
self.assertEqual( self.mgr.vehicle.mode.name, "LOITER" )
def testBadEKF(self):
""" We have bad EKF, should disallow entry into shot """
self.mgr.last_ekf_ok = False
self.mgr.curController = "yeah"
self.mgr.currentShot = shots.APP_SHOT_SELFIE
self.mgr.enterShot( shots.APP_SHOT_FOLLOW )
self.assertEqual( self.mgr.curController, None )
packetDisallow = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_BAD_EKF)
calls = [call(packetDisallow)]
self.mgr.appMgr.sendPacket.assert_has_calls(calls)
class TestModeCallback(unittest.TestCase):
@patch.object(socket.socket, 'bind')
def setUp(self, mock_bind):
self.mgr = shotManager.ShotManager()
self.v = mock.create_autospec(Vehicle)
self.v.system_status = 'ACTIVE'
self.mgr.Start(self.v)
self.mgr.buttonManager = Mock()
def testModeStartup(self):
""" Shot manager mode on startup should be loiter """
self.assertEqual( self.mgr.vehicle.mode.name, "LOITER" )
self.assertEqual( self.mgr.lastMode, "LOITER" )
def testLoiterToAltHold(self):
""" Loiter to alt hold """
self.v.mode = VehicleMode("ALT_HOLD")
self.v._mode_mapping = { "ALT_HOLD" : 2 }
self.mgr.mode_callback( self.v, "mode", self.v.mode )
self.mgr.buttonManager.setArtooShot.assert_called_with( -1, 2 )
def testExitShot(self):
""" If we're in a guided shot and we enter any other mode, kick us out of our shot """
self.v.mode = VehicleMode("GUIDED")
self.mgr.mode_callback( self.v, "mode", self.v.mode )
# don't send guided to setArtooShot
self.assertFalse( self.mgr.buttonManager.setArtooShot.called )
self.mgr.currentShot = shots.APP_SHOT_CABLECAM
self.v.mode = VehicleMode("LOITER")
self.mgr.mode_callback( self.v, "mode", self.v.mode )
self.assertTrue( self.mgr.currentShot == shots.APP_SHOT_NONE )
def testRTLExitsShot(self):
""" Even if we're not in guided, RTL should exit shots """
self.mgr.currentShot = shots.APP_SHOT_SELFIE
self.v.mode = VehicleMode("RTL")
self.mgr.getParam = Mock(return_value=500.0)
self.mgr.mode_callback( self.v, "mode", self.v.mode )
#self.assertTrue( self.mgr.currentShot == shots.APP_SHOT_RTL )
class TestGetParam(unittest.TestCase):
@patch.object(socket.socket, 'bind')
def setUp(self, mock_bind):
self.mgr = shotManager.ShotManager()
self.v = mock.create_autospec(Vehicle)
self.mgr.Start(self.v)
def testGetParamValid(self):
""" Test retrieval of a valid parameter """
base = {
"WPNAV_SPEED" : 33.4
}
self.v.parameters = Mock(base)
self.v.parameters.get = lambda x, wait_ready=False: base.get(x)
self.assertEqual( self.mgr.getParam( "WPNAV_SPEED", 100.0), 33.4 )
def testGetParamInvalid(self):
""" If a parameter is invalid, use the default value """
base = {
"WPNAV_SPEED" : 33.4
}
self.v.parameters = Mock(base)
self.v.parameters.get = lambda x, wait_ready=False: base.get(x)
self.assertEqual( self.mgr.getParam( "INVALID", 12.0), 12.0 )
class TestTick(unittest.TestCase):
@patch.object(socket.socket, 'bind')
def setUp(self, mock_bind):
self.mgr = shotManager.ShotManager()
self.v = mock.create_autospec(Vehicle)
self.mgr.Start(self.v)
def testTickSetsTime(self):
""" Test that Tick updates the time of when it was last called """
self.mgr.rcMgr.numTicksSinceRCUpdate = 0
lastTime = self.mgr.timeOfLastTick
self.mgr.Tick()
self.assertFalse( lastTime == self.mgr.timeOfLastTick )
self.assertEqual( self.mgr.rcMgr.numTicksSinceRCUpdate, 1 )
def testTickNoRemapping(self):
""" If we stop getting new RC packets, don't tell the RCmanager to send old packets to pixRC """
self.mgr.curController = Mock()
self.mgr.currentShot = shots.APP_SHOT_CABLECAM
self.mgr.rcMgr.channels = [13, 14, 15]
self.mgr.rcMgr = Mock()
self.mgr.rcMgr.remap = Mock(return_value=888)
self.mgr.rcMgr.numTicksSinceRCUpdate = 5
self.mgr.Tick()
self.mgr.rcMgr.remap.assert_called_with()
self.mgr.curController.handleRCs.assert_called_with(888)
class TestCameraCallback(unittest.TestCase):
@patch.object(socket.socket, 'bind')
def setUp(self, mock_bind):
self.mgr = shotManager.ShotManager()
vehicle = mock.create_autospec(Vehicle)
vehicle.camera_trigger_msg = Mock();
self.mgr.Start(vehicle)
self.mgr.cameraTLogFile = Mock()
def testCameraTriggerCallbackIsAutoMission(self):
self.mgr.vehicle.mode.name = 'AUTO'
self.mgr.goproManager = Mock()
self.mgr.camera_feedback_callback( self.mgr.vehicle, "camera_feedback", Mock() )
self.assertTrue(self.mgr.goproManager.handleRecordCommand.called)
def testCameraTriggerCallbackIsNotAutoMission(self):
self.mgr.vehicle.mode.name = 'GUIDED'
self.mgr.goproManager = Mock()
self.mgr.camera_feedback_callback( self.mgr.vehicle, "camera_feedback", None )
self.assertFalse(self.mgr.goproManager.handleRecordCommand.called)
def testCameraTriggerCallbackIsNotSiteScanShot(self):
self.mgr.currentShot = shots.APP_SHOT_MULTIPOINT
self.mgr.goproManager = Mock()
self.mgr.camera_feedback_callback( self.mgr.vehicle, "camera_feedback", None )
self.assertFalse(self.mgr.goproManager.handleRecordCommand.called)

133
shotmanager/Test/TestVectorPathHandler.py Normal file
View File

@ -0,0 +1,133 @@
# Unit tests for ROI
import mock
from mock import Mock
import os
from os import sys, path
from pymavlink import mavutil
from shotManagerConstants import *
import unittest
import random
import math
from dronekit import Vehicle, LocationGlobalRelative
sys.path.append(os.path.realpath('..'))
import pathHandler
from shotManager import ShotManager
import shots
import vectorPathHandler
#Random number generator seed
SEED = 94739473
#Number of tests to run
REPEAT = 10
class TestMove(unittest.TestCase):
def setUp(self):
mgr = mock.create_autospec(ShotManager)
#mgr.currentShot = shots.APP_SHOT_ZIPLINE
v = mock.create_autospec(Vehicle)
v.message_factory = Mock()
v.commands = Mock()
self.handler = vectorPathHandler.VectorPathHandler(v, mgr, 0, 0)
random.seed(SEED)
def testTogglePauseShouldPause(self):
'''If cruiseSpeed != 0 then pause() should be executed'''
self.handler.pause = Mock()
self.handler.cruiseSpeed = 4.0
self.handler.togglePause()
self.handler.pause.assert_called_with()
def testTogglePauseShouldResume(self):
'''If cruiseSpeed == 0 then resume() should be executed'''
self.handler.resume = Mock()
self.handler.cruiseSpeed = 0.0
self.handler.togglePause()
self.handler.resume.assert_called_with()
def testSpotLock(self):
''' test accurcy of spot lock'''
vect = self.handler.getUnitVectorFromHeadingAndTilt(0, -45)
self.assertEqual(int(vect.y*1000), 0)
self.assertEqual(int(vect.x*1000), 707)
self.assertEqual(int(vect.z*1000), -707)
vect = self.handler.getUnitVectorFromHeadingAndTilt(90, -15)
self.assertEqual(int(vect.y*1000), 965)
self.assertEqual(int(vect.x*1000), 0)
self.assertEqual(int(vect.z*1000), -258)
vect = self.handler.getUnitVectorFromHeadingAndTilt(180, 0)
self.assertEqual(int(vect.y*1000), 0)
self.assertEqual(int(vect.x*1000), -1000)
self.assertEqual(int(vect.z*1000), 0)
def testNoCruiseForwardsFullStick(self):
""" Command should reach (and not exceed) MAX_SPEED [Flying in forwards (+) direction, full stick in positive direction] """
for test in range(REPEAT): #test REPEAT times with random values based on SEED
self.handler.cruiseSpeed = 0.0 #No cruise
initialSpeed = random.uniform(0.0,vectorPathHandler.MAX_PATH_SPEED) #Start at a random positive speed within acceptable limits
self.handler.accel = 0.136000
self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #Full positive stick
channels[ROLL] = 1.0
#calculate number of ticks required to reach MAX_SPEED
deltaSpeed = vectorPathHandler.MAX_PATH_SPEED - self.handler.currentSpeed
ticksRequired = int(math.ceil(deltaSpeed/self.handler.accel))
for ticks in range(1,ticksRequired+1):
#run controller tick
returnSpeed = self.handler.move( channels )
#make sure we are targeted to the right loc
self.assertEqual(returnSpeed, vectorPathHandler.MAX_PATH_SPEED)
def testNoCruiseBackwardsFullNegStick(self):
""" Command should reach (and not exceed) MAX_SPEED [Flying in rewards (-) direction, full stick in negative direction] """
for test in range(REPEAT): #test REPEAT times with random values based on SEED
self.handler.cruiseSpeed = 0.0 #No cruise
self.handler.accel = 0.136000
initialSpeed = 0
self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #Full negative stick
channels[ROLL] = -1.0
self.handler.accel = 1.0
#calculate number of ticks required to reach MAX_SPEED
deltaSpeed = -vectorPathHandler.MAX_PATH_SPEED - self.handler.currentSpeed
ticksRequired = int(math.ceil(abs(deltaSpeed/self.handler.accel)))
for ticks in range(1,ticksRequired+1):
#run controller tick
returnSpeed = self. handler.move( channels )
#make sure we are targeted to the right loc
self.assertGreaterEqual(returnSpeed, 0.0)
self.assertEqual(returnSpeed, vectorPathHandler.MAX_PATH_SPEED)
# THROTTLE = 0
# ROLL = 1
# PITCH = 2
# YAW = 3
# FILTERED_PADDLE = 5
# RAW_PADDLE = 7
def testCruiseSameDir(self):
""" Cruising with full same stick towards loc 2- should accelerate """
self.handler.cruiseSpeed = 4.0 #random positive cruise speed
self.handler.accel = 0.136000
self.handler.currentSpeed = self.handler.cruiseSpeed #start out at cruise
channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #Stick forward
channels[PITCH] = -1.0
returnSpeed = self.handler.move( channels )
self.assertGreaterEqual(returnSpeed, self.handler.cruiseSpeed)
def testCruiseOppositeDir(self):
""" Cruising towards loc 2 with opposite stick - should decelerate to a steady state speed"""
self.handler.cruiseSpeed = 4.0 #random positive cruise speed
self.handler.accel = 0.136000
self.handler.currentSpeed = self.handler.cruiseSpeed #start out at cruise
channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #Stick backward
channels[PITCH] = 1.0
returnSpeed = self.handler.move( channels )
self.assertLessEqual(returnSpeed, self.handler.cruiseSpeed)

209
shotmanager/Test/TestYawPitchOffsetter.py Normal file
View File

@ -0,0 +1,209 @@
# Unit tests for yawPitchOffsetter
import mock
from mock import Mock
import os
from os import sys, path
import struct
import unittest
sys.path.append(os.path.realpath('..'))
from shotManagerConstants import *
import yawPitchOffsetter
class TestInit(unittest.TestCase):
def testInit(self):
""" test initial variables """
setter = yawPitchOffsetter.YawPitchOffsetter()
self.assertEquals( setter.yawOffset, 0.0 )
self.assertEquals( setter.pitchOffset, 0.0 )
self.assertTrue( setter.handlePitch )
self.assertTrue( setter.isNudge )
def testNoHandlePitch(self):
""" Test if we set handlePitch to False """
setter = yawPitchOffsetter.YawPitchOffsetter(False)
self.assertFalse( setter.handlePitch )
class TestUpdate(unittest.TestCase):
def setUp(self):
self.setter = yawPitchOffsetter.YawPitchOffsetter()
self.setter.offsetYaw = Mock()
self.setter.offsetPitch = Mock()
def testChan3ToYaw(self):
""" Channel 3 is used to offset yaw """
channels = [0.0, 1.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0]
self.setter.Update( channels )
self.setter.offsetYaw.assert_called_with( 0.3 )
def testChan7Pitch(self):
""" When abs(chan7) > abs (chan0) it's used for pitch """
channels = [0.3, 1.0, 0.0, 0.3, 0.0, 0.0, 0.0, -0.8]
self.setter.Update( channels )
self.setter.offsetPitch.assert_called_with( -0.8 )
def testChan0Pitch(self):
""" When abs(chan0) > abs (chan7) it's used for pitch """
channels = [0.38, 1.0, 0.0, 0.3, 0.0, 0.0, 0.0, -0.142]
self.setter.Update( channels )
self.setter.offsetPitch.assert_called_with( 0.38 )
def testUpdateNoChange(self):
""" Update should return false if neither yawOffset nor pitchOffset change """
channels = [0.38, 1.0, 0.0, 0.3, 0.0, 0.0, 0.0, -0.142]
result = self.setter.Update( channels )
self.assertFalse( result )
def testUpdateYawChange(self):
""" Update should return true if yawOffset changes """
def alter(a):
self.setter.yawOffset = 20.0
channels = [0.38, 1.0, 0.0, 0.3, 0.0, 0.0, 0.0, -0.142]
self.setter.offsetYaw = Mock(side_effect = alter )
result = self.setter.Update( channels )
self.assertTrue( result )
def testUpdatePitchChange(self):
""" Update should return true if pitchOffset changes """
def alter(a):
self.setter.pitchOffset = 20.0
channels = [0.38, 1.0, 0.0, 0.3, 0.0, 0.0, 0.0, -0.142]
self.setter.offsetPitch = Mock(side_effect = alter )
result = self.setter.Update( channels )
self.assertTrue( result )
class TestOffsetYaw(unittest.TestCase):
def setUp(self):
self.setter = yawPitchOffsetter.YawPitchOffsetter()
def testOffsetYawOnce(self):
""" offset yaw once """
self.setter.offsetYaw( 0.5 )
self.assertEquals( self.setter.yawOffset, 0.5 * yawPitchOffsetter.YAW_OFFSET_SPEED * UPDATE_TIME )
def testOffsetYawNeg10x(self):
""" offset yaw 10 x in the negative direction """
startYaw = 20.2
self.setter.yawOffset = startYaw
for i in range(10):
self.setter.offsetYaw( -0.3 )
startYaw -= 0.3 * yawPitchOffsetter.YAW_OFFSET_SPEED * UPDATE_TIME
self.assertEquals( self.setter.yawOffset, startYaw )
self.assertEquals( self.setter.yawDir, -1 )
def testReturnToZero(self):
""" Eventually this yawOffset should return to zero """
self.setter.yawOffset = 80.5
for i in range(1000):
self.setter.offsetYaw( 0.00 )
self.assertEquals( self.setter.yawOffset, 0.0 )
self.assertEquals( self.setter.yawDir, 1 )
def testNoReturnToZero(self):
""" If nudge mode is off we do not return to zero """
self.setter.yawOffset = 30.5
self.setter.isNudge = False
for i in range(100):
self.setter.offsetYaw( 0.0 )
self.assertEquals( self.setter.yawOffset, 30.5 )
def testYawBounds(self):
""" Should not go past YAW_OFFSET_THRESHOLD """
self.setter.yawOffset = -59.5
for i in range(1000):
self.setter.offsetYaw( -1.0 )
self.assertEquals( self.setter.yawOffset, -yawPitchOffsetter.YAW_OFFSET_THRESHOLD )
def testNoYawBounds(self):
""" In non-nudge mode, we are unconstrained, except for keeping things in the (0, 360) range """
startYaw = 18.546584
self.setter.yawOffset = startYaw
self.setter.isNudge = False
for i in range(1000):
self.setter.offsetYaw( -0.8 )
startYaw -= 0.8 * yawPitchOffsetter.YAW_OFFSET_SPEED * UPDATE_TIME
if startYaw < 0.0:
startYaw += 360.0
self.assertEquals( self.setter.yawOffset, startYaw )
class TestOffsetPitch(unittest.TestCase):
def setUp(self):
self.setter = yawPitchOffsetter.YawPitchOffsetter()
def testOffsetPitchOnce(self):
""" offset pitch once """
self.setter.offsetPitch( 0.22 )
self.assertEquals( self.setter.pitchOffset, 0.22 * yawPitchOffsetter.PITCH_OFFSET_SPEED * UPDATE_TIME )
def testOffsetPitchNeg10x(self):
""" offset pitch 12 x """
startPitch = 2.8
self.setter.pitchOffset = startPitch
for i in range(10):
self.setter.offsetPitch( 0.6 )
startPitch += 0.6 * yawPitchOffsetter.PITCH_OFFSET_SPEED * UPDATE_TIME
self.assertEquals( self.setter.pitchOffset, startPitch )
def testReturnToZero(self):
""" Eventually this pitchOffset should return to zero """
self.setter.pitchOffset = -26.8
for i in range(1000):
self.setter.offsetPitch( 0.02 )
self.assertEquals( self.setter.yawOffset, 0.0 )
def testNoReturnToZero(self):
""" With nudging off, we do not return to zero """
self.setter.isNudge = False
self.setter.pitchOffset = -33.333
for i in range(100):
self.setter.offsetPitch( 0.0 )
self.assertEquals( self.setter.pitchOffset, -33.333 )
def testPitchBounds(self):
"""Should not go past negative PITCH_OFFSET_THRESHOLD """
self.setter.pitchOffset = -19.5
for i in range(1000):
self.setter.offsetPitch( -1.0 )
self.assertEquals( self.setter.pitchOffset, -yawPitchOffsetter.PITCH_OFFSET_THRESHOLD )
def testNoNudgePitchBounds(self):
""" With nudge off, we have different constraints """
self.setter.pitchOffset = -19.645
self.setter.isNudge = False
for i in range(1000):
self.setter.offsetPitch( 1.0 )
self.assertEquals( self.setter.pitchOffset, 0.0 )
def testNoNudgePitchBoundsNegative(self):
""" With nudge off, our constraint is PITCH_OFFSET_NO_NUDGE_THRESHOLD (-90) """
self.setter.pitchOffset = -19.5
self.setter.isNudge = False
for i in range(1000):
self.setter.offsetPitch( -1.0 )
self.assertEquals( self.setter.pitchOffset, yawPitchOffsetter.PITCH_OFFSET_NO_NUDGE_THRESHOLD )
class TestEnableNudge(unittest.TestCase):
def testEnableNudge(self):
""" test that EnableNudge sets isNudge and clears out yaw/pitch offsets """
setter = yawPitchOffsetter.YawPitchOffsetter()
setter.isNudge = False
setter.yawOffset = -17.9
setter.pitchOffset = 28.9
setter.enableNudge()
self.assertTrue(setter.isNudge)
self.assertEquals(setter.yawOffset, 0.0)
self.assertEquals(setter.pitchOffset, 0.0)
class TestDisableNudge(unittest.TestCase):
def testDisableNudge(self):
""" test that DisableNudge sets isNudge to false and initializes yaw/pitch offsets """
setter = yawPitchOffsetter.YawPitchOffsetter()
setter.isNudge = True
setter.yawOffset = -17.9
setter.pitchOffset = 28.9
setter.disableNudge( -13.444, 87.56 )
self.assertFalse(setter.isNudge)
self.assertEquals(setter.pitchOffset, -13.444)
self.assertEquals(setter.yawOffset, 87.56)

111
shotmanager/Test/TestZipline.py Normal file
View File

@ -0,0 +1,111 @@
# Unit tests for ZiplineShot
import math
import mock
from mock import call
from mock import Mock
from mock import patch
import os
from os import sys, path
from pymavlink import mavutil
import struct
import unittest
from dronekit import LocationGlobalRelative, Vehicle
sys.path.append(os.path.realpath('..'))
import location_helpers
import zipline
from zipline import ZiplineShot
from shotManagerConstants import *
import shots
# on host systems these files are located here
from sololink import btn_msg
import app_packet
ERROR = 0.1
class TestNewZipline(unittest.TestCase):
def setUp(self):
mgr = Mock(spec = ["sendPacket", "remapper", "rcMgr", "appMgr", "getParam"])
mgr.currentShot = shots.APP_SHOT_ZIPLINE
mgr.buttonManager = Mock()
mgr.getParam.return_value = 0 # so mock doesn't do lazy binds
self.mock_vehicle = mock.create_autospec(Vehicle)
self.controller = ZiplineShot(self.mock_vehicle, mgr)
self.mock_vehicle.location.global_relative_frame = LocationGlobalRelative(37.242124, -122.12841, 15.3)
def test3DZipline(self):
""" Test 3D """
self.controller.is2D = False
self.controller.setupZipline()
def testRCsZero(self):
""" Test RCs Max """
channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.controller.handleRCs(channels)
def testRCsMax(self):
""" Test RCs Max """
channels = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
self.controller.handleRCs(channels)
def testRCsMin(self):
""" Test RCs Min """
channels = [-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0]
self.controller.handleRCs(channels)
def TestHandleAButtonNewZipline(self):
""" This should record a new Zipline """
self.controller.setupZipline = Mock()
self.controller.handleButton( btn_msg.ButtonA, btn_msg.Press )
self.controller.setupZipline.assert_called_with()
def TestHandleBButtonSpotLock(self):
""" This should Trigger Spot Lock """
self.controller.spotLock = Mock()
self.controller.camPointing = zipline.FREE_LOOK
self.controller.handleButton( btn_msg.ButtonB, btn_msg.Press )
self.controller.spotLock.assert_called_with()
self.assertEqual(self.controller.camPointing, zipline.SPOT_LOCK)
def TestHandleBButtonFreeCam(self):
""" This should Revert to Free Look """
self.controller.manualGimbalTargeting = Mock()
self.controller.camPointing = zipline.SPOT_LOCK
self.controller.handleButton( btn_msg.ButtonB, btn_msg.Press )
self.controller.manualGimbalTargeting.assert_called_with()
self.assertEqual(self.controller.camPointing, zipline.FREE_LOOK)
def TestMoveSpotLock(self):
""" Move Spot Lock """
self.controller.handleSpotLock = Mock()
self.controller.state = zipline.ZIPLINE_RUN
self.controller.camPointing = zipline.FREE_LOOK
self.controller.handleButton( btn_msg.ButtonB, btn_msg.Press )
self.assertEqual(self.controller.camPointing, zipline.SPOT_LOCK)
channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
channels[YAW] = 1.0
channels[RAW_PADDLE] = 1.0
self.controller.handleRCs(channels)
self.controller.handleSpotLock.assert_called_with(channels)
def TestRaiseSpotLock(self):
""" Raise Spot Lock """
#self.controller.updateROIAlt = Mock()
self.controller.camPointing = zipline.FREE_LOOK
self.controller.state = zipline.ZIPLINE_RUN
self.controller.handleButton( btn_msg.ButtonB, btn_msg.Press )
self.controller.camPointing = zipline.SPOT_LOCK
channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
channels[RAW_PADDLE] = 1.0
self.controller.handleRCs(channels)
self.assertGreaterEqual(self.controller.roi.alt, .24)
channels[RAW_PADDLE] = -1.0
self.controller.handleRCs(channels)
self.assertEqual(self.controller.roi.alt, 0)

0
shotmanager/Test/shotmanager.conf Normal file
View File

1
shotmanager/Test/shotmanager.conf.md5 Normal file
View File

@ -0,0 +1 @@
d41d8cd98f00b204e9800998ecf8427e Test/shotmanager.conf

249
shotmanager/appManager.py Normal file
View File

@ -0,0 +1,249 @@
#
# appManager.py
# shotmanager
#
# Handles app connection state and IO.
#
# Created by Will Silva on 3/5/2016.
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import errno
import os
import platform
import select
import socket
import string
import sys
import threading
import monotonic
import time
sys.path.append(os.path.realpath(''))
import modes
import settings
import shots
import Queue
import struct
from dronekit.lib import LocationGlobalRelative
from sololink import btn_msg
import app_packet
import GoProManager
import shotLogger
import GeoFenceManager
logger = shotLogger.logger
APP_SERVER_PORT = 5507
APP_TCP_BUFSIZE = 1024
class appManager():
def __init__(self, shotMgr):
self.shotMgr = shotMgr
self.connected = False
self.client = None
self.client_address = None
self.clientQueue = None
self.packetBuffer = ""
self.bindServer()
def bindServer(self):
self.server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
if platform.system() != 'Darwin':
self.server.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1) # After 1 second, start KEEPALIVE
self.server.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPIDLE, 1) # TCP Idle true
self.server.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPINTVL, 5) # 5 seconds in between keepalive pings
self.server.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPCNT, 5) # 5 max fails
# Bind the socket to the port
while True:
try:
self.server.bind(('', APP_SERVER_PORT))
except:
logger.log("[app]: Can't bind, address in use. Retrying in 1 second.")
time.sleep(1.0)
else:
break
logger.log("[app]: Ready for connections from app.")
# Listen for incoming connections
self.server.listen(0)
# establishes a connection to a Solo app
def connectClient(self):
client, client_address = self.server.accept()
if self.isAppConnected():
if client_address[0] != self.client_address[0]:
logger.log("[app]: Already connected to client %s - rejecting %s." % (self.client_address, client_address))
#TO DO:send system INFO packet to app with rejection flag True
client.close()
return
else:
logger.log("[app]: Detected re-connection attempt for client %s - reconnecting.")
self.disconnectClient()
self.client = client
self.client_address = client_address
logger.log("[app]: Connected to App. %s" % (self.client_address,))
#TO DO:send system INFO packet to app with rejection flag False
self.connected = True
self.client.setblocking(0)
self.shotMgr.inputs.append(self.client)
self.clientQueue = Queue.Queue()
self.broadcastShotToApp(self.shotMgr.currentShot)
self.shotMgr.buttonManager.setButtonMappings() # called to un-grey out Artoo buttons
self.shotMgr.goproManager.sendState() # send gopro state to app
def disconnectClient(self):
if self.isAppConnected():
logger.log("[app]: Closing client connection with %s." % (self.client_address,))
self.connected = False
if self.client in self.shotMgr.outputs:
self.shotMgr.outputs.remove(self.client)
if self.client in self.shotMgr.inputs:
self.shotMgr.inputs.remove(self.client)
self.client.close()
self.client = None
self.clientQueue = None
self.shotMgr.buttonManager.setButtonMappings() # called to grey-out Artoo buttons
# if this type of shot requires a client present at all times, then kill the shot
if self.shotMgr.currentShot in shots.ALWAYS_NEEDS_APP_CONNECTION:
self.shotMgr.enterShot(shots.APP_SHOT_NONE)
# Clear Geofence when app disconnect
self.shotMgr.geoFenceManager.clearGeoFence()
else:
logger.log('[app]: Attempted to close app connection, but no app was connected!')
def isAppConnected(self):
return self.connected
def sendPacket(self, pkt):
if self.isAppConnected():
self.clientQueue.put(pkt)
if self.client not in self.shotMgr.outputs:
self.shotMgr.outputs.append(self.client)
else:
logger.log('[app]: Can\'t send packet - app is not connected!')
def broadcastShotToApp(self, shot):
packet = struct.pack('<IIi', app_packet.SOLO_MESSAGE_GET_CURRENT_SHOT, 4, shot)
self.sendPacket(packet)
def exception(self):
logger.log("[app]: Exception with " + self.client.getpeername())
self.appMgr.disconnectClient()
def write(self):
if self.clientQueue:
try:
msg = self.clientQueue.get_nowait()
except Queue.Empty:
# no messages left, stop checking
self.shotMgr.outputs.remove(self.client)
else:
try:
self.client.send(msg)
except Exception as ex:
logger.log("[app]: Exception on send. (%s)" % ex)
self.disconnectClient()
def parse(self):
try:
data = self.client.recv(APP_TCP_BUFSIZE) # grab one kB
if not data:
raise socket.error()
except socket.error:
logger.log('[app]: Data from client %s is nil.' % (self.client_address,))
self.disconnectClient()
return
self.packetBuffer += data
while self.packetBuffer is not '':
if len(self.packetBuffer) < app_packet.SOLO_MESSAGE_HEADER_LENGTH:
logger.log('[app]: Not enough data for a Solo packet header yet.')
return
(packetType, packetLength) = struct.unpack('<II', self.packetBuffer[:app_packet.SOLO_MESSAGE_HEADER_LENGTH])
if len(self.packetBuffer) < app_packet.SOLO_MESSAGE_HEADER_LENGTH + packetLength:
logger.log('[app]: Not enough data for a Solo packet (ID: %s) yet.' % (packetType,))
return
# extract packet value from TLV based on known packetLength and packetType
packetValue = self.packetBuffer[app_packet.SOLO_MESSAGE_HEADER_LENGTH:(app_packet.SOLO_MESSAGE_HEADER_LENGTH+packetLength)]
handled = False
# if a shot is active, pass the TLV packet to the shot's handlePacket function
if self.shotMgr.curController:
handled = self.shotMgr.curController.handlePacket(packetType, packetLength, packetValue)
# if the packet wasn't understood by the shot, then try to handle it
if not handled:
handled = self.handlePacket(packetType, packetLength, packetValue)
# crop out the packet from the buffer and move on
self.packetBuffer = self.packetBuffer[app_packet.SOLO_MESSAGE_HEADER_LENGTH+packetLength:]
def handlePacket(self, packetType, packetLength, packetValue):
try:
if packetType == app_packet.SOLO_MESSAGE_SET_CURRENT_SHOT:
shot = struct.unpack('<i', packetValue)[0]
logger.log("[app]: App requested shot : %s." % shots.SHOT_NAMES[shot])
self.shotMgr.enterShot(shot)
elif packetType == app_packet.SOLO_MESSAGE_GET_BUTTON_SETTING:
# this is a request for the current button mapping.
# fill in the fields and send it back
(button, event, shot, APMmode) = struct.unpack('<iiii', packetValue)
if event == btn_msg.Press:
(mappedShot, mappedMode) = self.shotMgr.buttonManager.getFreeButtonMapping(button)
logger.log("[app]: App requested button mapping for %d"%(button))
# send back to the app
packet = struct.pack('<IIiiii', app_packet.SOLO_MESSAGE_GET_BUTTON_SETTING, 16, button, event, mappedShot, mappedMode)
self.sendPacket(packet)
# app is trying to map a button
elif packetType == app_packet.SOLO_MESSAGE_SET_BUTTON_SETTING:
(button, event, shot, APMmode) = struct.unpack('<iiii', packetValue)
if event == btn_msg.Press:
self.shotMgr.buttonManager.setFreeButtonMapping( button, shot, APMmode )
# Gopromanager handles these messages
elif packetType in GoProManager.GOPROMESSAGES:
self.shotMgr.goproManager.handlePacket( packetType, packetValue )
elif packetType == app_packet.SOLO_REWIND_OPTIONS or packetType == app_packet.SOLO_HOME_LOCATION:
self.shotMgr.rewindManager.handlePacket( packetType, packetLength, packetValue )
# Geofence messages
elif packetType in GeoFenceManager.GEO_FENCE_MESSAGES:
self.shotMgr.geoFenceManager.handleFenceData(packetType, packetValue)
else:
logger.log("[app]: Got an unknown packet type: %d." % (packetType,))
except Exception as e:
logger.log('[app]: Error handling packet. (%s)' % e)
return False
else:
return True

98
shotmanager/app_packet.py Executable file
View File

@ -0,0 +1,98 @@
#!/usr/bin/env python
# definitions of packets that go from the App to Artoo or App to Solo and vice versa.
# All packets are of the form (in little endian)
# 32-bit type identifier
# 32-bit length
# n bytes value
# https://docs.google.com/a/3drobotics.com/document/d/1rA1zs3T7X1n9ip9YMGZEcLCW6Mx1RR1bNlh9gF0i8nM/edit#heading=h.tcfcw63p9sfk
# packet type definitions
# Solo-App messages
# NOTE: Make sure this stays in sync with the app's definitions! Those are in iSolo/networking/SoloPacket.swift
SOLO_MESSAGE_HEADER_LENGTH = 8
# Sends Solo's current shot to the app
SOLO_MESSAGE_GET_CURRENT_SHOT = 0
SOLO_MESSAGE_SET_CURRENT_SHOT = 1
# send a location
SOLO_MESSAGE_LOCATION = 2
# record a position (for cable cam)
SOLO_RECORD_POSITION = 3
SOLO_CABLE_CAM_OPTIONS = 4
SOLO_MESSAGE_GET_BUTTON_SETTING = 5
SOLO_MESSAGE_SET_BUTTON_SETTING = 6
SOLO_PAUSE = 7
SOLO_FOLLOW_OPTIONS = 19
SOLO_FOLLOW_OPTIONS_V2 = 119
SOLO_SHOT_OPTIONS = 20
SOLO_SHOT_ERROR = 21
SOLO_PANO_OPTIONS = 22
SOLO_ZIPLINE_OPTIONS = 23
SOLO_REWIND_OPTIONS = 24
SOLO_PANO_STATE = 25
SOLO_HOME_LOCATION = 26
SOLO_POWER_STATE = 27
SOLO_ZIPLINE_LOCK = 28
SOLO_SPLINE_RECORD = 50
SOLO_SPLINE_PLAY = 51
SOLO_SPLINE_POINT = 52
SOLO_SPLINE_SEEK = 53
SOLO_SPLINE_PLAYBACK_STATUS = 54
SOLO_SPLINE_PATH_SETTINGS = 55
SOLO_SPLINE_DURATIONS = 56
SOLO_SPLINE_ATTACH = 57
# Artoo-App messages start at 100
# Shot manager to app messages start at 1000
SOLO_MESSAGE_SHOTMANAGER_ERROR = 1000
# recorded waypoint contents
SOLO_CABLE_CAM_WAYPOINT = 1001
# IG shots.
## IG Inspect - app to shotmanager
SOLO_INSPECT_START = 10001
SOLO_INSPECT_SET_WAYPOINT = 10002
SOLO_INSPECT_MOVE_GIMBAL = 10003
SOLO_INSPECT_MOVE_VEHICLE = 10004
## IG Scan
SOLO_SCAN_START = 10101
## IG Survey
SOLO_SURVEY_START = 10201
# Geo Fence
GEOFENCE_SET_DATA = 3000
GEOFENCE_SET_ACK = 3001
GEOFENCE_UPDATE_POLY = 3002
GEOFENCE_CLEAR = 3003
GEOFENCE_ACTIVATED = 3004
# Gopro control messages
GOPRO_SET_ENABLED = 5000
GOPRO_SET_REQUEST = 5001
GOPRO_RECORD = 5003
GOPRO_V1_STATE = 5005
GOPRO_V2_STATE = 5006
GOPRO_REQUEST_STATE = 5007
GOPRO_SET_EXTENDED_REQUEST = 5009
# enums for packet types
# failure to enter a shot due to poor ekf
SHOT_ERROR_BAD_EKF = 0
# can't enter shot if we're not armed
SHOT_ERROR_UNARMED = 1
#can't enter shot if we're RTL
SHOT_ERROR_RTL = 2
# status error codes for spline point message
SPLINE_ERROR_NONE = 0
SPLINE_ERROR_MODE = -1
SPLINE_ERROR_DUPLICATE = -2

278
shotmanager/buttonManager.py Normal file
View File

@ -0,0 +1,278 @@
#
# This file handles incoming buttons from Artoo
# It also handles all communication to Artoo
#
import errno
import os
import platform
import select
import socket
import string
import sys
import threading
import monotonic
import time
from pymavlink import mavutil
from dronekit import VehicleMode
sys.path.append(os.path.realpath(''))
import modes
import settings
import shots
import shotLogger
from sololink import btn_msg
from GoProConstants import *
import shots
PORT = 5016
# connect to our simulation
if os.path.dirname(__file__) != '/usr/bin':
HOST = "localhost"
else:
HOST = "10.1.1.1"
logger = shotLogger.logger
class buttonManager():
def __init__(self, shotMgr):
self.shotMgr = shotMgr
self.connected = False
self.buttonsInitialized = False
self.connectingThread = None
self.connect()
# These are the mappings for the A,B buttons during Free Flight
# The A+B buttons are mapped to shot, mode tuples
# only one of which should ever be active at any time
self.freeButtonMappings = [(-1, -1), (-1, -1)]
try:
aMapping = settings.readSetting("A")
bMapping = settings.readSetting("B")
values = string.split(aMapping, ",")
self.freeButtonMappings[0] = (int(values[0]), int(values[1]))
values = string.split(bMapping, ",")
self.freeButtonMappings[1] = (int(values[0]), int(values[1]))
except:
logger.log("[button]: error reading config file")
else:
logger.log("[button]: read in button mappings")
logger.log("[button]: Button A - shot %s, mode %s"%(shots.SHOT_NAMES[self.freeButtonMappings[0][0]], modes.MODE_NAMES[self.freeButtonMappings[0][1]]))
logger.log("[button]: Button B - shot %s, mode %s"%(shots.SHOT_NAMES[self.freeButtonMappings[1][0]], modes.MODE_NAMES[self.freeButtonMappings[1][1]]))
def connect(self):
if not self.connectingThread or not self.connectingThread.is_alive():
logger.log("[button]: Creating a new thread to connect to Artoo.")
self.client = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.client.setblocking(0)
self.connectingThread = threading.Thread(target = self.connectThread)
self.connectingThread.daemon = True
self.connectingThread.start()
def connectThread(self):
while not self.connected:
try:
self.client.connect((HOST, PORT))
except socket.error as e:
pass
finally:
if e.errno == errno.EINPROGRESS:
time.sleep(1.0)
elif e.errno == errno.ECONNREFUSED:
time.sleep(1.0)
elif e.errno == errno.EALREADY:
time.sleep(1.0)
elif e.errno == errno.EINVAL:
break
elif e.errno == errno.EISCONN:
logger.log("[button]: Connected to Artoo.")
self.connected = True
self.buttonsInitialized = False
else:
logger.log("[button]: Unexpected socket exception: %s" % e)
time.sleep(1.0)
def parse(self):
try:
msg = btn_msg.recv(self.client)
if not msg:
raise Exception('No msg from Artoo.')
except Exception as e:
logger.log('[button]: Data from Artoo is nil.')
self.disconnect()
else:
self.handleButtons((msg[1],msg[2]))
def disconnect(self):
logger.log('[button]: Disconnecting from Artoo.')
self.shotMgr.inputs.remove(self.client)
self.client.close()
self.connected = False
self.buttonsInitialized = False
def checkButtonConnection(self):
if not self.isButtonConnected():
if not self.connectingThread or not self.connectingThread.is_alive():
self.connect()
return
if not self.isButtonInited():
self.shotMgr.inputs.append(self.client)
self.setButtonMappings()
self.setArtooShot(self.shotMgr.currentShot, self.shotMgr.currentModeIndex)
self.buttonsInitialized = True
# set Artoo's button mappings
def setButtonMappings(self):
if not self.isButtonConnected():
return
if self.shotMgr.currentShot == shots.APP_SHOT_NONE:
aString = "\0"
bString = "\0"
enabled1 = btn_msg.ARTOO_BITMASK_ENABLED
enabled2 = btn_msg.ARTOO_BITMASK_ENABLED
if self.freeButtonMappings[0][0] >= 0:
aString = shots.SHOT_NAMES[self.freeButtonMappings[0][0]]
# if ekf is bad or the app is not connected, gray out
if not self.shotMgr.vehicle.ekf_ok or not self.shotMgr.appMgr.isAppConnected():
enabled1 = 0
elif self.freeButtonMappings[0][1] >= 0:
aString = modes.MODE_NAMES[self.freeButtonMappings[0][1]]
if self.freeButtonMappings[1][0] >= 0:
bString = shots.SHOT_NAMES[self.freeButtonMappings[1][0]]
# if ekf is bad or the app is not connected, gray out
if not self.shotMgr.vehicle.ekf_ok or not self.shotMgr.appMgr.isAppConnected():
enabled2 = 0
elif self.freeButtonMappings[1][1] >= 0:
bString = modes.MODE_NAMES[self.freeButtonMappings[1][1]]
self.setArtooButton(btn_msg.ButtonA, self.freeButtonMappings[0][0], enabled1, aString)
self.setArtooButton(btn_msg.ButtonB, self.freeButtonMappings[1][0], enabled2, bString)
else:
self.shotMgr.curController.setButtonMappings()
# only enable the pause button if we're armed
brakeEnabled = self.shotMgr.vehicle.armed
mask = btn_msg.ARTOO_BITMASK_ENABLED if brakeEnabled else 0
self.setArtooButton(btn_msg.ButtonLoiter, shots.APP_SHOT_NONE, mask, "\0")
def setArtooButton(self, button_id, shot, mask, string):
if self.isButtonConnected():
try:
btn_msg.sendArtooString(self.client, button_id, shot, mask, string)
except Exception as e:
logger.log("[button]: %s" % e)
self.disconnect()
# Sends a single string to Artoo that it can display as the current user-facing mode
# This is usually what shot the user is in, but if the user is not in a shot it can be the APM mode
# Pass in the index of the shot and mode
def setArtooShot(self, shot, mode = -1):
if self.isButtonConnected():
try:
if shot == shots.APP_SHOT_NONE and mode >= 0:
btn_msg.sendShotString(self.client, modes.MODE_NAMES[mode].upper())
logger.log("[button]: sending %s to Artoo as mode"%(modes.MODE_NAMES[mode].upper()))
else:
btn_msg.sendShotString(self.client, shots.SHOT_NAMES[shot].upper())
logger.log("[button]: sending %s to Artoo as shot"%(shots.SHOT_NAMES[shot].upper()))
except:
logger.log("[button]: %s" % e)
self.disconnect()
def getFreeButtonMapping(self, button):
# our index is 0 -> Button A, 1 -> Button B
index = button - btn_msg.ButtonA
if index < 0 or index > 1:
logger.log("[button]: Error, someone requested a button mapping for button %d"%(button))
return (-1, -1)
return self.freeButtonMappings[index]
# update one of our free button mappings
def setFreeButtonMapping(self, button, shot, APMmode):
# our index is 0 -> Button A, 1 -> Button B
index = button - btn_msg.ButtonA
if index < 0 or index > 1:
logger.log("[button]: Error, someone tried to map button %d"%(button))
return
if APMmode not in modes.MODE_NAMES.keys():
logger.log("[button]: Error, someone tried to map an invalid mode %d"%(APMmode))
return
if shot not in shots.SHOT_NAMES.keys():
logger.log("[button]: Error, someone tried to map an invalid shot %d"%(shot))
return
self.freeButtonMappings[index] = (shot, APMmode)
self.setButtonMappings()
buttonName = "A" if button == btn_msg.ButtonA else "B"
value = "%d, %d"%(shot, APMmode)
settings.writeSetting(buttonName, value)
def isButtonConnected(self):
return self.connected
def isButtonInited(self):
return self.buttonsInitialized
def handleButtons(self, buttonEvent):
if buttonEvent is None:
return
button, event = buttonEvent
#if button == btn_msg.ButtonPreset1 and event == btn_msg.Press:
# crash
if self.shotMgr.currentShot == shots.APP_SHOT_NONE:
if event == btn_msg.Press:
if button == btn_msg.ButtonA or button == btn_msg.ButtonB:
# see what the button is mapped to
(shot, mode) = self.getFreeButtonMapping(button)
# only allow entry into these shots if the app is attached
if shot in shots.CAN_START_FROM_ARTOO:
logger.log("[button]: Trying shot via Artoo button: %s" % shots.SHOT_NAMES[shot])
self.shotMgr.enterShot(shot)
elif mode >= 0:
self.shotMgr.vehicle.mode = VehicleMode(mavutil.mode_mapping_acm.get(mode))
logger.log("[button]: Requested a mode change via Artoo button to %s." % (modes.MODE_NAMES[mode]))
# check while on release to avoid issues with entering Rewind
if event == btn_msg.Release and button == btn_msg.ButtonLoiter:
self.shotMgr.notifyPause() #trigger brake if not in Loiter
else:
if button == btn_msg.ButtonFly and event == btn_msg.Press:
self.shotMgr.enterShot(shots.APP_SHOT_NONE)
logger.log("[button]: Exited shot via Artoo Fly button.")
else:
self.shotMgr.curController.handleButton(button, event)
if button == btn_msg.ButtonRTL:
if self.shotMgr.currentShot == shots.APP_SHOT_RTL:
self.shotMgr.curController.handleButton(button, event)
elif event == btn_msg.Press:
self.shotMgr.enterShot(shots.APP_SHOT_RTL)
if button == btn_msg.ButtonCameraClick and event == btn_msg.Press:
self.shotMgr.goproManager.handleRecordCommand(self.shotMgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE)
if event == btn_msg.LongHold and button == btn_msg.ButtonLoiter:
self.shotMgr.enterShot(shots.APP_SHOT_REWIND)
# we are holding Pause - dont simply RTL at the end of the rewind spline
if self.shotMgr.currentShot is shots.APP_SHOT_REWIND:
self.shotMgr.curController.exitToRTL = False

450
shotmanager/cableController.py Normal file
View File

@ -0,0 +1,450 @@
# cableController.py
# shotmanager
#
# The cable movement controller.
# Runs as a DroneKit-Python script.
#
# Created by Jon Challinger and Will Silva on 1/21/2015.
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from catmullRom import CatmullRom
from vector3 import *
from numpy import linspace
import math
import threading
import itertools
# epsilon to detect if we've reached a target in meters
TARGET_EPSILON_M = 0.1
# Length of each segment that is assigned a maximum speed based on its maximum curvature
CURVATURE_MAP_RES = 1. # meters
def goldenSection(func, a, b, tol = 1e-5):
gr = 0.61803398875
c = b - gr * (b - a)
d = a + gr * (b - a)
fc = func(c)
fd = func(d)
while abs(c-d) > tol:
if fc < fd:
b = d
d = c
c = b - gr * (b - a)
fd = fc
fc = func(c)
else:
a = c
c = d
d = a + gr * (b - a)
fc = fd
fd = func(d)
return (b+a) / 2.
def constrain(val,minval,maxval):
if val < minval:
return minval
elif val > maxval:
return maxval
return val
class CableController():
def __init__(self, points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt):
# Maximum tangential acceleration along the cable, m/s^2
self.tanAccelLim = tanAccelLim
# Maximum acceleration normal to the cable, m/s^2
self.normAccelLim = normAccelLim
# Smoothness of stops at the endpoints and at targets along the cable
self.smoothStopP = smoothStopP
# Maximum speed along the cable, m/s
self.maxSpeed = maxSpeed
# Minimum speed along the cable, m/s
self.minSpeed = minSpeed
# Minimum allowable position.z, meters (AKA max altitude), Convert Altitude (NEU) to NED
if maxAlt is not None:
self.posZLimit = -maxAlt
else:
self.posZLimit = None
# Input speed
self.desiredSpeed = 0.
# Current speed along the cable, m/s
self.speed = 0.
# Catmull-Rom spline with added virtual tangency control points at either end
self.spline = CatmullRom([points[0]*2 - points[1]]+points+[points[-1]*2 - points[-2]])
# Number of spline segments (should really come from CatmullRom)
self.numSegments = len(points)-1
# Current position in P domain, parameter normalized to cable total arc length
self.currentP = 1.0
# Target position in P domain
self.targetP = self.currentP
# Previously reached target, once set
self.prevReachedTarget = None
# Current segment, ranges from 0 to # of segments-1
self.currentSeg, self.currentU = self.spline.arclengthToNonDimensional(self.currentP)
# Current position as a Vector3, meters
self.position = self.spline.position(self.currentSeg, self.currentU)
# Current velocity as a Vector3, m/s
self.velocity = Vector3()
# Flag to indicate that the maximum altitude has been exceeded
self.maxAltExceeded = False
# Number of segments in curvature map
self.curvatureMapNumSegments = int(math.ceil(self.spline.totalArcLength/CURVATURE_MAP_RES))
# Number of joints in curvature map
self.curvatureMapNumJoints = self.curvatureMapNumSegments+1
# Curvature map joint positions in p domain
self.curvatureMapJointsP, self.curvatureMapSegLengthP = linspace(0., 1., self.curvatureMapNumJoints, retstep = True)
# Curvature map segment length in meters
self.curvatureMapSegLengthM = self.curvatureMapSegLengthP * self.spline.totalArcLength
# Non-dimensional curvature map joint position (cache)
self.curvatureMapJointsNonDimensional = [None for _ in range(self.curvatureMapNumJoints)]
# Speed limits for each curvature map segment (cache)
self.curvatureMapSpeedLimits = [None for _ in range(self.curvatureMapNumSegments)]
# Thread lock on curvature map segments
self.curvatureMapLocks = [threading.Lock() for _ in range(self.curvatureMapNumSegments)]
self.curvatureMapSegmentsComputedLock = threading.Lock()
# number of map segments that have been computed by the curvatureMapThread
self.curvatureMapSegmentsComputed = 0
# flag that indicates to the thread to die
self.poisonPill = False
# setup a worker thread to compute map segment maximum speeds
self.curvatureMapThread = threading.Thread(target=self._computeCurvatureMap)
self.curvatureMapThread.setDaemon(True)
# start the worker thread
self.curvatureMapThread.start()
def __del__(self):
self.poisonPill = True
self.curvatureMapThread.join(timeout = 2)
# Public interface:
def reachedTarget(self):
'''Return True if we've reached the target, else False'''
return abs(self.currentP - self.targetP) * self.spline.totalArcLength < TARGET_EPSILON_M
def setTargetP(self, targetP):
'''Interface to set a target P'''
self.targetP = targetP
def trackSpeed(self, speed):
'''Updates controller desired speed'''
self.desiredSpeed = speed
def update(self, dt):
'''Advances controller along cable by dt'''
# Speed always in direction of target
self.desiredSpeed = math.copysign(self.desiredSpeed, self.targetP - self.currentP)
self.speed = constrain(self._constrainSpeed(self.desiredSpeed), self.speed - self.tanAccelLim*dt, self.speed + self.tanAccelLim*dt)
self._traverse(dt)
def setCurrentP(self,p):
'''Sets the controller's current P position on the cable'''
self.currentP = p
self.currentSeg, self.currentU = self.spline.arclengthToNonDimensional(self.currentP)
def killCurvatureMapThread(self):
'''Sets poisonPill to True so the curvatureMapThread knows to die'''
self.poisonPill = True
# Internal functions:
def _computeCurvatureMap(self):
'''Computes curvature map, prioritizes map construction based on vehicle position and direction of motion'''
while True:
searchStart = self._getCurvatureMapSegment(self.currentP)
if self.speed > 0:
# Search ahead, then behind
for i in range(searchStart, self.curvatureMapNumSegments)+list(reversed(range(0, searchStart))):
if self._computeCurvatureMapSpeedLimit(i):
break
elif self.speed < 0:
# Search behind, then ahead
for i in list(reversed(range(0, searchStart+1)))+range(searchStart+1, self.curvatureMapNumSegments):
if self._computeCurvatureMapSpeedLimit(i):
break
else: # speed == 0
# Search alternately ahead and behind
searchList = [x for t in list(itertools.izip_longest(range(searchStart, self.curvatureMapNumSegments), reversed(range(0, searchStart)))) for x in t if x is not None]
for i in searchList:
if self._computeCurvatureMapSpeedLimit(i):
break
# if all map segments have been computed then quit the thread
with self.curvatureMapSegmentsComputedLock:
if self.curvatureMapSegmentsComputed == self.curvatureMapNumSegments:
self.poisonPill = True
if self.poisonPill:
break
def _computeCurvatureMapSpeedLimit(self, mapSeg):
'''Computes speed limit for the requested map segment'''
with self.curvatureMapLocks[mapSeg]:
# if the speed limit has already been computed for this map segment, then don't do any work
if self.curvatureMapSpeedLimits[mapSeg] is not None:
return False
# if non-dimensional parameter has not yet been created for the associated left joint, then create it
if self.curvatureMapJointsNonDimensional[mapSeg] is None:
self.curvatureMapJointsNonDimensional[mapSeg] = self.spline.arclengthToNonDimensional(self.curvatureMapJointsP[mapSeg])
# if non-dimensional parameter has not yet been created for the associated right joint, then create it
if self.curvatureMapJointsNonDimensional[mapSeg+1] is None:
self.curvatureMapJointsNonDimensional[mapSeg+1] = self.spline.arclengthToNonDimensional(self.curvatureMapJointsP[mapSeg+1])
# split returned non-dimensional parameter tuple (seg,u) into separate values
seg1, u1 = self.curvatureMapJointsNonDimensional[mapSeg]
seg2, u2 = self.curvatureMapJointsNonDimensional[mapSeg+1]
# returns arc length for current spline segment, or the larger of the two segments if our map segment spans across multiple spline segments
maxSegLen = max(self.spline.arcLengths[seg1:seg2+1]) # m
# run a golden section search to find the segment,u pair for the point of maximum curvature in the requested map segment
# (segment,u) are stored as segment+u, e.g. segment 1, u = 0.25 -> 1.25
maxCurvatureSegU = goldenSection(lambda x: -self.spline.curvature(int(x), x-int(x)), seg1+u1, seg2+u2, tol = 1e-1/maxSegLen)
# run a golden section search to find the segment,u pair for the point of minimum Z (aka max altitude)
minPosZSegU = goldenSection(lambda x: self.spline.position(int(x), x-int(x)).z, seg1+u1, seg2+u2, tol = 1e-1/maxSegLen)
# split segment+u into segment,u and evaluate curvature at this point
maxCurvature = self.spline.curvature(int(maxCurvatureSegU),maxCurvatureSegU-int(maxCurvatureSegU))
#split segment+u into segment,u and evalute position.z at this point
minPosZ = self.spline.position(int(minPosZSegU),minPosZSegU-int(minPosZSegU)).z #m
# this prevents the copter from traversing segments of the cable
# that are above its altitude limit
if self.posZLimit is not None and minPosZ < self.posZLimit:
self.maxAltExceeded = True
#this cable will breach the altitude limit, make the speed limit for this segment 0 to stop the vehicle
self.curvatureMapSpeedLimits[mapSeg] = 0.
else:
if maxCurvature != 0.:
# limit maxspeed by the max allowable normal acceleration at that point, bounded on the lower end by minSpeed
self.curvatureMapSpeedLimits[mapSeg] = max(math.sqrt(self.normAccelLim / maxCurvature), self.minSpeed)
else:
# if curvature is zero, means a straight segment
self.curvatureMapSpeedLimits[mapSeg] = self.maxSpeed
with self.curvatureMapSegmentsComputedLock:
self.curvatureMapSegmentsComputed += 1
return True
def _getCurvatureMapSpeedLimit(self, mapSeg):
'''Look up the speed limit for the requested map segment'''
# sanitize mapSeg
if mapSeg < 0 or mapSeg >= self.curvatureMapNumSegments:
return 0.
self._computeCurvatureMapSpeedLimit(mapSeg)
return self.curvatureMapSpeedLimits[mapSeg]
def _traverse(self, dt):
''' Advances the controller along the spline '''
spline_vel_unit = self.spline.velocity(self.currentSeg, self.currentU)
spline_vel_norm = spline_vel_unit.normalize()
# advances u by the amount specified by our speed and dt
self.currentU += self.speed * dt / spline_vel_norm
# handle traversing spline segments
if self.currentU > 1.:
if self.currentSeg < self.numSegments-1:
self.currentSeg += 1
self.currentU = 0. # NOTE: this truncates steps which cross spline joints
else:
self.currentU = 1.
elif self.currentU < 0.:
if self.currentSeg > 0:
self.currentSeg -= 1
self.currentU = 1. # NOTE: this truncates steps which cross spline joints
else:
self.currentU = 0.
# calculate our currentP
self.currentP = self.spline.nonDimensionalToArclength(self.currentSeg, self.currentU)[0]
# calculate our position and velocity commands
self.position = self.spline.position(self.currentSeg, self.currentU)
self.velocity = spline_vel_unit * self.speed
def _constrainSpeed(self, speed):
'''Looks ahead and behind current controller position and constrains to a speed limit'''
if speed > 0:
return min(self.maxSpeed, speed, self._getPosSpeedLimit(self.currentP))
elif speed < 0:
return max(-self.maxSpeed, speed, self._getNegSpeedLimit(self.currentP))
return speed
def _speedCurve(self, dist, speed):
'''Returns speed based on the sqrt function or a linear ramp (depending on dist)'''
linear_velocity = self.tanAccelLim / self.smoothStopP
linear_dist = linear_velocity / self.smoothStopP
if speed > linear_velocity:
return math.sqrt(2. * self.tanAccelLim * (speed**2/(2.*self.tanAccelLim) + dist))
else:
p1 = speed / self.smoothStopP
p2 = p1 + dist
if p2 > linear_dist:
return math.sqrt(2. * self.tanAccelLim * (p2 - 0.5*linear_dist))
else:
return p2 * self.smoothStopP
def _maxLookAheadDist(self):
'''Calculate how far it would take to come to a complete stop '''
linear_velocity = self.tanAccelLim / self.smoothStopP
linear_dist = linear_velocity / self.smoothStopP
if abs(self.speed) > linear_velocity:
return 0.5 * abs(self.speed)**2 / self.tanAccelLim + 0.5*linear_dist
else:
return abs(self.speed)/self.smoothStopP
def _getCurvatureMapSegment(self, p):
'''Get the curvature map segment index at the location p'''
return int(min(math.floor(p / self.curvatureMapSegLengthP),self.curvatureMapNumSegments-1))
def _getDistToCurvatureMapSegmentBegin(self, p1, idx):
'''Get distance from p1 to the beginning of the idx curvature map segment in meters'''
p2 = self.curvatureMapJointsP[idx]
return abs(p1-p2) * self.spline.totalArcLength
def _getDistToCurvatureMapSegmentEnd(self, p1, idx):
'''Get distance from p1 to the end of the idx curvature map segment in meters'''
p2 = self.curvatureMapJointsP[idx+1]
return abs(p1-p2) * self.spline.totalArcLength
def _getPosSpeedLimit(self, p):
'''Returns speed limit for a requested arc length normalized parameter, p, moving in the positive direction'''
# Identify our current curvature map segment
mapSeg = self._getCurvatureMapSegment(p)
# get speed limit for the upcoming curvature map segment
nextMapSegSpeed = self._getCurvatureMapSpeedLimit(mapSeg+1)
# get distance (in meters) from current position to start of next curvature map segment
nextMapSegDist = self._getDistToCurvatureMapSegmentEnd(p, mapSeg)
# set speed limit to the minimum of the current curvature map segment and the transition to the next curvature map segment speed
speedLimit = min(self._getCurvatureMapSpeedLimit(mapSeg), self._speedCurve(nextMapSegDist, nextMapSegSpeed)) # m/s
# loop through all remaining segments in that direction
for mapSeg in range(mapSeg+1,self.curvatureMapNumSegments):
# increment distance by another curvature map segment length
nextMapSegDist += self.curvatureMapSegLengthM
# if that distance is greater than the distance it would take to stop, then break to save time (no need to look ahead any further)
if nextMapSegDist > self._maxLookAheadDist():
break
# get curvature map seg speed at this next segment
nextMapSegSpeed = self._getCurvatureMapSpeedLimit(mapSeg+1) # NOTE: self.getCurvatureMapSpeedLimit(self.curvatureMapNumSegments) is 0
# limit us if the new map segment speed is slower than our current speed limit
speedLimit = min(speedLimit, self._speedCurve(nextMapSegDist, nextMapSegSpeed))
# if targetP is ahead of currentP then check for a speed limit to slow down at the target
if self.targetP >= self.currentP:
speedLimit = min(speedLimit, self._speedCurve(abs(self.targetP - self.currentP)*self.spline.totalArcLength, 0))
return speedLimit
def _getNegSpeedLimit(self, p):
'''Returns speed limit for a requested arc length normalized parameter, p, moving in the negative direction'''
# Identify our current curvature map segment
mapSeg = self._getCurvatureMapSegment(p)
# get speed limit for the previous curvature map segment
prevMapSegSpeed = self._getCurvatureMapSpeedLimit(mapSeg-1)
# get distance (in meters) from current position to start of previous curvature map segment
prevMapSegDist = self._getDistToCurvatureMapSegmentBegin(p, mapSeg)
# set speed limit to the minimum of the current curvature map segment and the transition to the previous curvature map segment speed
speedLimit = min(self._getCurvatureMapSpeedLimit(mapSeg), self._speedCurve(prevMapSegDist, prevMapSegSpeed)) # m/s
# loop through all remaining segments in that direction
for mapSeg in reversed(range(0,mapSeg)):
# increment distance by another curvature map segment length
prevMapSegDist += self.curvatureMapSegLengthM
# if that distance is greater than the distance it would take to stop, then break to save time (no need to look ahead any further)
if prevMapSegDist > self._maxLookAheadDist():
break
# get curvature map seg speed at this previous segment
prevMapSegSpeed = self._getCurvatureMapSpeedLimit(mapSeg-1) # NOTE: self.getCurvatureMapSpeedLimit(-1) is 0
# limit us if the new map segment speed is slower than our current speed limit
speedLimit = min(speedLimit, self._speedCurve(prevMapSegDist, prevMapSegSpeed))
if self.targetP <= self.currentP:
speedLimit = min(speedLimit, self._speedCurve(abs(self.targetP - self.currentP)*self.spline.totalArcLength, 0))
return -speedLimit

424
shotmanager/cable_cam.py Normal file
View File

@ -0,0 +1,424 @@
#
# Implement cable cam using DroneAPI
# This is called from shotManager running in MavProxy
# As of Solo version 2.0.0 this shot is no longer supported. It is replaced with multipoint.py.
# It is preserved here for compatibility with old versions of the app.
#
from dronekit.lib import VehicleMode
from dronekit.lib import LocationGlobal
from pymavlink import mavutil
import os
from os import sys, path
import math
import struct
sys.path.append(os.path.realpath(''))
import app_packet
import camera
import location_helpers
import pathHandler
import shotLogger
import shots
from shotManagerConstants import *
import yawPitchOffsetter
# on host systems these files are located here
sys.path.append(os.path.realpath('../../flightcode/stm32'))
from sololink import btn_msg
# in degrees per second
YAW_SPEED = 120.0
# cm / second
DEFAULT_WPNAV_ACCEL_VALUE = 100.0
logger = shotLogger.logger
class Waypoint():
def __init__(self, loc, yaw, pitch):
self.loc = loc
self.yaw = yaw
self.pitch = pitch
if self.pitch is None:
self.pitch = 0.0
class CableCamShot():
def __init__(self, vehicle, shotmgr):
self.vehicle = vehicle
self.shotmgr = shotmgr
self.waypoints = []
self.totalDistance = 0.0
self.yawPitchOffsetter = yawPitchOffsetter.YawPitchOffsetter()
self.pathHandler = None
# this is the total yaw between the recording of point 1 and point 2
self.aggregateYaw = 0.0
self.lastYaw = 0.0
# Cable cam options - set from the app
# cam interpolation is whether to do camera interpolation or not
self.camInterpolation = 1
# yaw direction is which way direction to turn while on the cable
# (from endpoint 0 to 1)
# This is automatically set by seeing how the user yaws while flying
# but it can be overridden by the app
self.yawDirection = 0
# use this to perform dead reckoning
self.lastPerc = 0.0
self.deadReckoningTicks = 0
self.accel = shotmgr.getParam( "WPNAV_ACCEL", DEFAULT_WPNAV_ACCEL_VALUE ) / 100.0
logger.log("[cable cam]: Retrieved WPNAV_ACCEL: %f" % (self.accel * 100.0))
self.desiredSpeed = 0.0
# set this to True once our targeting is set up
self.haveSetupTargeting = False
# channels are expected to be floating point values in the (-1.0, 1.0) range
def handleRCs( self, channels ):
if len(self.waypoints) < 2:
# if we've already recorded one waypoint, start
# trying to remember the intended rotation direction here
currentYaw = camera.getYaw(self.vehicle)
yawDiff = currentYaw - self.lastYaw
if yawDiff > 180.0:
yawDiff -= 360.0
elif yawDiff < -180.0:
yawDiff += 360.0
self.aggregateYaw += yawDiff
self.lastYaw = currentYaw
return
self.desiredSpeed, goingForwards = self.pathHandler.MoveTowardsEndpt(channels)
# the moment we enter guided for the first time, setup our targeting
# For some as yet unknown reason, this still doesn't work unless we call
# InterpolateCamera a few times, which is the reason for the
# not self.haveSetupTargeting check below
if not self.haveSetupTargeting and self.vehicle.mode.name == "GUIDED" and self.pathHandler.curTarget:
self.setupTargeting()
self.haveSetupTargeting = True
logger.log("[cable cam]: did initial setup of targeting")
self.yawPitchOffsetter.Update(channels)
if self.camInterpolation > 0 or not self.haveSetupTargeting:
# because we flipped cable cam around, we actually need to flip goingForwards
self.InterpolateCamera(not goingForwards)
else:
self.handleFreePitchYaw()
if self.pathHandler.isNearTarget():
self.pathHandler.currentSpeed = 0.0
self.pathHandler.pause()
self.updateAppOptions()
def recordLocation(self):
degreesYaw = camera.getYaw(self.vehicle)
pitch = camera.getPitch(self.vehicle)
# don't allow two waypoints near each other
if len(self.waypoints) == 1:
if location_helpers.getDistanceFromPoints3d(self.waypoints[0].loc, self.vehicle.location.global_relative_frame) < WAYPOINT_NEARNESS_THRESHOLD:
logger.log("[cable cam]: attempted to record a point too near the first point")
# update our waypoint in case yaw, pitch has changed
self.waypoints[0].yaw = degreesYaw
self.waypoints[0].pitch = pitch
# force the app to exit and restart cable cam
packet = struct.pack('<IIi', app_packet.SOLO_MESSAGE_GET_CURRENT_SHOT, 4, shots.APP_SHOT_NONE)
self.shotmgr.appMgr.sendPacket(packet)
packet = struct.pack('<IIi', app_packet.SOLO_MESSAGE_GET_CURRENT_SHOT, 4, shots.APP_SHOT_CABLECAM)
self.shotmgr.appMgr.sendPacket(packet)
packet = struct.pack('<IIddfff', app_packet.SOLO_CABLE_CAM_WAYPOINT,
28, self.vehicle.location.global_relative_frame.lat, self.vehicle.location.global_relative_frame.lon,
self.vehicle.location.global_relative_frame.alt, degreesYaw, pitch)
self.shotmgr.appMgr.sendPacket(packet)
return
# create a waypoint and add it to our list
waypt = Waypoint(self.vehicle.location.global_relative_frame, degreesYaw, pitch)
logger.log("[cable cam]: Recorded location %f, %f, %f. Yaw = %f" %
( self.vehicle.location.global_relative_frame.lat, self.vehicle.location.global_relative_frame.lon,
self.vehicle.location.global_relative_frame.alt, degreesYaw))
logger.log("[cable cam]: gimbal pitch is " + str(pitch))
self.waypoints.append(waypt)
self.setButtonMappings()
# send this waypoint to the app
packet = struct.pack('<IIddfff', app_packet.SOLO_CABLE_CAM_WAYPOINT,
28, self.vehicle.location.global_relative_frame.lat, self.vehicle.location.global_relative_frame.lon,
self.vehicle.location.global_relative_frame.alt, degreesYaw, pitch)
self.shotmgr.appMgr.sendPacket(packet)
#start monitoring heading changes
if len(self.waypoints) == 1:
self.aggregateYaw = 0.0
self.lastYaw = degreesYaw
elif len(self.waypoints) == 2:
# Now change the vehicle into guided mode
self.vehicle.mode = VehicleMode("GUIDED")
logger.log("[cable cam]: Got second cable point. Should be in guided %s" % (str(self.vehicle.mode)))
self.totalDistance = location_helpers.getDistanceFromPoints3d(
self.waypoints[0].loc, self.waypoints[1].loc)
# handle the 0-360 border
if self.waypoints[1].yaw - self.waypoints[0].yaw > 180.0:
self.waypoints[0].yaw += 360.0
elif self.waypoints[1].yaw - self.waypoints[0].yaw < -180.0:
self.waypoints[1].yaw += 360.0
#disregard aggregate yaw if it's less than 180
if abs(self.aggregateYaw) < 180.0:
self.aggregateYaw = self.waypoints[1].yaw - self.waypoints[0].yaw
self.yawDirection = 1 if self.aggregateYaw > 0.0 else 0
logger.log("[cable cam]: Aggregate yaw = %f. Yaw direction saved as %s" % (self.aggregateYaw, "CCW" if self.yawDirection == 1 else "CW"))
# send this yawDir to the app
self.updateAppOptions()
self.pathHandler = pathHandler.TwoPointPathHandler( self.waypoints[1].loc, self.waypoints[0].loc, self.vehicle, self.shotmgr )
def InterpolateCamera(self, goingForwards):
# Current request is to interpolate yaw and gimbal pitch separately
# This can result in some unsmooth motion, but maybe it's good enough.
# Alternatives would include slerp, but we're not interested in the shortest path.
# first, see how far along the path we are
dist = location_helpers.getDistanceFromPoints3d(self.waypoints[0].loc, self.vehicle.location.global_relative_frame)
if self.totalDistance == 0.0:
perc = 0.0
else:
perc = dist / self.totalDistance
# offset perc using dead reckoning
if self.lastPerc == perc:
self.deadReckoningTicks += 1
# adjust our last seen velocity based on WPNAV_ACCEL
DRspeed = self.vehicle.groundspeed
timeElapsed = self.deadReckoningTicks * UPDATE_TIME
# one problem here is we don't simulate deceleration when reaching the endpoints
if self.desiredSpeed > DRspeed:
DRspeed += (timeElapsed * self.accel)
if DRspeed > self.desiredSpeed:
DRspeed = self.desiredSpeed
else:
DRspeed -= (timeElapsed * self.accel)
if DRspeed < self.desiredSpeed:
DRspeed = self.desiredSpeed
if self.totalDistance > 0.0:
percSpeed = DRspeed * timeElapsed / self.totalDistance
#logger.log("same location, dead reckoning with groundspeed: %f"%(DRspeed))
# and adjust perc
if goingForwards:
perc += percSpeed
else:
perc -= percSpeed
else:
self.deadReckoningTicks = 0
self.lastPerc = perc
# logger.log("NEW location, resetting dead reckoning. Groundspeed: %f"%(self.vehicle.groundspeed))
if perc < 0.0:
perc = 0.0
elif perc > 1.0:
perc = 1.0
invPerc = 1.0 - perc
yawPt0 = self.waypoints[0].yaw
yawPt1 = self.waypoints[1].yaw
# handle if we're going other than the shortest dir
if yawPt1 > yawPt0 and self.yawDirection == 0:
yawPt0 += 360.0
elif yawPt0 > yawPt1 and self.yawDirection == 1:
yawPt1 += 360.0
# interpolate vehicle yaw and gimbal pitch
newYaw = perc * yawPt1 + invPerc * yawPt0
# add in yaw offset
newYaw += self.yawPitchOffsetter.yawOffset
# since we possibly added 360 to either waypoint, correct for it here
newYaw %= 360.0
# print "point 0 yaw: " + str(self.waypoints[0].yaw) + " point 1 yaw: " + str(self.waypoints[1].yaw) + " interpolated yaw = " + str(newYaw) + " at position " + str(perc)
newPitch = perc * self.waypoints[1].pitch + invPerc * self.waypoints[0].pitch
newPitch += self.yawPitchOffsetter.pitchOffset
if newPitch >= 0:
newPitch = 0
elif newPitch <= -90:
newPitch = -90
# if we do have a gimbal, mount_control it
if self.vehicle.mount_status[0] is not None:
msg = self.vehicle.message_factory.mount_control_encode(
0, 1, # target system, target component
newPitch * 100, # pitch is in centidegrees
0.0, # roll
newYaw * 100, # yaw is in centidegrees
0 # save position
)
else:
# if we don't have a gimbal, just set CONDITION_YAW
msg = 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
YAW_SPEED, # param 2 - yaw speed
self.yawDirection, # param 3 - direction
0.0, # relative offset
0, 0, 0 # params 5-7 (unused)
)
self.vehicle.send_mavlink(msg)
# if we can handle the button we do
def handleButton(self, button, event):
if button == btn_msg.ButtonA and event == btn_msg.Press:
if len(self.waypoints) == 0:
self.recordLocation()
if button == btn_msg.ButtonB and event == btn_msg.Press:
if len(self.waypoints) == 1:
self.recordLocation()
if button == btn_msg.ButtonLoiter and event == btn_msg.Press:
if self.pathHandler:
self.shotmgr.notifyPause(True)
self.pathHandler.togglePause()
self.updateAppOptions()
else:
# notify autopilot of pause press (technically not in shot)
self.shotmgr.notifyPause(False)
# pass in the value portion of the SOLO_CABLE_CAM_OPTIONS packet
def handleOptions(self, options):
oldInterp = self.camInterpolation
(self.camInterpolation, self.yawDirection, cruiseSpeed) = options
logger.log( "[cable cam]: Set cable cam options to interpolation: %d, \
yawDir: %d, cruising speed %f"%(self.camInterpolation, self.yawDirection, cruiseSpeed))
# don't handle these if we're not ready
if not self.pathHandler:
return
# pause or set new cruise speed
if cruiseSpeed == 0.0:
self.pathHandler.pause()
else:
self.pathHandler.setCruiseSpeed(cruiseSpeed)
# change to a different targeting mode
if oldInterp != self.camInterpolation:
self.setupTargeting()
def setButtonMappings(self):
buttonMgr = self.shotmgr.buttonManager
if len( self.waypoints ) == 0:
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_CABLECAM, btn_msg.ARTOO_BITMASK_ENABLED, "Record Point\0")
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_CABLECAM, 0, "\0")
elif len( self.waypoints ) == 1:
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_CABLECAM, 0, "\0")
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_CABLECAM, btn_msg.ARTOO_BITMASK_ENABLED, "Record Point\0")
else:
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_CABLECAM, 0, "\0")
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_CABLECAM, 0, "\0")
# send our current set of options to the app
def updateAppOptions(self):
speed = 0.0
if self.pathHandler:
speed = self.pathHandler.cruiseSpeed
packet = struct.pack('<IIHHf', app_packet.SOLO_CABLE_CAM_OPTIONS,
8, self.camInterpolation, self.yawDirection, speed)
self.shotmgr.appMgr.sendPacket(packet)
def setupTargeting(self):
# set gimbal targeting mode
msg = self.vehicle.message_factory.mount_configure_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, #mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
self.shotmgr.rcMgr.enableRemapping( True )
logger.log("[cable cam]: setting gimbal to mavlink mode")
if self.camInterpolation > 0:
logger.log("[cable cam]: turning on camera interpolation")
self.yawPitchOffsetter.enableNudge()
else:
logger.log("[cable cam]: turning off camera interpolation")
self.yawPitchOffsetter.disableNudge( camera.getPitch(self.vehicle), camera.getYaw(self.vehicle))
self.vehicle.send_mavlink(msg)
"""
In the case that we don't have view lock on, we enable 'free' movement of
pitch/yaw. Previously, we allowed flight code to control yaw, but since
we want the throttle stick to control pitch, we need to handle it here.
Our yawPitchOffsetter will contain the absolute pitch/yaw we want, so
this function just needs to set them.
"""
def handleFreePitchYaw(self):
# if we do have a gimbal, use mount_control to set pitch and yaw
if self.vehicle.mount_status[0] is not None:
msg = self.vehicle.message_factory.mount_control_encode(
0, 1, # target system, target component
self.yawPitchOffsetter.pitchOffset * 100, # pitch is in centidegrees
0.0, # roll
self.yawPitchOffsetter.yawOffset * 100, # yaw is in centidegrees
0 # save position
)
else:
# if we don't have a gimbal, just set CONDITION_YAW
msg = self.vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
0, #confirmation
self.yawPitchOffsetter.yawOffset, # param 1 - target angle
YAW_SPEED, # param 2 - yaw speed
self.yawPitchOffsetter.yawDir, # param 3 - direction
0.0, # relative offset
0, 0, 0 # params 5-7 (unused)
)
self.vehicle.send_mavlink(msg)
def handlePacket(self, packetType, packetLength, packetValue):
try:
if packetType == app_packet.SOLO_RECORD_POSITION:
self.recordLocation()
elif packetType == app_packet.SOLO_CABLE_CAM_OPTIONS:
options = struct.unpack('<HHf', packetValue)
self.handleOptions(options)
else:
return False
except Exception as e:
logger.log('[cable cam]: Error handling packet. (%s)' % e)
return False
else:
return True

26
shotmanager/camera.py Normal file
View File

@ -0,0 +1,26 @@
# functions to retrieve camera pitch and yaw
# These are factored out so we can handle things like
# whether we have a gimbal or not in one location
import math
def getYaw( vehicle ):
# do we have a gimbal? If so, get the yaw from it
# can't do until we have pointing working on our gimbal
"""
For now, gimbal yaw reporting is not the way to go. We will use vehicle yaw reporting in all cases
if vehicle.mount_status[1] != None:
return vehicle.mount_status[1]
else:
return math.degrees( vehicle.attitude.yaw )
"""
return math.degrees( vehicle.attitude.yaw )
def getPitch( vehicle ):
if vehicle.mount_status[0]:
return vehicle.mount_status[0]
else:
return 0.0

257
shotmanager/catmullRom.py Normal file
View File

@ -0,0 +1,257 @@
#
# catmullRom.py
# shotmanager
#
# A Catmull-Rom spline class.
#
# Created by Will Silva on 11/22/2015.
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from vector3 import Vector3
TOL = 1.0e-3
class CatmullRom:
# Derivation: https://en.wikipedia.org/wiki/Centripetal_Catmull%E2%80%93Rom_spline
# Class inspiration:
# https://code.google.com/p/gamekernel/source/browse/kgraphics/math/CatmullRom.cpp
def __init__(self, Pts):
# internal point storage
self.points = Pts
# make sure we have enough points
if len(Pts) < 4:
raise ValueError(
'Not enough points provided to generate a spline.')
# calculate total length of the spline
self.splineCoefficients = []
self.arcLengths = []
self.totalArcLength = 0.0
for seg in range(0, len(self.points) - 3):
self.splineCoefficients.append(self.updateSplineCoefficients(seg))
self.arcLengths.append(self.arcLength(seg, 0, 1))
self.totalArcLength = sum(self.arcLengths)
def updateSplineCoefficients(self, seg):
if seg < 0 or seg > len(self.points) - 4:
raise ValueError(
'Invalid segment number received (%d). Check the number of spline control points.' % seg)
P0 = self.points[seg]
P1 = self.points[seg + 1]
P2 = self.points[seg + 2]
P3 = self.points[seg + 3]
A = 3.0 * P1 \
- P0 \
- 3.0 * P2 \
+ P3
B = 2.0 * P0 \
- 5.0 * P1 \
+ 4.0 * P2 \
- P3
C = P2 - P0
return P0,P1,P2,P3,A,B,C
def position(self, seg, u):
'''Returns x,y,z position of spline at parameter u'''
P0,P1,P2,P3,A,B,C = self.splineCoefficients[seg]
return P1 + (0.5 * u) * (C + u * (B + u * A))
def velocity(self, seg, u):
'''Returns x,y,z velocity of spline at parameter u'''
P0,P1,P2,P3,A,B,C = self.splineCoefficients[seg]
return (0.5 * C + u * (B + 1.5 * u * A))
def acceleration(self, seg, u):
'''Returns x,y,z acceleration of spline at parameter u'''
P0,P1,P2,P3,A,B,C = self.splineCoefficients[seg]
return B + (3.0 * u) * A
def curvature(self, seg, u):
'''Returns Frenet curvature of spline at parameter u'''
# https://en.wikipedia.org/wiki/Frenet-Serret_formulas
vel = self.velocity(seg,u)
return Vector3.cross(vel, self.acceleration(seg, u)).length() / (vel.length())**3
def arcLength(self, seg, u1, u2):
'''Calculates arc length between u1 and u2 using Gaussian Quadrature'''
# sanitize u1,u2
if u2 <= u1 or u1 > 1.0 or u2 < 0.0:
return 0.0
if u1 < 0.0:
u1 = 0.0
if u2 > 1.0:
u2 = 1.0
# Gaussian Quadrature weights and abscissae for n = 5
abscissae = [
0.0000000000, 0.5384693101, -0.5384693101, 0.9061798459, -0.9061798459]
weights = [0.5688888889, 0.4786286705,
0.4786286705, 0.2369268850, 0.2369268850]
# Gaussian Quadrature
length = 0.0
for j in range(0, 5):
u = 0.5 * ((u2 - u1) * abscissae[j] + u2 + u1)
length += weights[j] * self.velocity(seg, u).length()
length *= 0.5 * (u2 - u1)
return length
def findParameterByDistance(self, seg, u1, s, newtonIterations = 32):
'''Returns a parameter u that is s meters ahead of u1'''
'''From CatmullRom.cpp: This extends the approach in the text and uses a mixture of bisection and
Newton-Raphson to find the root. The result is more stable than Newton-
Raphson alone because a) we won't end up with a situation where we divide by
zero in the Newton-Raphson step and b) the end result converges faster.
See Numerical Recipes or http://www.essentialmath.com/blog for more details.'''
# if desired arclength is beyond the end of the current segment then
# just return end of segment
if s >= self.arcLength(seg, u1, 1.0):
return 1.0
# can't do negative distances
if s <= 0.0:
return u1
a = u1
b = 1.0
# make first guess
p = u1 + s * (1 / self.arcLengths[seg])
# iterate and look for zeros
for i in range(0, newtonIterations):
# compute function value and test against zero
func = self.arcLength(seg, u1, p) - s
# if within tolerance, return root
if abs(func) < TOL:
return p
# if result less than zero then adjust lower limit
if func < 0.0:
a = p
# if result greater than zero then adjust upper limit
else:
b = p
# Use speed to do a bisection method (will accelerate convergence)
# get speed along the curve
speed = self.velocity(seg, p).length()
# if result lies outside [a,b]
if ((p - a) * speed - func) * ((p - b) * speed - func) > -TOL:
# do bisection
p = 0.5 * (a + b)
else:
p -= func / speed
return float("inf")
def arclengthToNonDimensional(self, p, dp=None):
'''
inputs:
p - spline position reparameterized to total spline arcLength
dp - spline velocity reparameterized to total spline arclength
returns:
seg - current spline segment
u - current non-dimensional spline parameter on segment "seg"
v - velocity along spline in meters per second
'''
# sanitize p
if p > 1:
p = 1
elif p < 0:
p = 0
# calculate target arc length
targetArcLength = p * self.totalArcLength
# find out what segment that's in
for seg in range(1, len(self.arcLengths) + 1):
if sum(self.arcLengths[0:seg]) > targetArcLength:
break
# increment down one
seg -= 1
# calculate distance in that segment
dist = targetArcLength - sum(self.arcLengths[0:seg])
# calculate dist ahead of u = 0 on seg
u = self.findParameterByDistance(seg, 0, dist)
if dp is not None:
v = dp * self.totalArcLength
return (seg, u, v)
else:
return (seg, u)
def nonDimensionalToArclength(self, seg, u, v=None):
'''
inputs:
seg - current spline segment
u - current non-dimensional spline parameter on segment "seg"
v - velocity along spline in meters per second
returns:
p - spline position reparameterized to total spline arcLength
dp - spline velocity reparameterized to total spline arcLength
'''
# sanitize seg
if seg < 0:
seg = 0
elif seg > len(self.points) - 4:
seg = len(self.points) - 4
# sanitize u
if u < 0:
u = 0
elif u > 1:
u = 1
# calculate distance along in segment
dist = self.arcLength(seg, 0, u)
# add all previous segment distances
dist += sum(self.arcLengths[0:seg])
# convert to arclength parameter
p = dist / self.totalArcLength
if v is not None:
dp = v / self.totalArcLength
return (p, dp)
else:
return (p,)

44
shotmanager/config/shotmanager.orig Normal file
View File

@ -0,0 +1,44 @@
[shotManager]
A=2, -1
B=1, -1
GoProEnabled=1
[loggers]
keys=shot,root
[handlers]
keys=sysLog2Handler,consoleHandler
[formatters]
keys=syslogFormatter,simpleFormatter
[logger_shot]
level=INFO
handlers=sysLog2Handler
qualname=shot
propagate=0
[handler_sysLog2Handler]
class=handlers.SysLogHandler
level=DEBUG
formatter=syslogFormatter
args=("/dev/log", handlers.SysLogHandler.LOG_LOCAL2)
[formatter_syslogFormatter]
format=%(name)s: %(message)s
datefmt=
[logger_root]
level=INFO
handlers=consoleHandler
[handler_consoleHandler]
class=StreamHandler
level=ERROR
formatter=simpleFormatter
args=(sys.stdout,)
[formatter_simpleFormatter]
format=%(asctime)s %(name)-4s %(levelname)-8s %(message)s
datefmt=

1934
shotmanager/docs/App-ShotManager communication.md Normal file
View File

File diff suppressed because it is too large Load Diff

167
shotmanager/flyController.py Normal file
View File

@ -0,0 +1,167 @@
# flyController.py
# shotmanager
#
# The fly movement controller.
# Runs as a DroneKit-Python script.
#
# Created by Will Silva on 1/22/2015.
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from pymavlink import mavutil
import location_helpers
import shotLogger
from shotManagerConstants import *
import math
from vector3 import Vector3
logger = shotLogger.logger
#Path accel/decel constants
PATH_ACCEL = 3
ACCEL_PER_TICK = PATH_ACCEL * UPDATE_TIME
class FlyController():
'''A rectangular-coordinates based motion controller than be permuted by user-sticks (operates similarly to Solo's FLY mode)'''
def __init__(self, origin, xOffset, yOffset, zOffset, heading):
'''Initialize the controller at a rectangular coordinate relative to an origin origin
NEU frame (x-North,y-East,z-Up)'''
self.origin = origin
self.offset = Vector3(xOffset, yOffset, zOffset)
self.heading = heading * math.pi/180.
self.approachSpeed = 0.0
self.strafeSpeed = 0.0
self.climbSpeed = 0.0
# options attributes
self.maxAlt = None
self.maxClimbRate = None
self.maxSpeed = MAX_SPEED
def setOptions(self, maxClimbRate, maxAlt):
'''Sets maximum limits in open-loop controller'''
self.maxClimbRate = maxClimbRate
self.maxAlt = maxAlt
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 _approach(self,stick):
'''Handles approach stick input to move forwards or backwards'''
# get max approach speed based on current strafe speed
maxSpeed = self._maxApproachSpeed(self.strafeSpeed)
# Approach speed is controlled by user PITCH stick
speed = stick * maxSpeed
# Synthetic acceleration limit
if speed > self.approachSpeed:
self.approachSpeed += ACCEL_PER_TICK
self.approachSpeed = min(self.approachSpeed, speed)
elif speed < self.approachSpeed:
self.approachSpeed -= ACCEL_PER_TICK
self.approachSpeed = max(self.approachSpeed, speed)
# rotate vehicle into NEU frame
xVel = math.cos(self.heading) * self.approachSpeed
yVel = math.sin(self.heading) * self.approachSpeed
return Vector3(xVel,yVel,0)
def _strafe(self, stick):
'''Handles strafe stick input to move left or right'''
# get max strafe speed
maxSpeed = self._maxStrafeSpeed(self.approachSpeed)
# Strafe speed is controlled by user ROLL stick
speed = stick * maxSpeed
# Synthetic acceleration limit
if speed > self.strafeSpeed:
self.strafeSpeed += ACCEL_PER_TICK
self.strafeSpeed = min(self.strafeSpeed, speed)
elif speed < self.strafeSpeed:
self.strafeSpeed -= ACCEL_PER_TICK
self.strafeSpeed = max(self.strafeSpeed, speed)
# rotate vehicle into NEU frame
xVel = math.cos(self.heading+math.pi/2.) * self.strafeSpeed
yVel = math.sin(self.heading+math.pi/2.) * self.strafeSpeed
return Vector3(xVel,yVel,0)
def _climb(self, stick):
'''Handles altitude stick input to increase or decrease altitude'''
# calculate z vel
zVel = stick * self.maxClimbRate
return Vector3(0,0,zVel)
def _maxApproachSpeed(self, strafeSpeed):
'''Returns maximum approach speed as a function of strafe speed'''
maxSpeed = math.sqrt(self.maxSpeed**2 - strafeSpeed**2)
return maxSpeed
def _maxStrafeSpeed(self, approachSpeed):
'''Returns maximum strafe speed as a function of approach speed'''
maxSpeed = math.sqrt(self.maxSpeed**2 - approachSpeed**2)
return maxSpeed

675
shotmanager/follow.py Normal file
View File

@ -0,0 +1,675 @@
# follow.py
# shotmanager
#
# The Follow Me smart shot controller.
# Runs as a DroneKit-Python script.
#
# Created by Will Silva on 12/14/2015.
# Altitude and Leash follow created by Jason Short 2/25/2016
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from dronekit import Vehicle, LocationGlobalRelative, VehicleMode
from pymavlink import mavutil
import os
from os import sys, path
import math
import struct
import monotonic
import collections
sys.path.append(os.path.realpath(''))
import app_packet
import camera
import location_helpers
import pathHandler
import shotLogger
from shotManagerConstants import *
import shots
import socket
from orbitController import OrbitController
from lookAtController import LookAtController
from flyController import FlyController
from leashController import LeashController
from vector3 import Vector3
# on host systems these files are located here
from sololink import btn_msg
FOLLOW_PORT = 14558
SOCKET_TIMEOUT = 0.01
DEFAULT_PILOT_VELZ_MAX_VALUE = 133.0
ZVEL_FACTOR = 0.95
# in degrees per second for FREE CAM
YAW_SPEED = 120.0
MIN_RAW_ROI_QUEUE_LENGTH = 5
MIN_FILT_ROI_QUEUE_LENGTH = 4
#Path accel/decel constants
PATH_ACCEL = 2.5
ACCEL_PER_TICK = PATH_ACCEL * UPDATE_TIME
FOLLOW_ALT_NUDGE_SPEED = 6 # m/s
FOLLOW_ALT_ROI_OFFSET = 100
FOLLOW_ALT_PADDLE_DEADZONE = 0.02
FOLLOW_YAW_SPEED = 60.0 # deg/s
FOLLOW_PITCH_SPEED = 60.0 # deg/s
ROI_ALT_FILTER_GAIN = 0.65 # Relative contribution of the previous and new data in the phone altitude filter. Higher is slower and smoother.
logger = shotLogger.logger
# Define the different followStates:
# Follow Wait
# Initial state before everything is ready. This state exits when the first ROI is sent from the app. (in filterROI()) The shot will probably only be in this state for a few ticks.
# Look At Me
# Copter does not move unless commanded by stick actions, but the camera rotates to keep the ROI in frame
# Follow Orbit: Default behaviour.
# Copter translates to keep the user in frame, maintining a offset in a particular direction (i.e. North) of the user.
# Once the orbit is started (cruise via the app or with sticks) then the copter orbits around the ROI, keeping the subject in frame.
# Follow Leash
# Copter tries to stay behind the user's direction of motion.
# If the copter is within the leash length from the user, the leash is "slack" so the copter rotates but does not move.
# Once the copter gets further from the user. It swings around to get behind the user and keep up.
# User can adjust altitude and radius with sticks but cannot strafe manually.
# Follow Free Look
# Copter maintains an offset from the ROI and can get dragged around as the ROI moves
# User can move the copter with the roll/pitch sticks like FLY mode
# User can adjust the altitude with the controller paddle
# User can freely control the camera pan/tilt with the left stick.
# Camera maintains constant yaw/pitch unless the user permutes it.
FOLLOW_WAIT = 0
FOLLOW_LOOKAT = 1
FOLLOW_ORBIT = 2
FOLLOW_LEASH = 3
FOLLOW_FREELOOK = 4
# used to manage user preferences for different follow styles
FOLLOW_PREF_HOLD_ANGLE = 0
FOLLOW_PREF_FREELOOK = 1
FOLLOW_PREF_LEASH = 2
class FollowShot():
def __init__(self, vehicle, shotmgr):
# initialize vehicle object
self.vehicle = vehicle
# initialize shotmanager object
self.shotmgr = shotmgr
# initialize pathController to None
self.pathController = None
# used to manage cruise speed and pause control for orbiting
self.pathHandler = pathHandler.PathHandler( self.vehicle, self.shotmgr )
''' Shot State '''
self.followState = FOLLOW_WAIT
self.followPreference = FOLLOW_PREF_HOLD_ANGLE
# init camera mount style
self.updateMountStatus()
''' ROI control '''
# initialize raw & filtered rois to None
self.filteredROI = None
self.rawROI = None
# initialize raw and filtered roi queues
self.rawROIQueue = collections.deque(maxlen=MIN_RAW_ROI_QUEUE_LENGTH)
self.filteredROIQueue = collections.deque(maxlen=MIN_FILT_ROI_QUEUE_LENGTH)
# initialize roiVelocity to None
self.roiVelocity = None
# for limiting follow acceleration could lead to some bad lag
self.translateVel = Vector3()
''' Altitude Limit'''
# get maxClimbRate and maxAltitude from APM params
self.maxClimbRate = shotmgr.getParam( "PILOT_VELZ_MAX", DEFAULT_PILOT_VELZ_MAX_VALUE ) / 100.0 * ZVEL_FACTOR
self.maxAlt = self.shotmgr.getParam( "FENCE_ALT_MAX", DEFAULT_FENCE_ALT_MAX )
logger.log("[follow]: Maximum altitude stored: %f" % self.maxAlt)
# check APM params to see if Altitude Limit should apply
if self.shotmgr.getParam( "FENCE_ENABLE", DEFAULT_FENCE_ENABLE ) == 0:
self.maxAlt = None
logger.log("[Follow Me]: Altitude Limit disabled.")
# the open loop altitude of the follow controllers, relative to the ROI
self.followControllerAltOffset = 0
# used to adjust frame of ROI vertically (Just affects camera pointing, not copter position)
self.ROIAltitudeOffset = 0
''' Communications '''
# configure follow socket
self.setupSocket()
# take away user control of vehicle
self.vehicle.mode = VehicleMode("GUIDED")
self.shotmgr.rcMgr.enableRemapping( True )
# channels are expected to be floating point values in the (-1.0, 1.0) range
def handleRCs( self, channels ):
# check socket for a new ROI from the app
self.checkSocket()
# if we have never received an ROI
if not self.rawROI:
return
# smooth ROI and calculate translateVel for follow
self.filterROI()
# if we are not warmed up, dont do anything
if self.followState == FOLLOW_WAIT:
return
'''
Position Control
Note: All follow controllers return position and velocity of the drone in "absolute" space (as opposed to relative to the ROI)
Pos: Lat, lon, alt. Alt is relative to home location. NEU frame, of course.
Vel: Speed in the x,y, and z directions. NEU frame. vel.z needs to be inverted before passing to the autopilot.
'''
# Look At Me Mode (Vehicle stays put)
if self.followState == FOLLOW_LOOKAT:
pos, vel = self.pathController.move(channels)
# Free Look Follow Mode (Vehicle controls are similar to FLY)
elif self.followState == FOLLOW_FREELOOK:
pos, vel = self.pathController.move(channels, newHeading = self.camYaw, newOrigin = self.filteredROI, roiVel = self.translateVel)
# Follow Me Mode (Vehicle controls are similar to ORBIT)
elif self.followState == FOLLOW_ORBIT:
pos, vel = self.pathController.move(channels, cruiseSpeed = self.pathHandler.cruiseSpeed, newroi = self.filteredROI, roiVel = self.translateVel)
elif self.followState == FOLLOW_LEASH:
pos, vel = self.pathController.move(channels, newroi = self.filteredROI, roiVel = self.translateVel)
# learn any changes to controller alt offset to apply it to other controllers when instantiated (to avoid jerks)
self.followControllerAltOffset = pos.alt - self.filteredROI.alt
# mavlink expects 10^7 integer for accuracy
latInt = int(pos.lat * 10000000)
lonInt = int(pos.lon * 10000000)
# Convert from NEU to NED to send to autopilot
vel.z *= -1
# create the SET_POSITION_TARGET_GLOBAL_INT command
msg = 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, # Pos Vel type_mask
latInt, lonInt, pos.alt, # x, y, z positions
vel.x, vel.y, vel.z, # x, y, z velocity in m/s
0, 0, 0, # x, y, z acceleration (not used)
0, 0) # yaw, yaw_rate (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
''' Pointing Control'''
# If followState mandates that the user controls the pointing
if self.followState == FOLLOW_FREELOOK:
self.manualPitch(channels[THROTTLE])
self.manualYaw(channels)
self.handleFreeLookPointing()
# If followState mandates that the copter should point at the ROI
else:
# Adjust the height of the ROI using the paddle
# we pass in both the filtered gimbal paddle value as well as the raw one
self.updateROIAltOffset(channels[RAW_PADDLE])
# we make a copy of the ROI to allow us to add in an altitude offset
# this means the ROI doesn't get polluted with the alt nudging
tempROI = LocationGlobalRelative(self.filteredROI.lat, self.filteredROI.lon, self.filteredROI.alt)
self.handleLookAtPointing(tempROI)
# moves an offset of the ROI altitude up or down
def updateROIAltOffset(self, rawPaddleValue):
# no gimbal, no reason to change ROI
if self.vehicle.mount_status[0] == None:
return
if abs(rawPaddleValue) > FOLLOW_ALT_PADDLE_DEADZONE:
self.ROIAltitudeOffset += (rawPaddleValue * FOLLOW_ALT_NUDGE_SPEED * UPDATE_TIME)
# if we can handle the button we do
def handleButton(self, button, event):
if button == btn_msg.ButtonA and event == btn_msg.Press:
# Allow the user to exit Look At Me mode (into the previous follow mode) with the A button
if self.followPreference == FOLLOW_PREF_LEASH:
self.initState(FOLLOW_LEASH)
elif self.followPreference == FOLLOW_PREF_FREELOOK:
self.initState(FOLLOW_FREELOOK)
else:
self.initState(FOLLOW_ORBIT)
# Change Follow Mode to Look At Me on Pause button press
if button == btn_msg.ButtonLoiter and event == btn_msg.Press:
self.initState(FOLLOW_LOOKAT)
self.shotmgr.notifyPause(True)
self.updateAppOptions()
self.setButtonMappings()
def setButtonMappings(self):
buttonMgr = self.shotmgr.buttonManager
# Allow the user to exit Look At Me mode (into the previous follow mode) with the A button
if self.followState == FOLLOW_LOOKAT:
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_FOLLOW, btn_msg.ARTOO_BITMASK_ENABLED, "Resume\0")
else:
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_FOLLOW, 0, "\0")
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_FOLLOW, 0, "\0")
# pass in the value portion of the SOLO_FOLLOW_OPTIONS packet
def handleOptions(self, options, version):
if version == 1:
logger.log("[Follow Me]: Received Follow Me Options v1 packet.")
(cruiseSpeed, _lookat) = struct.unpack('<fi', options)
elif version == 2:
logger.log("[Follow Me]: Received Follow Me Options v2 packet.")
(cruiseSpeed, _lookat, self.followPreference) = struct.unpack('<fii', options)
else:
logger.log("[follow]: Unknown packet version requested.")
return
logger.log( "[Follow Me]: -->Cruise speed: %f" % cruiseSpeed)
logger.log( "[Follow Me]: -->Look At: %i" % _lookat)
logger.log( "[Follow Me]: -->Follow Pref: %i" % self.followPreference)
self.pathHandler.setCruiseSpeed(cruiseSpeed)
# if user presses cruise arrows, force user into Orbit mode
if cruiseSpeed != 0:
# if we press cruise, force user into Orbit mode
# only work on state changes
# force follow Pref into Orbit
self.followPreference = FOLLOW_PREF_HOLD_ANGLE
newState = FOLLOW_ORBIT
elif _lookat == 1:
# Lookat overrides the follow preferences
newState = FOLLOW_LOOKAT
else:
# we may be exiting lookat into Orbit, Freelook or Leash
if self.followPreference == FOLLOW_PREF_FREELOOK:
newState = FOLLOW_FREELOOK
elif self.followPreference == FOLLOW_PREF_LEASH:
newState = FOLLOW_LEASH
else:
# enter default state
newState = FOLLOW_ORBIT
self.initState(newState)
def initState(self, newState):
'''Manages state changes'''
# Don't change state until we've received at least one ROI from phone
# Don't re-init previous state
if not self.rawROI or self.followState == newState:
return
self.followState = newState
if self.followState == FOLLOW_LOOKAT:
logger.log("[Follow Me]: enter Lookat")
self.initLookAtMeController()
elif self.followState == FOLLOW_ORBIT:
logger.log("[Follow Me]: enter Orbit")
self.initOrbitController()
elif self.followState == FOLLOW_LEASH:
logger.log("[Follow Me]: enter Leash")
self.initLeashController()
elif self.followState == FOLLOW_FREELOOK:
logger.log("[Follow Me]: enter Freelook")
self.initFreeLookController()
self.updateMountStatus()
# update UI
self.updateAppOptions()
self.setButtonMappings()
# send our current set of options to the app
def updateAppOptions(self):
_lookAtMe = self.followState == FOLLOW_LOOKAT
packetV1 = struct.pack('<IIfi', app_packet.SOLO_FOLLOW_OPTIONS, 8, self.pathHandler.cruiseSpeed, _lookAtMe)
self.shotmgr.appMgr.sendPacket(packetV1)
packetV2 = struct.pack('<IIfii', app_packet.SOLO_FOLLOW_OPTIONS_V2, 12, self.pathHandler.cruiseSpeed, _lookAtMe, self.followPreference)
self.shotmgr.appMgr.sendPacket(packetV2)
def setupSocket(self):
'''Sets up the socket for GPS data from app'''
# Follow me creates a socket to get new ROIs
self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.socket.setblocking(0)
try:
self.socket.bind(("", FOLLOW_PORT))
except Exception as e:
logger.log("[follow]: failed to bind follow socket - %s"%(type(e)))
self.socket.settimeout(SOCKET_TIMEOUT)
def checkSocket(self):
'''check our socket to see if a new follow roi is there'''
packetsConsumed = 0
#consume from socket until it's empty
while True:
try:
data, addr = self.socket.recvfrom(28)
except socket.timeout:
break
else:
newestData = data
packetsConsumed += 1
# make sure we have a packet to work with
if packetsConsumed > 0:
(id, length, lat, lon, alt) = struct.unpack('<IIddf', newestData)
if id == app_packet.SOLO_MESSAGE_LOCATION:
now = monotonic.monotonic()
if self.rawROI is None:
self.roiDeltaTime = None
else:
self.roiDeltaTime = now - self.previousROItime
self.previousROItime = now
self.rawROI = LocationGlobalRelative(lat,lon,alt)
else:
logger.log("[follow]: got an invalid packet from follow socket")
else:
pass # will implement 2 second timeout to kill shot here
def filterROI(self):
'''Filters app ROI using a 5th order linear filter and calculates an associated roi velocity'''
# store last 5 raw roi values
self.rawROIQueue.append(self.rawROI)
# only run filter if we have enough data in the queues.
if len(self.rawROIQueue) == MIN_RAW_ROI_QUEUE_LENGTH and len(self.filteredROIQueue) == MIN_FILT_ROI_QUEUE_LENGTH:
num = [0,0.000334672774973874,0.00111965413719632,-0.000469533537393159,-0.000199779184127412]
den = [1,-3.48113699710809,4.56705782792063,-2.67504447769757,0.589908661075676]
filteredLat = ( (num[4]*self.rawROIQueue[0].lat + num[3]*self.rawROIQueue[1].lat + num[2]*self.rawROIQueue[2].lat + num[1]*self.rawROIQueue[3].lat + num[0]*self.rawROIQueue[4].lat) - (den[4]*self.filteredROIQueue[0].lat + den[3]*self.filteredROIQueue[1].lat + den[2]*self.filteredROIQueue[2].lat + den[1]*self.filteredROIQueue[3].lat) ) / den[0]
filteredLon = ( (num[4]*self.rawROIQueue[0].lon + num[3]*self.rawROIQueue[1].lon + num[2]*self.rawROIQueue[2].lon + num[1]*self.rawROIQueue[3].lon + num[0]*self.rawROIQueue[4].lon) - (den[4]*self.filteredROIQueue[0].lon + den[3]*self.filteredROIQueue[1].lon + den[2]*self.filteredROIQueue[2].lon + den[1]*self.filteredROIQueue[3].lon) ) / den[0]
filteredAlt = ROI_ALT_FILTER_GAIN * self.filteredROIQueue[-1].alt + (1-ROI_ALT_FILTER_GAIN) * self.rawROI.alt# index -1 should refer to most recent value in the queue.
self.filteredROI = LocationGlobalRelative(filteredLat,filteredLon,filteredAlt)
else:
self.filteredROI = self.rawROI
# store last 4 filtered roi values
self.filteredROIQueue.append(self.filteredROI)
# only called once - when we have an ROI from phone
if len(self.filteredROIQueue) == 1:
# initialize ROI velocity for Guided controller
self.roiVelocity = Vector3(0,0,0)
# initialize the altitude maintained for each controller
# the first ROI sent from the app is supposed to have 0 altitude, but in case it doesn't subtract it out.
self.followControllerAltOffset = self.vehicle.location.global_relative_frame.alt - self.rawROI.alt
logger.log('[Follow Me]: First ROI received from app has altitude: %f m' %self.rawROI.alt)
# go into Look At Me as default state. iOS app changes state to Orbit after 3 seconds.
self.initState(FOLLOW_LOOKAT)
elif len(self.filteredROIQueue) > 1 and self.roiDeltaTime:
#calculate vector from previous to new roi in North,East,Up
vec = location_helpers.getVectorFromPoints(self.filteredROIQueue[-2] , self.filteredROIQueue[-1])
# calculate velocity based on time difference
# roiVelocity in NEU frame
self.roiVelocity = vec/self.roiDeltaTime
# calculate desiredYaw and desiredPitch from new ROI
self.desiredYaw, self.desiredPitch = location_helpers.calcYawPitchFromLocations(self.vehicle.location.global_relative_frame, self.filteredROI)
#x component accel limit
if self.roiVelocity.x > self.translateVel.x:
self.translateVel.x += ACCEL_PER_TICK
self.translateVel.x = min(self.translateVel.x, self.roiVelocity.x)
elif self.roiVelocity.x < self.translateVel.x:
self.translateVel.x -= ACCEL_PER_TICK
self.translateVel.x = max(self.translateVel.x, self.roiVelocity.x)
#y component accel limit
if self.roiVelocity.y > self.translateVel.y:
self.translateVel.y += ACCEL_PER_TICK
self.translateVel.y = min(self.translateVel.y, self.roiVelocity.y)
elif self.roiVelocity.y < self.translateVel.y:
self.translateVel.y -= ACCEL_PER_TICK
self.translateVel.y = max(self.translateVel.y, self.roiVelocity.y)
#z component accel limit
if self.roiVelocity.z > self.translateVel.z:
self.translateVel.z += ACCEL_PER_TICK
self.translateVel.z = min(self.translateVel.z, self.roiVelocity.z)
elif self.roiVelocity.z < self.translateVel.z:
self.translateVel.z -= ACCEL_PER_TICK
self.translateVel.z = max(self.translateVel.z, self.roiVelocity.z)
def initOrbitController(self):
'''Resets the controller'''
# reset Orbit
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 = OrbitController(self.filteredROI, resetRadius, resetAz, self.followControllerAltOffset)
# set controller options
self.pathController.setOptions(maxClimbRate = self.maxClimbRate, maxAlt = self.maxAlt)
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 initFreeLookController(self):
'''Enter/exit free look'''
vectorOffset = location_helpers.getVectorFromPoints(self.filteredROI, self.vehicle.location.global_relative_frame)
xOffset = vectorOffset.x
yOffset = vectorOffset.y
zOffset = self.followControllerAltOffset
heading = camera.getYaw(self.vehicle)
# Initialize the feed-forward orbit controller
self.pathController = FlyController(self.filteredROI, xOffset, yOffset, zOffset, heading)
# set controller options
self.pathController.setOptions(maxClimbRate = self.maxClimbRate, maxAlt = self.maxAlt)
# default camPitch and Yaw from vehicle
self.camPitch = camera.getPitch(self.vehicle)
self.camYaw = camera.getYaw(self.vehicle)
# only used for non-gimbaled copters
self.camDir = 1
def initLookAtMeController(self):
'''Enter lookat mode'''
# zero out any cruise speed
self.pathHandler.pause()
# Initialize the feed-forward orbit controller. Look at me is unique in that we pass total altitude, not just the offset
self.pathController = LookAtController(self.vehicle, self.followControllerAltOffset + self.filteredROI.alt)
# set controller options
self.pathController.setOptions(maxClimbRate = self.maxClimbRate, maxAlt = self.maxAlt)
def updateMountStatus(self):
if self.followState == FOLLOW_FREELOOK:
msg = self.vehicle.message_factory.mount_configure_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, #mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
else:
# set gimbal targeting mode
msg = self.vehicle.message_factory.mount_configure_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, #mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
self.vehicle.send_mavlink(msg)
def manualPitch(self, stick):
self.camPitch += stick * FOLLOW_PITCH_SPEED * UPDATE_TIME
if self.camPitch > 0.0:
self.camPitch = 0.0
elif self.camPitch < -90:
self.camPitch = -90
def manualYaw(self, channels):
if channels[YAW] == 0:
return
self.camYaw += channels[YAW] * FOLLOW_YAW_SPEED * UPDATE_TIME
if channels[YAW] > 0:
self.camDir = 1
else:
self.camDir = -1
self.camYaw = location_helpers.wrapTo360(self.camYaw)
def handleFreeLookPointing(self):
# if we do have a gimbal, use mount_control to set pitch and yaw
if self.vehicle.mount_status[0] is not None:
msg = self.vehicle.message_factory.mount_control_encode(
0, 1, # target system, target component
# pitch is in centidegrees
self.camPitch * 100,
0.0, # roll
# yaw is in centidegrees
self.camYaw * 100,
0 # save position
)
else:
# if we don't have a gimbal, just set CONDITION_YAW
msg = self.vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
0, #confirmation
self.camYaw, # param 1 - target angle
YAW_SPEED, # param 2 - yaw speed
self.camDir, # param 3 - direction
0.0, # relative offset
0, 0, 0 # params 5-7 (unused)
)
self.vehicle.send_mavlink(msg)
def handleLookAtPointing(self, tempROI):
# set ROI
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
tempROI.lat*1.E7,
tempROI.lon*1.E7,
tempROI.alt + self.ROIAltitudeOffset #offset for ROI
)
# send pointing message
self.vehicle.send_mavlink(msg)
def handlePacket(self, packetType, packetLength, packetValue):
try:
if packetType == app_packet.SOLO_FOLLOW_OPTIONS:
logger.log("[follow]: Received Follow Me Options v1 packet.")
self.handleOptions(packetValue, version=1)
elif packetType == app_packet.SOLO_FOLLOW_OPTIONS_V2:
logger.log("[follow]: Received Follow Me Options v2 packet.")
self.handleOptions(packetValue, version=2)
else:
return False
except Exception as e:
logger.log('[follow]: Error handling packet. (%s)' % e)
return False
else:
return True

157
shotmanager/leashController.py Normal file
View File

@ -0,0 +1,157 @@
# leashController.py
# shotmanager
#
# The leash movement controller.
# A cylindrical-coordinates based orbit motion controller than be permuted by user-sticks or a strafe cruise speed
# Automatically swings the copter around to be behind the direction of motion.
#
# Runs as a DroneKit-Python script.
#
# Created by Jason Short and Nick Speal on 2/26/2016.
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from pymavlink import mavutil
from dronekit import Vehicle, LocationGlobalRelative
import location_helpers
import shotLogger
from shotManagerConstants import *
import math
from vector3 import Vector3
from orbitController import OrbitController
logger = shotLogger.logger
#Path accel/decel constants
PATH_ACCEL = 2.5
ACCEL_PER_TICK = PATH_ACCEL * UPDATE_TIME
ORBIT_RAD_FOR_MIN_SPEED = 2.0
''' Crosstrack control '''
CROSSTRACK_GAIN = 3
CROSSTRACK_MINSPEED = .25
# seems to be the safest limit (CS gain of 4)
CROSSTRACK_MAX = 360 * UPDATE_TIME
class LeashController(OrbitController):
def __init__(self, roi, radius, azimuth, zOffset):
super(LeashController, self).__init__(roi, radius, azimuth, zOffset)
#initialize location
self.currentLoc = location_helpers.newLocationFromAzimuthAndDistance(self.roi, self.azimuth, self.radius)
self.currentLoc.altitude = roi.alt + zOffset
self.distance = radius
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

121
shotmanager/location_helpers.py Normal file
View File

@ -0,0 +1,121 @@
#helper functions for location
from dronekit.lib import LocationGlobalRelative
import math
from vector3 import Vector3
LATLON_TO_M = 111195.0
#optimize : store persistent sclaing
def getLonScale(lat):
scale = 1 / math.cos(math.radians(lat))
return scale
#returns distance between the given points in meters
def getDistanceFromPoints(loc1, loc2):
dlat = (loc2.lat - loc1.lat)
dlong = (loc2.lon - loc1.lon) / getLonScale(loc1.lat)
dist = math.sqrt((dlat * dlat) + (dlong * dlong)) * LATLON_TO_M
return dist
#returns distance between the given points in meters
def getDistanceFromPoints3d(loc1, loc2):
dlat = (loc2.lat - loc1.lat)
dlong = (loc2.lon - loc1.lon) / getLonScale(loc1.lat)
dalt = (loc2.alt - loc1.alt) / LATLON_TO_M
dist = math.sqrt((dlat * dlat) + (dlong * dlong) + (dalt * dalt)) * LATLON_TO_M
return dist
#Calculate a Location from a start location, azimuth (in degrees), and distance
#this only handles the 2D component (no altitude)
def newLocationFromAzimuthAndDistance(loc, azimuth, distance):
result = LocationGlobalRelative(loc.lat, loc.lon, loc.alt)
az = math.radians(azimuth)
distance = distance / LATLON_TO_M
result.lat = loc.lat + math.cos(az) * distance
result.lon = loc.lon + math.sin(az) * distance * getLonScale(loc.lat)
return result
#calculate azimuth between a start and end point (in degrees)
def calcAzimuthFromPoints(loc1, loc2):
off_x = (loc2.lon - loc1.lon) / getLonScale(loc1.lat)
off_y = (loc2.lat - loc1.lat)
az = 90 + math.degrees(math.atan2(-off_y, off_x))
return wrapTo360(az)
# given a start and an end point, return a Vector containing deltas in meters between start/end
# along each axis
# returns a Vector3
def getVectorFromPoints(start, end):
x = (end.lat - start.lat) * LATLON_TO_M
# calculate longitude scaling factor. We could cache this if necessary
# but we aren't doing so now
y = ((end.lon - start.lon) * LATLON_TO_M) / getLonScale(start.lat)
z = end.alt - start.alt
return Vector3(x, y, z)
# add the given Vector3 (storing meter deltas) to the given Location
# and return the resulting Location
def addVectorToLocation(loc, vec):
xToDeg = vec.x / LATLON_TO_M
# calculate longitude scaling factor. We could cache this if necessary
# but we aren't doing so now
yToDeg = (vec.y / LATLON_TO_M) * getLonScale(loc.lat)
return LocationGlobalRelative(loc.lat + xToDeg, loc.lon + yToDeg, loc.alt + vec.z)
# Casts a ray at the ground based on the location, heading and camera pitch
# The Spot lock location is always equal to home altitude (zero)
def getSpotLock(loc, pitch, yaw):
#expecting 0(straight) to -90 (down)
pitch = 90.0 - pitch
dist = math.tan(math.radians(-pitch)) * loc.alt
loc = newLocationFromAzimuthAndDistance(loc, yaw, dist)
loc.alt = 0
return loc
# Given a location, find yaw and pitch from Solo to look at that point
# returns a (yaw, pitch) tuple
def calcYawPitchFromLocations(start, end):
yaw = calcAzimuthFromPoints(start, end)
dist = getDistanceFromPoints(start, end)
# inverting the equation above:
# dist = loc.alt * math.tan(iPitchR)
# we get
# iPitchR = math.atan(dist/alt) where alt is the alt difference between our points
altDiff = start.alt - end.alt
if altDiff < 1.0:
return yaw, 0
iPitchR = math.atan(dist/altDiff)
iPitch = math.degrees(iPitchR)
pitch = iPitch - 90
return yaw, pitch
def wrapTo180(val):
if (val < -180) or (180 < val):
return wrapTo360(val + 180) - 180
else:
return val
def wrapTo360(val):
wrapped = val % 360
if wrapped == 0 and val > 0:
return 360
else:
return wrapped
def deg2rad(deg):
return deg * math.pi/180.
def rad2deg(rad):
return rad * 180./math.pi

87
shotmanager/lookAtController.py Normal file
View File

@ -0,0 +1,87 @@
# lookAtController.py
# shotmanager
#
# The lookAt movement controller. The drone stays still unless permuted by sticks or phone altitude.
# Runs as a DroneKit-Python script.
#
# Created by Jason Short on 2/26/2016.
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from pymavlink import mavutil
from dronekit import Vehicle, LocationGlobalRelative
import location_helpers
import shotLogger
from shotManagerConstants import *
import math
from vector3 import Vector3
logger = shotLogger.logger
class LookAtController():
'''Manages altitude while holding XY position'''
def __init__(self, vehicle, altitude):
'''Initialize the controller'''
# used to prevent user from going lower than initial alitide - prevents failed "landings"
self.initial_altitude = altitude
self.altitude = altitude
# options attributes
self.maxAlt = None
self.maxClimbRate = None
#initialize location
self.currentLoc = LocationGlobalRelative(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon, self.altitude)
def setOptions(self, maxClimbRate, maxAlt):
'''Sets maximum limits in open-loop controller'''
self.maxClimbRate = maxClimbRate
self.maxAlt = maxAlt
def move(self, channels):
'''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'''
# Climb
currentVel = self._climb(channels[THROTTLE])
# set altitude
self.currentLoc.alt = self.altitude
return self.currentLoc, currentVel
def _climb(self, stick):
'''Handles altitude stick input to increase or decrease altitude'''
zVel = stick * self.maxClimbRate
self.altitude += zVel * UPDATE_TIME
# prevent landing in LookAt mode
if self.altitude <= self.initial_altitude:
self.altitude = self.initial_altitude
return Vector3(0,0,0)
# check altitude limit
# In Follow Shot, this enforces alt limit relative to the initial ROI. (The ROI when the look at controller is instantiated)
# If the initial ROI is above the takeoff point, we depend on the follow shot to limit the superimposed altitude
# If the initial ROI is below the takeoff point, then the altitude limit relative to the ROI is enforced here.
if self.maxAlt is not None:
self.altitude = min(self.altitude, self.maxAlt)
return Vector3(0,0,zVel)

27
shotmanager/main.py Executable file
View File

@ -0,0 +1,27 @@
#!/usr/bin/env python
# Main entry point for shot manager
import os
from os import sys, path
sys.path.append(os.path.realpath(''))
import shotManager
from dronekit import connect
from dronekit_solo import SoloVehicle
from dronekit.mavlink import MAVConnection
# First get an instance of the API endpoint
vehicle = connect(sys.argv[1], wait_ready=False, vehicle_class=SoloVehicle, source_system=255, use_native=True, heartbeat_timeout=-1)
if 'SOLOLINK_SANDBOX' in os.environ:
from sim import rc_ipc_shim
rc_ipc_shim.pixrc_start()
out = MAVConnection(sys.argv[2], source_system=254)
vehicle._handler.pipe(out)
out.start()
mgr = shotManager.ShotManager()
mgr.Start(vehicle)
mgr.Run()

42
shotmanager/modes.py Executable file
View File

@ -0,0 +1,42 @@
#!/usr/bin/env python
# mode names - These are what Artoo displays
MODE_NAMES = {
-1 : "None\0",
0 : 'Stabilize\0',
1 : 'Acro\0',
2 : 'FLY: Manual\0',
3 : 'Autonomous\0',
4 : 'Takeoff\0',
5 : 'FLY\0',
6 : 'Return Home\0',
7 : 'Circle\0',
8 : 'Position\0',
9 : 'Land\0',
10 : 'OF_LOITER\0',
11 : 'Drift\0',
13 : 'Sport\0',
14 : 'Flip\0',
15 : 'Auto-tune\0',
16 : 'Position Hold\0',
17 : 'Brake\0',
}
# DroneKit uses APM's mode names. Here is a helper function to
# go from DroneKit's name to APM mode index
def getAPMModeIndexFromName( modeName, vehicle ):
# most of these names are in pymavlink's mode mapping.
# But not all!
# Here we handle cases that aren't
# if it's a mode that's not in mavlink's mapping, it's formatted
# like Mode(12)
if "Mode(" in modeName:
try:
return int(modeName[5:-1])
except:
return -1
elif modeName in vehicle._mode_mapping:
return vehicle._mode_mapping[modeName]
else:
return -1

943
shotmanager/multipoint.py Normal file
View File

@ -0,0 +1,943 @@
# multipoint.py
# shotmanager
#
# The multipoint cable cam smart shot controller.
# Runs as a DroneKit-Python script.
#
# Created by Will Silva on 11/22/2015.
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from dronekit import Vehicle, LocationGlobalRelative, VehicleMode
from pymavlink import mavutil
import math
import struct
import app_packet
import camera
import location_helpers
import shotLogger
from shotManagerConstants import *
import shots
import yawPitchOffsetter
from catmullRom import CatmullRom
from vector3 import Vector3
from vector2 import Vector2
from sololink import btn_msg
import cableController
from cableController import CableController
import monotonic
# initiate logger
logger = shotLogger.logger
# CONSTANTS
ACCEL_LIMIT = 2.5 #m/s^2
NORM_ACCEL_LIMIT = 2.25 #m/s^2
TANGENT_ACCEL_LIMIT = math.sqrt(ACCEL_LIMIT**2-NORM_ACCEL_LIMIT**2) #m/s^2
# Yaw rate (deg/s)
YAW_SPEED = 100.0
# Minimum cruise speed that can be set by the app
MIN_CRUISE_SPEED = 0.1 #m/s
# change in speed required to send the app a notification of speed/location for
# visualization purposes
SPEED_CHANGE_NOTIFICATION_THRESHOLD = 0.5 #m/s
# speed at which vehicle attaches to cable
ATTACH_SPEED = 2 #m/s
# yaw speed at which vehicle transitions to cable
ATTACH_YAW_SPEED = 80 #deg/s
# cm / s from APM
DEFAULT_WPNAV_SPEED_VALUE = 1100.0 # cm/s
# Number of times we notify the app of our playback status
NOTIFICATIONS_PER_SECOND = 10
# Maximum time, in seconds, that any cable can take
MAXIMUM_CABLE_DURATION = 20*60.
# constants for cruiseState
RIGHT = 1
PAUSED = 0
LEFT = -1
class Waypoint():
def __init__(self, loc, pitch, yaw):
self.loc = loc
self.yaw = yaw
self.pitch = pitch
if self.pitch is None:
self.pitch = 0.0
class MultipointShot():
def __init__(self, vehicle, shotmgr):
# assign vehicle object
self.vehicle = vehicle
# assign shotmanager object
self.shotmgr = shotmgr
# initialize YawPitchOffsetter object
self.yawPitchOffsetter = yawPitchOffsetter.YawPitchOffsetter()
# enable yaw nudge by default
self.yawPitchOffsetter.enableNudge()
# initializes/resets most member vars
self.resetShot()
# get current altitude limit
self.maxAlt = self.shotmgr.getParam( "FENCE_ALT_MAX", DEFAULT_FENCE_ALT_MAX ) # in meters
logger.log("[Multipoint]: Maximum altitude stored: %f" % self.maxAlt)
# check APM params to see if Altitude Limit should apply
if self.shotmgr.getParam( "FENCE_ENABLE", DEFAULT_FENCE_ENABLE ) == 0:
self.maxAlt = None
logger.log("[Multipoint]: Altitude Limit disabled.")
def resetShot(self):
'''initializes/resets most member vars'''
# camInterpolation == 1: free look
# camInterpolation == 0: tween pitch/yaw between keyframes
self.camInterpolation = 0
# True - Play mode, False - Record mode
self.cableCamPlaying = False
# initialize waypoints list
self.waypoints = []
# stores list of which direction to yaw while on the cable
self.yawDirections = []
# desired speed set by sticks or cruise speed
self.desiredSpeed = 0.0
# active cruise speed being used by controller
self.cruiseSpeed = 0
# stored cruise speed set from app
self.storedCruiseSpeed = 0
# current state of cruise (-1 = moving left, 0 = paused, 1 = moving right)
self.cruiseState = PAUSED
# declare camera spline object
self.camSpline = None
# initialize commanded velocity
self.commandVel = None
# initialize commanded position
self.commandPos = None
# flag that indicates whether we are in the attaching state
self.attaching = True
# last speed that was reported to the app for visualization purposes
self.lastNotifiedSpeed = 0.0
# last segment that was reported to the app for visualization purposes
self.lastNotifiedSegment = None
# Index to attach on spline during a cable load
self.attachIndex = -1
# ticks to track when to notify app with playback status update
self.ticksSinceNotify = 0
# default targetP
self.targetP = 0.0
# initialize cable to None
self.cable = None
# solo spline point version
self.splinePointVersion = 0
# absolute altitude reference
self.absAltRef = None
# initialize min/max times
self.maxTime = None
self.minTime = None
# last time that the controller was advanced
self.lastTime = None
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)
def interpolateCamera(self):
'''Interpolate (linear) pitch and yaw between cable control points'''
perc = self.cable.currentU
# sanitize perc
if perc < 0.0:
perc = 0.0
elif perc > 1.0:
perc = 1.0
# get pitch and yaw from spline (x is pitch, y is yaw)
pitchYaw = self.camSpline.position(self.cable.currentSeg,perc)
return pitchYaw.x, location_helpers.wrapTo360(pitchYaw.y)
def recordLocation(self):
'''record a cable control point'''
# don't run this function if we're already in the cable
if self.cableCamPlaying:
logger.log("[multipoint]: Cannot record a location when in PLAY mode.")
return
# record HOME absolute altitude reference if this is the first waypoint
if len(self.waypoints) == 0:
self.absAltRef = self.vehicle.location.global_frame.alt - self.vehicle.location.global_relative_frame.alt
logger.log("[multipoint]: HOME absolute altitude recorded: %f meters" % self.absAltRef)
# capture current pitch and yaw state
pitch = camera.getPitch(self.vehicle)
degreesYaw = location_helpers.wrapTo360(camera.getYaw(self.vehicle))
# check if last recorded point is a duplicate with this new one
if self.duplicateCheck(self.vehicle.location.global_relative_frame, len(self.waypoints)):
logger.log("[multipoint]: Overwriting last point.")
# overwrite last point
self.waypoints[-1].loc = self.vehicle.location.global_relative_frame
self.waypoints[-1].pitch = pitch
self.waypoints[-1].yaw = degreesYaw
else:
# append a new point
self.waypoints.append(
Waypoint(self.vehicle.location.global_relative_frame, pitch, degreesYaw))
# log this point
logger.log("[multipoint]: Successfully recorded waypoint #%d. Lat: %f, Lon: %f, Alt: %f, Pitch: %f, Yaw: %f" %
(len(self.waypoints)-1, self.vehicle.location.global_relative_frame.lat, self.vehicle.location.global_relative_frame.lon, self.vehicle.location.global_relative_frame.alt, pitch, degreesYaw))
# send the spline point to the app
self.sendSoloSplinePoint(self.splinePointVersion, self.absAltRef, len(self.waypoints) - 1, self.vehicle.location.global_relative_frame.lat, self.vehicle.location.global_relative_frame.lon, self.vehicle.location.global_relative_frame.alt, pitch, degreesYaw, 0, app_packet.SPLINE_ERROR_NONE)
logger.log("[multipoint]: Sent spline point #%d to app." % (len(self.waypoints)-1))
# update button mappings
self.setButtonMappings()
def loadSplinePoint(self, point):
'''load a cable control point'''
(version, absAltRef, index, lat, lon, alt, pitch, yaw, uPosition, app_packet.SPLINE_ERROR_MODE) = point
if self.cableCamPlaying:
logger.log("[multipoint]: Shot in PLAY mode, cannot load waypoint.")
self.sendSoloSplinePoint(version, absAltRef, index, lat, lon, alt, pitch, yaw, uPosition, app_packet.SPLINE_ERROR_MODE)
logger.log("[multipoint]: Sent failed spline point #%d to app." % index)
return
if version == 0:
if self.cableCamPlaying:
logger.log("[multipoint]: Shot in PLAY mode, cannot load waypoint.")
self.sendSoloSplinePoint(version, absAltRef, index, lat, lon, alt, pitch, yaw, uPosition, app_packet.SPLINE_ERROR_MODE)
logger.log("[multipoint]: Sent failed spline point #%d to app." % index)
return
if self.duplicateCheck(LocationGlobalRelative(lat, lon, alt), index):
logger.log("[multipoint]: Duplicate detected, rejecting waypoint #%d." % index)
self.sendSoloSplinePoint(version, absAltRef, index, lat, lon, alt, pitch, yaw, uPosition, app_packet.SPLINE_ERROR_DUPLICATE)
logger.log("[multipoint]: Sent failed spline point #%d to app." % index)
return
# if this is the first loaded waypoint, store absolute altitude reference and version
if len(self.waypoints) == 0:
self.splinePointVersion = version
self.absAltRef = absAltRef
logger.log("[multipoint]: previous HOME absolute altitude loaded: %f meters" % self.absAltRef)
else:
logger.log("[multipoint]: Spline point version (%d) not supported, cannot load waypoint." % version)
return
# if loaded waypoint index is higher than current waypoint list size
# then extend waypoint list to accomodate
if (index + 1) > len(self.waypoints):
self.waypoints.extend([None] * (index + 1 - len(self.waypoints)))
# store received waypoint
self.waypoints[index] = Waypoint(LocationGlobalRelative(lat, lon, alt), pitch, yaw)
# log waypoint
logger.log("[multipoint]: Successfully loaded waypoint #%d. Lat: %f, Lon: %f, Alt: %f, Pitch: %f, Yaw: %f" % (
index, lat, lon, alt, pitch, yaw))
# send the spline point to the app
self.sendSoloSplinePoint(self.splinePointVersion, self.absAltRef, index, lat, lon, alt, pitch, yaw, uPosition, app_packet.SPLINE_ERROR_NONE)
logger.log("[multipoint]: Sent spline point #%d to app." % index)
# update button mappings
self.setButtonMappings()
def sendSoloSplinePoint(self, version, absAltReference, index, lat, lon, alt, pitch, yaw, uPosition, status):
if version == 0:
packet = struct.pack('<IIhfIddffffh', app_packet.SOLO_SPLINE_POINT, 44,
version, absAltReference, index, lat, lon, alt, pitch, yaw, uPosition, status)
self.shotmgr.appMgr.sendPacket(packet)
def duplicateCheck(self, loc, desiredIndex):
'''Checks for duplicate waypoints
loc - desired waypoint location
desiredIndex - index that we would like to add the waypoint to in self.waypoints
'''
isDuplicate = False
# check point on the right
if desiredIndex < len(self.waypoints)-1:
if self.waypoints[desiredIndex+1] is not None:
if location_helpers.getDistanceFromPoints3d(self.waypoints[desiredIndex+1].loc, loc) < WAYPOINT_NEARNESS_THRESHOLD:
logger.log("[multipoint]: Duplicate waypoint detected. Conflicts with index %d." % (desiredIndex + 1))
isDuplicate = True
# check point on the left
if 0 < desiredIndex <= len(self.waypoints):
if self.waypoints[desiredIndex-1] is not None:
if location_helpers.getDistanceFromPoints3d(self.waypoints[desiredIndex-1].loc, loc) < WAYPOINT_NEARNESS_THRESHOLD:
logger.log("[multipoint]: Duplicate waypoint detected. Conflicts with index %d." % (desiredIndex - 1))
isDuplicate = True
return isDuplicate
def setButtonMappings(self):
buttonMgr = self.shotmgr.buttonManager
if len(self.waypoints) == 0:
buttonMgr.setArtooButton(
btn_msg.ButtonA, shots.APP_SHOT_MULTIPOINT, btn_msg.ARTOO_BITMASK_ENABLED, "Record Point\0")
buttonMgr.setArtooButton(
btn_msg.ButtonB, shots.APP_SHOT_MULTIPOINT, 0, "\0")
elif not self.cableCamPlaying:
buttonMgr.setArtooButton(
btn_msg.ButtonA, shots.APP_SHOT_MULTIPOINT, btn_msg.ARTOO_BITMASK_ENABLED, "Record Point\0")
buttonMgr.setArtooButton(
btn_msg.ButtonB, shots.APP_SHOT_MULTIPOINT, btn_msg.ARTOO_BITMASK_ENABLED, "Finish Point\0")
else:
buttonMgr.setArtooButton(
btn_msg.ButtonA, shots.APP_SHOT_MULTIPOINT, 0, "\0")
buttonMgr.setArtooButton(
btn_msg.ButtonB, shots.APP_SHOT_MULTIPOINT, 0, "\0")
def handleButton(self, button, event):
'''handle actions for button presses'''
if not self.cableCamPlaying:
if button == btn_msg.ButtonA and event == btn_msg.Press:
self.recordLocation()
if button == btn_msg.ButtonB and event == btn_msg.Press:
self.recordLocation()
# only try to start a cable if we have more than 2 points
if len(self.waypoints) > 1:
# initialize multiPoint cable
self.enterPlayMode()
# handle pause button
if button == btn_msg.ButtonLoiter and event == btn_msg.Press:
self.setCruiseSpeed(state = PAUSED)
logger.log("[multipoint]: Pause button pressed on Artoo.")
def handlePathSettings(self, pathSettings):
'''pass in the value portion of the SOLO_SPLINE_PATH_SETTINGS packet'''
(cameraMode, desiredTime) = pathSettings
# don't run this function if we're not in Play Mode
if not self.cableCamPlaying:
logger.log("[multipoint]: Can't set cable settings. Not in play mode.")
return
# change pointing modes
if cameraMode != self.camInterpolation:
if cameraMode == 0:
self.yawPitchOffsetter.enableNudge()
logger.log("[multipoint]: Entering InterpolateCam.")
self.camInterpolation = 0
elif cameraMode == 1:
self.yawPitchOffsetter.disableNudge(
camera.getPitch(self.vehicle), camera.getYaw(self.vehicle))
logger.log("[multipoint]: Entering FreeCam.")
self.camInterpolation = 1
else:
logger.log("[multipoint]: Camera mode not recognized (%d)." % self.camInterpolation)
self.yawPitchOffsetter.enableNudge()
# calculate cruise speed from desiredTime
self.setCruiseSpeed(speed = self.estimateCruiseSpeed(desiredTime))
def setCruiseSpeed(self, speed=None, state=None):
'''Uses storedCruiseSpeed and cruiseState to assign a cruiseSpeed for the controller'''
# RIGHT == 1
# PAUSED == 0
# LEFT == -1
if speed is not None and speed != self.storedCruiseSpeed:
# limit it
if abs(speed) > MAX_SPEED:
speed = MAX_SPEED
self.storedCruiseSpeed = speed
logger.log("[multipoint]: New cruise speed stored: %f m/s." % self.storedCruiseSpeed)
if state is not None and state != self.cruiseState:
self.cruiseState = state
logger.log("[multipoint]: New cruise state set: %d" % self.cruiseState)
if self.cruiseState == RIGHT:
self.cruiseSpeed = self.storedCruiseSpeed * math.copysign(1,self.cruiseState)
self.cable.setTargetP(1.0)
elif self.cruiseState == LEFT:
self.cruiseSpeed = self.storedCruiseSpeed * math.copysign(1,self.cruiseState)
self.cable.setTargetP(0.0)
else:
self.cruiseSpeed = 0
def updatePathTimes(self):
'''Sends min and max path times to app'''
# don't run this function if we're not in Play Mode
if self.cable is None:
logger.log("[multipoint]: Can't estimate cable times. A spline hasn't been generated yet!")
return
# pack it up and send to app
packet = struct.pack('<IIff', app_packet.SOLO_SPLINE_DURATIONS, 8, self.minTime, self.maxTime)
self.shotmgr.appMgr.sendPacket(packet)
logger.log("[multipoint]: Sent times to app.")
def updatePlaybackStatus(self):
if self.cable is None:
logger.log("[multipoint]: A spline hasn't been generated yet!")
return
if self.cable.reachedTarget():
reportP = self.targetP
self.setCruiseSpeed(state = PAUSED)
else:
reportP = self.cable.currentP
packet = struct.pack('<IIfi', app_packet.SOLO_SPLINE_PLAYBACK_STATUS, 8, reportP, self.cruiseState)
self.shotmgr.appMgr.sendPacket(packet)
def handleSeek(self, seek):
(p, cruiseState) = seek
if self.cable is None:
logger.log("[multipoint]: A spline hasn't been generated yet!")
return
if self.attaching:
logger.log("[multipoint]: Can't seek yet, Solo still attaching to cable.")
return
# update cruise state
self.setCruiseSpeed(state = cruiseState)
logger.log("[multipoint]: New SEEK packet received. p: %f, cruiseState: %d" % (p,cruiseState))
# notify the app
self.checkToNotifyApp(notify=True)
def enterRecordMode(self):
logger.log("[multipoint]: Entering RECORD mode.")
# send record to app
packet = struct.pack('<II', app_packet.SOLO_SPLINE_RECORD, 0)
self.shotmgr.appMgr.sendPacket(packet)
logger.log("[multipoint]: Sent RECORD mode to app.")
# Change the vehicle into loiter mode
self.vehicle.mode = VehicleMode("LOITER")
# reset shot
self.resetShot()
def enterPlayMode(self):
logger.log("[multipoint]: Entering PLAY mode.")
if self.cableCamPlaying:
logger.log("[multipoint]: Already in PLAY mode.")
return
# make sure no waypoints are empty
if None in self.waypoints:
logger.log("[multipoint]: Missing at least one keyframe, reverting to RECORD mode.")
self.enterRecordMode()
return
# make sure we have at least 2 waypoints recorded before continuing
if len(self.waypoints) < 2:
logger.log("[multipoint]: Tried to begin multipoint cable with less than 2 keyframes. Reverting to RECORD mode.")
self.enterRecordMode()
return
# generate position and camera splines
if not self.generateSplines(self.waypoints):
logger.log("[multipoint]: Spline generation failed. Reverting to RECORD mode.")
self.enterRecordMode()
return
# send play to app
packet = struct.pack('<II', app_packet.SOLO_SPLINE_PLAY, 0)
self.shotmgr.appMgr.sendPacket(packet)
logger.log("[multipoint]: Sent PLAY mode to app.")
# send path min/max times to app
self.updatePathTimes()
# send all waypoints to app with arc length normalized parameters
for index, pt in enumerate(self.waypoints):
# special case: handle last point
if index == len(self.waypoints) - 1:
seg = index - 1
u = 1
else:
seg = index
u = 0
# calculate p for point (segment usually at index, u = 0, v doesn't
# matter)
p = self.cable.spline.nonDimensionalToArclength(seg, u, 0)[0]
# send point packet to app
self.sendSoloSplinePoint(self.splinePointVersion, self.absAltRef, index, pt.loc.lat, pt.loc.lon, pt.loc.alt, pt.pitch, pt.yaw, p, app_packet.SPLINE_ERROR_NONE)
logger.log("[multipoint]: Sent spline point %d of %d to app." % (index,len(self.waypoints)-1))
# Change the vehicle into guided mode
self.vehicle.mode = VehicleMode("GUIDED")
# set gimbal to MAVLink targeting mode
msg = self.vehicle.message_factory.mount_configure_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, # mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
# send gimbal targeting message to vehicle
self.vehicle.send_mavlink(msg)
# remap the sticks
self.shotmgr.rcMgr.enableRemapping( True )
# set state flag to true (partially unblocks controller)
# (also needs unblock of attaching flag)
self.cableCamPlaying = True
# update button mappings
self.setButtonMappings()
# wait for app to send an attach request
logger.log("[multipoint]: Waiting for ATTACH command from app to continue.") # wait for a SPLINE_ATTACH message to tell us where to go
def generateSplines(self, waypoints):
'''Generate the multi-point spline'''
# shortest distance between each angle
for i in range(0, len(waypoints) - 1):
# calculate difference of yaw and next yaw
delta = self.waypoints[i + 1].yaw - self.waypoints[i].yaw
# don't take the long way around
if abs(delta) > 180:
# add 360 to all following yaws (CW)
if delta < 0.0:
direction = 1
# or remove 360 from all following yaws (CCW)
else:
direction = -1
# list tracking directions
self.yawDirections.append(direction)
# update all following yaws
for j in range(i + 1, len(self.waypoints)):
self.waypoints[j].yaw = self.waypoints[
j].yaw + direction * 360
# generate camera spline
camPts = [Vector2(x.pitch, x.yaw) for x in self.waypoints]
# Generate artificial end points for spline
# extend slope of first two points to generate initial control point
endPt1 = camPts[0] + (camPts[0] - camPts[1])
# extend slope of last two points to generate final control point
endPt2 = camPts[-1] + (camPts[-1] - camPts[-2])
# append virtual control points to cartesian list
camPts = [endPt1] + camPts + [endPt2]
# Build spline object
try:
self.camSpline = CatmullRom(camPts)
except ValueError, e:
logger.log("%s", e)
self.shotMgr.enterShot(shots.APP_SHOT_NONE) # exit the shot
return False
# generate 3d position spline
ctrlPtsLLA = [x.loc for x in self.waypoints]
ctrlPtsCart = []
# set initial control point as origin
ctrlPtsCart.append(Vector3(0, 0, 0))
self.splineOrigin = ctrlPtsLLA[0]
for n in range(1, len(ctrlPtsLLA)):
ctrlPtsCart.append(location_helpers.getVectorFromPoints(self.splineOrigin, ctrlPtsLLA[n]))
ctrlPtsCart[-1].z *= -1. #NED
# Build spline object
try:
self.cable = cableController.CableController(points = ctrlPtsCart, maxSpeed = MAX_SPEED, minSpeed = MIN_CRUISE_SPEED, tanAccelLim = TANGENT_ACCEL_LIMIT, normAccelLim = NORM_ACCEL_LIMIT, smoothStopP = 0.7, maxAlt = self.maxAlt)
except ValueError, e:
logger.log("%s", e)
self.shotMgr.enterShot(shots.APP_SHOT_NONE) # exit the shot
return False
# calculate min and max parametrix velocities for the spline
self.minTime = self.estimateTime(MAX_SPEED)
self.maxTime = min(MAXIMUM_CABLE_DURATION, self.estimateTime(MIN_CRUISE_SPEED))
logger.log("[multipoint]: min time for cable: %f s." % (self.minTime))
logger.log("[multipoint]: max time for cable: %f s." % (self.maxTime))
return True
def estimateTime(self,speed):
'''Tries to guess a time from a given cruiseSpeed (inverse of estimateCruiseSpeed())'''
#AKA f(s) = l/s + s/a [in seconds]
rampUpTime = speed / TANGENT_ACCEL_LIMIT
rampUpDistance = 0.5*TANGENT_ACCEL_LIMIT*(rampUpTime**2)
cruiseTime = (self.cable.spline.totalArcLength - 2.*rampUpDistance) / speed
timeToComplete = (rampUpTime + cruiseTime + rampUpTime)
return timeToComplete
def estimateCruiseSpeed(self,time):
'''Tries to guess a cruiseSpeed from a desired time (inverse of estimateTime())'''
#AKA s^2 - ast + al = 0
# quadratic equation: A = 1, B = -at, C = al
# (-B +/- sqrt(B^2 - 4AC)) / 2A
# solving for f(s) = (at +/- sqrt(a^2 * t^2 - 4*1*al))/2*1
# re-written f(s) = (1/2) * (at +/- sqrt(a^2 * t^2 - 4*a*l)) [in m/s]
speed = 0.5 * (TANGENT_ACCEL_LIMIT * time - math.sqrt(TANGENT_ACCEL_LIMIT**2 * time**2 - 4 * TANGENT_ACCEL_LIMIT * self.cable.spline.totalArcLength))
# speed2 = 0.5 * (TANGENT_ACCEL_LIMIT * time + math.sqrt(TANGENT_ACCEL_LIMIT**2 * time**2 - 4 * TANGENT_ACCEL_LIMIT * self.cable.spline.totalArcLength))
return speed
def handleAttach(self, attach):
'''Requests that the vehicle attach to a cable at the given index'''
(self.attachIndex,) = attach
if not self.cableCamPlaying:
logger.log("[multipoint]: Attach request not valid in RECORD mode. Will not attach.")
return
if self.attachIndex >= len(self.waypoints):
logger.log("[multipoint]: Invalid keypointIndex received. Waypoint range is 0 to %d but received %d. Will not attach." % (len(self.waypoints)-1, self.attachIndex))
self.attachIndex = -1
return
logger.log("[multipoint]: Attaching to spline point %d at %.1f m/s." % (self.attachIndex,ATTACH_SPEED))
# set currentSegment and currentU for this keyframe.
# currentU is 0 unless we're talking about the end point
if self.attachIndex == len(self.waypoints)-1:
attachSeg = self.attachIndex - 1
attachU = 1.
else:
attachSeg = self.attachIndex
attachU = 0.
p = self.cable.spline.nonDimensionalToArclength(attachSeg, attachU)[0]
self.cable.setCurrentP(p)
self.commandPos = self.waypoints[self.attachIndex].loc
# Go to keyframe
self.vehicle.simple_goto(self.commandPos)
# Set attach speed
speedMsg = self.vehicle.message_factory.command_long_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, ATTACH_SPEED, -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
# send command to vehicle
self.vehicle.send_mavlink(speedMsg)
# yaw to appropriate heading
startYaw = self.waypoints[self.attachIndex].yaw
startPitch = self.waypoints[self.attachIndex].pitch
if self.vehicle.mount_status[0] is not None:
pointingMsg = self.vehicle.message_factory.mount_control_encode(
0, 1, # target system, target component
startPitch * 100, # pitch (centidegrees)
0.0, # roll (centidegrees)
startYaw * 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
startYaw, # param 1 - target angle (degrees)
ATTACH_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)
def listenForAttach(self):
'''Checks if vehicle attaches to cable'''
# don't check until a valid attachIndex has been set
if self.attachIndex == -1:
return
# if distance to spline point is less than threshold
distToSpline = location_helpers.getDistanceFromPoints3d(self.vehicle.location.global_relative_frame,self.waypoints[self.attachIndex].loc)
# check if we are in range
if distToSpline < WAYPOINT_NEARNESS_THRESHOLD:
# locked on
logger.log("[multipoint]: Attached to cable at index %d." % self.attachIndex)
# and notify the app that we've attached
packet = struct.pack('<III', app_packet.SOLO_SPLINE_ATTACH, 4, self.attachIndex)
self.shotmgr.appMgr.sendPacket(packet)
logger.log("[multipoint]: Sent attach message to app for index %d." % (self.attachIndex))
# reset attaching flag
self.attaching = False
# reset attachIndex
self.attachIndex = -1
# get APM WPNAV_SPEED parameter to reset speed
speed = self.shotmgr.getParam( "WPNAV_SPEED", DEFAULT_WPNAV_SPEED_VALUE ) / 100.0
# reset MAV_CMD_DO_CHANGE_SPEED
msg = self.vehicle.message_factory.command_long_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, speed, -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
def checkToNotifyApp(self, notify=False):
'''Checks if we should notify the app of our position on spline.
Conditions to notify app:
-if we haven't notified the app within NOTIFICATIONS_PER_SECOND (s)
'''
self.ticksSinceNotify += 1
# notify at 5Hz
if self.ticksSinceNotify >= UPDATE_RATE/NOTIFICATIONS_PER_SECOND or notify == True:
self.ticksSinceNotify = 0
self.updatePlaybackStatus()
def handlePacket(self, packetType, packetLength, packetValue):
try:
if packetType == app_packet.SOLO_RECORD_POSITION:
self.recordLocation()
elif packetType == app_packet.SOLO_SPLINE_RECORD:
self.enterRecordMode()
elif packetType == app_packet.SOLO_SPLINE_PLAY:
self.enterPlayMode()
elif packetType == app_packet.SOLO_SPLINE_POINT:
point = struct.unpack('<hfIddffffh', packetValue)
self.loadSplinePoint(point)
elif packetType == app_packet.SOLO_SPLINE_SEEK:
seek = struct.unpack('<fi', packetValue)
self.handleSeek(seek)
elif packetType == app_packet.SOLO_SPLINE_PATH_SETTINGS:
pathSettings = struct.unpack('<If', packetValue)
self.handlePathSettings(pathSettings)
elif packetType == app_packet.SOLO_SPLINE_ATTACH:
attach = struct.unpack('<I', packetValue)
self.handleAttach(attach)
else:
return False
except Exception as e:
logger.log('[multipoint]: Error handling packet. (%s)' % e)
return False
else:
return True

353
shotmanager/orbit.py Normal file
View File

@ -0,0 +1,353 @@
# orbit.py
# shotmanager
#
# The orbit smart shot.
# Runs as a DroneKit-Python script.
#
# Created by Will Silva and Eric Liao on 1/19/2015.
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from dronekit import Vehicle, LocationGlobalRelative, VehicleMode
from pymavlink import mavutil
import os
from os import sys, path
import math
import struct
sys.path.append(os.path.realpath(''))
import app_packet
import camera
import location_helpers
import pathHandler
import shotLogger
from shotManagerConstants import *
import shots
import socket
import orbitController
from orbitController import OrbitController
from vector3 import Vector3
from sololink import btn_msg
SHALLOW_ANGLE_THRESHOLD = -5
# if we didn't aim at the ground or we don't have a gimbal
MAX_ALT_DIFF = 9999.0
TICKS_TO_IGNORE_PADDLE = 2.0 * UPDATE_RATE
# in degrees per second
YAW_SPEED = 25.0
APP_UPDATE = 15
DEFAULT_PILOT_VELZ_MAX_VALUE = 133.0
ZVEL_FACTOR = 0.95
logger = shotLogger.logger
class OrbitShot():
def __init__(self, vehicle, shotmgr):
# copy vehicle object
self.vehicle = vehicle
# reference shotManager object
self.shotmgr = shotmgr
# initialize roi to None
self.roi = None
# Altitude difference between where the ROI is and where the camera should look
self.ROIAltitudeOffset = 0
# should we inform the app the ROI has moved
self.roi_needsUpdate = False
# counter to send perioidic app updates when needed
self.appNeedsUpdate = 0
# initialize path controller to None
self.pathController = None
# keep track of how long we've kept the paddle centered
self.ticksPaddleCentered = float('inf')
# initialize pathHandler to None
self.pathHandler = None
# get max climb rate from APM parameters
self.maxClimbRate = self.shotmgr.getParam( "PILOT_VELZ_MAX", DEFAULT_PILOT_VELZ_MAX_VALUE ) / 100.0 * ZVEL_FACTOR
# log the max altitude downloaded from APM
logger.log("[orbit]: Maximum climb rate stored: %f" % self.maxClimbRate)
# get max altitude from APM parameters
self.maxAlt = self.shotmgr.getParam( "FENCE_ALT_MAX", DEFAULT_FENCE_ALT_MAX )
# log the max altitude downloaded from APM
logger.log("[orbit]: Maximum altitude stored: %f" % self.maxAlt)
# check APM params to see if Altitude Limit should apply
if self.shotmgr.getParam( "FENCE_ENABLE", DEFAULT_FENCE_ENABLE ) == 0:
self.maxAlt = None
logger.log("[Orbit]: Altitude Limit is disabled.")
# Now change the vehicle into guided mode
self.vehicle.mode = VehicleMode("GUIDED")
# set gimbal targeting mode
msg = self.vehicle.message_factory.mount_configure_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, #mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
self.vehicle.send_mavlink(msg)
# enable stick remappings
self.shotmgr.rcMgr.enableRemapping( True )
# channels are expected to be floating point values in the (-1.0, 1.0) range
def handleRCs( self, channels ):
# don't continue until an roi is set
if not self.pathHandler or not self.roi:
return
# allow user to rotate the ROI about the vehicle
if abs(channels[YAW]) > 0:
# adding 180 flips the az (ROI to vehicle) to (vehicle to ROI)
tmp = self.roi.alt
self.pathController.azimuth += channels[YAW] * YAW_SPEED * UPDATE_TIME
self.roi = location_helpers.newLocationFromAzimuthAndDistance(
self.vehicle.location.global_relative_frame,
180 + self.pathController.azimuth,
self.pathController.radius)
self.roi.alt = tmp
self.roi_needsUpdate = True
# call path controller
pos, vel = self.pathController.move(channels, self.pathHandler.cruiseSpeed, self.roi)
# mavlink expects 10^7 integer for accuracy
latInt = int(pos.lat * 10000000)
lonInt = int(pos.lon * 10000000)
# create the SET_POSITION_TARGET_GLOBAL_INT command
msg = 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
latInt, lonInt, pos.alt, # x, y, z positions
vel.x, vel.y, vel.z, # x, y, z velocity in m/s
0, 0, 0, # x, y, z acceleration (not used)
0, 0) # yaw, yaw_rate (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
# pointing logic
# Adjust the height of the ROI using the paddle
# we pass in both the filtered gimbal paddle value as well as the raw one
self.setROIAltitude( channels[5], channels[7] )
# set ROI
msg = self.vehicle.message_factory.command_long_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_SET_ROI, #command
0, #confirmation
0, 0, 0, 0, #params 1-4
self.roi.lat,
self.roi.lon,
self.roi.alt + self.ROIAltitudeOffset
)
# send pointing message for either cam mode
self.vehicle.send_mavlink(msg)
self.updateApp()
def updateApp(self):
# used to limit the bandwidth needed to transmit many ROI updates
self.appNeedsUpdate += 1
if self.appNeedsUpdate > APP_UPDATE:
self.appNeedsUpdate = 0
if self.roi_needsUpdate:
self.roi_needsUpdate = False
# send this ROI to the app
#logger.log("ROI %f %f" % (self.roi.lat, self.roi.lon))
packet = struct.pack('<IIddf', app_packet.SOLO_MESSAGE_LOCATION, 20, self.roi.lat, self.roi.lon, self.roi.alt)
self.shotmgr.appMgr.sendPacket(packet)
def spotLock(self):
'''take the angle of the copter and lock onto a ground level target'''
logger.log("[Orbit] spotLock")
# don't use a shallow angle resulting in massively distant ROIs
pitch = min(camera.getPitch(self.vehicle), SHALLOW_ANGLE_THRESHOLD)
# Get ROI for the vehicle to look at
spotLock = location_helpers.getSpotLock(self.vehicle.location.global_relative_frame, pitch, camera.getYaw(self.vehicle))
self.addLocation(spotLock)
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 initOrbitShot(self):
'''Initialize the orbit autonomous controller - called once'''
# create pathHandler object
self.pathHandler = pathHandler.PathHandler( self.vehicle, self.shotmgr )
# set button mappings
self.setButtonMappings()
def setButtonMappings(self):
'''Map the controller buttons'''
buttonMgr = self.shotmgr.buttonManager
if self.roi:
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_ORBIT, 0, "\0")
else:
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_ORBIT, btn_msg.ARTOO_BITMASK_ENABLED, "Begin\0")
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_ORBIT, 0, "\0")
def handleButton(self, button, event):
'''Handle a controller button press'''
if button == btn_msg.ButtonA and event == btn_msg.Press:
if self.roi is None:
self.spotLock()
if button == btn_msg.ButtonLoiter and event == btn_msg.Press:
if self.pathHandler:
self.pathHandler.togglePause()
self.updateAppOptions()
self.shotmgr.notifyPause(True)
else:
self.shotmgr.notifyPause(False)
def updateAppOptions(self):
'''send our current set of options to the app'''
packet = struct.pack('<IIf', app_packet.SOLO_SHOT_OPTIONS, 4, self.pathHandler.cruiseSpeed)
self.shotmgr.appMgr.sendPacket(packet)
# Sets our ROI according to the filtered gimbal paddle.
# We know the desired gimbal angle (linear conversion from filtered gimbal paddle value)
# So we just need to convert that to an ROI altitude
# We only use this to set our ROI if the user is actually adjusting it.
# In order to tell, we compare rawPaddleValue to 0.0, but we need to wait a bit
# so that filteredPaddleInput settles
# NOTE: This will not handle preset gimbal animations from Artoo. But
# I don't think there's a way to do that without adding more messaging between Artoo/shot manager.
# Using a preset during an Orbit isn't something anyone's asked for.
#
# Note that we no longer update the actual ROI and instead just maintain an offset that is adjusted for pointing (but the vehicle is still controlled relative to the original ROI)
def setROIAltitude(self, filteredPaddleInput, rawPaddleValue):
# no gimbal, no reason to change ROI
if self.vehicle.mount_status[0] == None:
return
# if the paddle is centered, we tick up our count
# If it's been centered for long enough don't allow the filtered paddle value to
# overwrite our ROI Alt
# We want to wait a little bit after the gimbal paddle returns to center so that
# the filtered value stabilizes
if rawPaddleValue == 0.0:
self.ticksPaddleCentered += 1
if self.ticksPaddleCentered > TICKS_TO_IGNORE_PADDLE:
return
else:
self.ticksPaddleCentered = 0
# our input is in the range (-1.0, 1.0)
# let's convert it to a desired angle, where -1.0 -> -90.0 and 1.0 -> 0.0
input = ( filteredPaddleInput + 1.0 ) / 2.0
# inverse it
input = 1.0 - input
# convert to gimbal range in radians
theta = input * math.radians(90.0)
# now we want to calculate our altitude difference of our ROI
# we want our ROI to be at angle theta from our copter
# tan theta = altDiff / radius
# altDiff = radius * tan theta
tanTheta = math.tan(theta)
altDiff = self.pathController.radius * tanTheta
# In the case that we're pointing straight down,
# cap the number we're sending, because otherwise it'll cause issues.
if altDiff > MAX_ALT_DIFF:
altDiff = MAX_ALT_DIFF
# XXX suspect - why bring in vehicle alt to open loop controller?
self.ROIAltitudeOffset = self.vehicle.location.global_relative_frame.alt - altDiff
def handlePacket(self, packetType, packetLength, packetValue):
try:
if packetType == app_packet.SOLO_RECORD_POSITION:
logger.log("[orbit]: record spotlock")
self.spotLock()
elif packetType == app_packet.SOLO_MESSAGE_LOCATION:
(lat, lon, alt) = struct.unpack('<ddf', packetValue)
logger.log("[orbit]: Location received from app: %f, %f, %f." %( lat, lon, alt ) )
# forces the controller to reset the pathhandler
self.roi = None
self.addLocation(LocationGlobalRelative(lat, lon, alt))
elif packetType == app_packet.SOLO_SHOT_OPTIONS:
if self.pathHandler:
(cruiseSpeed,) = struct.unpack('<f', packetValue)
self.pathHandler.setCruiseSpeed(cruiseSpeed)
logger.log("[orbit]: Cruise speed set to %.2f." % (cruiseSpeed,))
else:
return False
except Exception as e:
logger.log('[orbit]: Error handling packet. (%s)' % e)
return False
else:
return True

236
shotmanager/orbitController.py Normal file
View File

@ -0,0 +1,236 @@
# orbitController.py
# shotmanager
#
# The orbit movement controller.
#
# Runs as a DroneKit-Python script
#
# Created by Will Silva and Eric Liao on 1/19/2015.
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Usage:
# Instantiated by Orbit and Follow shots.
# Inherited by leashController.
from pymavlink import mavutil
import location_helpers
import shotLogger
from shotManagerConstants import *
import math
from vector3 import Vector3
logger = shotLogger.logger
#Path accel/decel constants
PATH_ACCEL = 2.5
ACCEL_PER_TICK = PATH_ACCEL * UPDATE_TIME
ORBIT_MIN_SPEED = 3.0
ORBIT_MAX_SPEED = 8.0
ORBIT_RAD_FOR_MIN_SPEED = 2.0
ORBIT_RAD_FOR_MAX_SPEED = 30.0
THROTTLE = 0
ROLL = 1
PITCH = 2
YAW = 3
RAW_PADDLE = 7
class OrbitController(object):
'''A cylindrical-coordinates based orbit motion controller than be permuted by user-sticks or a strafe cruise speed'''
def __init__(self, roi, radius, azimuth, zOffset):
'''Initialize the controller at a cylindrical coordinate relative to an ROI origin'''
self.roi = roi
self.radius = radius
self.azimuth = azimuth
self.zOffset = zOffset
self.approachSpeed = 0.0
self.strafeSpeed = 0.0
# options attributes (never reset unless object is destroyed)
self.maxAlt = None
self.maxClimbRate = None
def setOptions(self, maxClimbRate, maxAlt):
'''Sets maximum limits in open-loop controller'''
self.maxClimbRate = maxClimbRate
self.maxAlt = maxAlt
def move(self, channels, cruiseSpeed, newroi=None, roiVel=None):
'''Handles RC channels and a given strafeSpeed 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 roiVel is None:
roiVel = Vector3(0,0,0)
# Approach
approachVel = self._approach(-channels[PITCH])
# Strafe
strafeVel = self._strafe(channels[ROLL], cruiseSpeed)
# Climb
climbVel = self._climb(channels[THROTTLE])
# calculate new LLA position
if newroi is not None:
self.roi = newroi
currentPos = location_helpers.newLocationFromAzimuthAndDistance(self.roi, self.azimuth, self.radius)
# set altitude (This is for generality with follow. In the Orbit shot, roi.alt should be zero.)
currentPos.alt = self.roi.alt + self.zOffset
# check altitude limit
if self.maxAlt is not None:
currentPos.alt = min(currentPos.alt, self.maxAlt)
# sum velocities
currentVel = approachVel + strafeVel + climbVel + roiVel
return currentPos, currentVel
def _approach(self,stick):
'''Handles approach stick input to grow or shrink orbit radius'''
# set to vehicle max speed
maxSpeed = MAX_SPEED
# Approach speed is controlled by user PITCH stick
speed = stick * maxSpeed
# Synthetic acceleration limit
if speed > self.approachSpeed:
self.approachSpeed += ACCEL_PER_TICK
self.approachSpeed = min(self.approachSpeed, speed)
elif speed < self.approachSpeed:
self.approachSpeed -= ACCEL_PER_TICK
self.approachSpeed = max(self.approachSpeed, speed)
else:
self.approachSpeed = speed
# update current radius
self.radius -= ( self.approachSpeed * UPDATE_TIME )
# az from vehicle to ROI
#az = location_helpers.calcAzimuthFromPoints(self.vehicle.location, self.roi)
az = location_helpers.wrapTo360(self.azimuth + 180.0)
az = math.radians( az )
# rotate vehicle from entering min radius
xVel = math.cos(az) * self.approachSpeed
yVel = math.sin(az) * self.approachSpeed
# stop vehicle from entering min radius
if self.radius < ORBIT_RAD_FOR_MIN_SPEED:
self.radius = ORBIT_RAD_FOR_MIN_SPEED
xVel = yVel = 0.0
return Vector3(xVel,yVel,0)
def _strafe(self, stick, strafeSpeed = 0):
'''Handles strafe stick input and cruiseSpeed to strafe left or right on the circle'''
# get max speed at current radius
maxSpeed = self._maxStrafeSpeed(self.radius)
# cruiseSpeed can be permuted by user ROLL stick
if strafeSpeed == 0.0:
speed = stick * maxSpeed
else:
speed = abs(strafeSpeed)
# if sign of stick and cruiseSpeed don't match then...
if math.copysign(1,stick) != math.copysign(1,strafeSpeed): # slow down
speed *= (1.0 - abs(stick))
else: # speed up
speed += (maxSpeed - speed) * abs(stick)
# carryover user input sign
if strafeSpeed < 0:
speed = -speed
# limit speed
if speed > maxSpeed:
speed = maxSpeed
elif -speed > maxSpeed:
speed = -maxSpeed
# Synthetic acceleration limit
if speed > self.strafeSpeed:
self.strafeSpeed += ACCEL_PER_TICK
self.strafeSpeed = min(self.strafeSpeed, speed)
elif speed < self.strafeSpeed:
self.strafeSpeed -= ACCEL_PER_TICK
self.strafeSpeed = max(self.strafeSpeed, speed)
else:
self.strafeSpeed = speed
# circumference of current circle
circum = self.radius * 2.0 * math.pi
# # of degrees of our circle to shoot for moving to this update
# had problems when we made this too small
degrees = abs(self.strafeSpeed) * UPDATE_TIME / circum * 360.0
# rotate heading by the number of degrees/update
if self.strafeSpeed > 0:
self.azimuth -= degrees
else:
self.azimuth += degrees
# make sure we keep it 0-360
self.azimuth = location_helpers.wrapTo360(self.azimuth)
# generate a velocity tangent to our circle from our destination point
if self.strafeSpeed > 0:
tangent = self.azimuth - 90.0
else:
tangent = self.azimuth + 90.0
tangent = math.radians(tangent)
#based on that, construct a vector to represent what direction we should go in, scaled by our speed
xVel = math.cos(tangent) * abs(self.strafeSpeed)
yVel = math.sin(tangent) * abs(self.strafeSpeed)
return Vector3(xVel,yVel,0)
def _climb(self, stick):
'''Handles altitude stick input to increase or decrease altitude'''
# Z is down, so invert it
zVel = stick * self.maxClimbRate
self.zOffset += zVel * UPDATE_TIME
return Vector3(0,0,zVel)
def _maxStrafeSpeed(self, radius):
'''Returns maximum strafe speed as a function of orbit radius'''
# start out at max orbit strafe speed
maxSpeed = ORBIT_MAX_SPEED
# scale down max speed if we're really close to our target
if self.radius < ORBIT_RAD_FOR_MIN_SPEED:
maxSpeed = ORBIT_MIN_SPEED
elif self.radius >= ORBIT_RAD_FOR_MAX_SPEED:
maxSpeed = ORBIT_MAX_SPEED
else:
ratio = (self.radius - ORBIT_RAD_FOR_MIN_SPEED) / (ORBIT_RAD_FOR_MAX_SPEED - ORBIT_RAD_FOR_MIN_SPEED)
maxSpeed = ORBIT_MIN_SPEED + ((ORBIT_MAX_SPEED - ORBIT_MIN_SPEED) * ratio)
return maxSpeed

497
shotmanager/pano.py Normal file
View File

@ -0,0 +1,497 @@
#
# pano.py
# shotmanager
#
# The pano smart shot controller.
# Runs as a DroneKit-Python script under MAVProxy.
#
# Created by Jason Short and Will Silva on 11/30/2015.
# Copyright (c) 2015 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from dronekit import Vehicle, LocationGlobalRelative, VehicleMode
from pymavlink import mavutil
import os
from os import sys, path
import math
import struct
sys.path.append(os.path.realpath(''))
import app_packet
import camera
import location_helpers
import shotLogger
import shots
from shotManagerConstants import *
import GoProManager
from GoProConstants import *
from sololink import btn_msg
# in degrees per second
YAW_SPEED = 10.0
# Cylinder Pano constants
CYLINDER_LENS_ANGLE = 160.0
PANO_CAPTURE_DELAY = int(UPDATE_RATE * 2.5)
PANO_MOVE_DELAY = int(UPDATE_RATE * 3.5)
PANO_YAW_SPEED = 60.0 # deg/s
PANO_PITCH_SPEED = 60.0 # deg/s
PANO_DEFAULT_VIDEO_YAW_RATE = 2.0
PANO_DEFAULT_FOV = 220
MAX_YAW_RATE = 60 # deg/s
MAX_YAW_ACCEL_PER_TICK = (MAX_YAW_RATE) / (4 * UPDATE_RATE) # deg/s^2/tick
# modes for Pano
PANO_CYLINDER = 0
PANO_SPHERE = 1
PANO_VIDEO = 2
PANO_SETUP = 0
PANO_RUN = 1
PANO_EXIT = 2
VIDEO_COUNTER = 15
TICKS_TO_BEGIN = -25
logger = shotLogger.logger
class PanoShot():
def __init__(self, vehicle, shotmgr):
# assign the vehicle object
self.vehicle = vehicle
# assign the shotManager object
self.shotmgr = shotmgr
# ticks to track timing in shot
self.ticks = 0
#state machine
self.state = PANO_SETUP
# Default panoType to None
self.panoType = PANO_CYLINDER
# steps to track incrementing in shot
self.step = 0
self.stepsTotal = 0
# Yaw rate for Video Pano shot
self.degSecondYaw = PANO_DEFAULT_VIDEO_YAW_RATE
# default FOV for Cylinder Pano shot
self.cylinder_fov = PANO_DEFAULT_FOV
self.lensFOV = CYLINDER_LENS_ANGLE
# list of angles in Cylinder Pano shot
self.cylinderAngles = None
self.sphereAngles = None
# default camPitch
self.camPitch = camera.getPitch(self.vehicle)
# default camYaw to current pointing
self.camYaw = camera.getYaw(self.vehicle)
# only used for non-gimbaled copters
self.camDir = 1
self.counter = 0
# set button mappings on Artoo
self.setButtonMappings()
# switch vehicle into GUIDED mode
self.vehicle.mode = VehicleMode("GUIDED")
# switch gimbal into MAVLINK TARGETING mode
self.setupTargeting()
# give the app our defaults
self.updateAppOptions()
# take over RC
self.shotmgr.rcMgr.enableRemapping( True )
# channels are expected to be floating point values in the (-1.0, 1.0)
# range
def handleRCs(self, channels):
if self.panoType == PANO_VIDEO:
self.runVideo(channels)
self.manualPitch(channels)
self.handlePitchYaw()
else:
if self.state == PANO_SETUP:
self.manualPitch(channels)
self.manualYaw(channels[YAW])
self.handlePitchYaw()
else:
if self.panoType == PANO_SPHERE:
self.runSphere()
self.handlePitchYaw()
elif self.panoType == PANO_CYLINDER:
self.runCylinder()
self.handlePitchYaw()
def initPano(self):
# We reset to the beginning for all states
if self.panoType == PANO_SPHERE:
logger.log("[PANO]: Init Sphere")
self.enterPhotoMode()
self.initSphere()
elif self.panoType == PANO_CYLINDER:
logger.log("[PANO]: Init Cylinder")
self.enterPhotoMode()
self.initCylinder()
else:
logger.log("[PANO]: Init Video Pan")
def resetPano(self):
self.cylinderAngles = None
self.sphereAngles = None
def initSphere(self):
'''Initialize the cylinder pano shot'''
# give first move some room to get there
self.ticks = TICKS_TO_BEGIN
# Store angle presets in array
# more steps
if self.lensFOV < 140:
self.sphereAngles = [[-90,0], [-45,300], [-45,240], [-45,180], [-45,120], [-45,60], [-45,0], [0,315], [0,270], [0,225], [0,180], [0,135], [0,90], [0,45], [0,0]]
else:
# Fewer steps
self.sphereAngles = [[-90,0], [-45,270], [-45,180], [-45,90], [-45,0], [0,300], [0,240], [0,180], [0,120], [0,60], [0,0]]
self.stepsTotal = len(self.sphereAngles)
self.updatePanoStatus(0, self.stepsTotal)
# go to first angle
tmp = self.sphereAngles.pop()
self.camPitch = tmp[0]
self.camYaw = tmp[1]
def runSphere(self):
'''Run the Hemi pano program'''
self.ticks += 1
if self.sphereAngles is None:
self.initPano()
if self.state == PANO_RUN:
# time delay between camera moves
if self.ticks == PANO_CAPTURE_DELAY:
self.shotmgr.goproManager.handleRecordCommand(self.shotmgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE)
self.updatePanoStatus((self.stepsTotal) - len(self.sphereAngles), self.stepsTotal)
if self.ticks > PANO_MOVE_DELAY:
self.ticks = 0
if len(self.sphereAngles) == 0:
self.state = PANO_EXIT
# grab a last photo?
else:
# select new angle
tmp = self.sphereAngles.pop()
self.camPitch = tmp[0]
self.camYaw = tmp[1]
elif self.state == PANO_EXIT:
self.camPitch = -10
logger.log("[PANO]: completed Hemi")
# let user take a new Pano
self.state = PANO_SETUP
self.resetPano()
self.updateAppOptions()
self.setButtonMappings()
def initCylinder(self):
'''Initialize the cylinder pano shot'''
# limit the inputed FOV from App
self.cylinder_fov = max(self.cylinder_fov, 91.0)
self.cylinder_fov = min(self.cylinder_fov, 360.0)
self.yaw_total = self.cylinder_fov - (self.lensFOV/4)
steps = math.ceil(self.cylinder_fov / (self.lensFOV/4))
num_photos = math.ceil(self.cylinder_fov / (self.lensFOV/4))
yawDelta = self.yaw_total / (num_photos - 1)
self.origYaw = camera.getYaw(self.vehicle)
yawStart = self.origYaw - (self.yaw_total / 2.0)
self.cylinderAngles = []
for x in range(0, int(steps)):
tmp = yawStart + (x * yawDelta)
if tmp < 0:
tmp += 360
elif tmp > 360:
tmp -= 360
self.cylinderAngles.append(int(tmp))
self.stepsTotal = len(self.cylinderAngles)
# go to first angle
self.camYaw = self.cylinderAngles.pop()
# give first move an extra second to get there
self.ticks = TICKS_TO_BEGIN
self.updatePanoStatus(0, self.stepsTotal)
def runCylinder(self):
'''Run the Cylinder pano program'''
self.ticks += 1
if self.cylinderAngles is None:
self.initPano()
if self.state == PANO_RUN:
# time delay between camera moves
if self.ticks == PANO_CAPTURE_DELAY:
self.shotmgr.goproManager.handleRecordCommand(self.shotmgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE)
self.updatePanoStatus((self.stepsTotal) - len(self.cylinderAngles), self.stepsTotal)
if self.ticks > PANO_MOVE_DELAY:
self.ticks = 0
if len(self.cylinderAngles) == 0:
self.state = PANO_EXIT
else:
# select new angle
self.camYaw = self.cylinderAngles.pop()
elif self.state == PANO_EXIT:
self.camYaw = self.origYaw
# let user take a new Pano
self.state = PANO_SETUP
self.resetPano()
self.updateAppOptions()
self.setButtonMappings()
def runVideo(self, channels):
'''Run the Video pano program'''
# modulate yaw rate based on yaw stick input
if channels[YAW] != 0:
self.degSecondYaw = self.degSecondYaw + (channels[YAW] * MAX_YAW_ACCEL_PER_TICK)
# limit yaw rate
self.degSecondYaw = min(self.degSecondYaw, MAX_YAW_RATE)
self.degSecondYaw = max(self.degSecondYaw, -MAX_YAW_RATE)
# increment desired yaw angle
self.camYaw += (self.degSecondYaw * UPDATE_TIME)
self.camYaw %= 360.0
#logger.log("[PANO]: self.camYaw %f" % self.camYaw)
self.counter += 1
if self.counter > VIDEO_COUNTER:
self.counter = 0
self.updateAppOptions()
# send our current set of options to the app
def updateAppOptions(self):
# B = uint_8
# h = int_16
# f = float_32
# d = float_64
packet = struct.pack('<IIBBhff', app_packet.SOLO_PANO_OPTIONS, 12, self.panoType, self.state, self.cylinder_fov, self.degSecondYaw, self.lensFOV)
self.shotmgr.appMgr.sendPacket(packet)
def updatePanoStatus(self, _index, _total):
#logger.log("[Pano]: steps %d of %d" % (_index, _total))
packet = struct.pack('<IIBB', app_packet.SOLO_PANO_STATE, 2, _index, _total)
self.shotmgr.appMgr.sendPacket(packet)
def handlePacket(self, packetType, packetLength, packetValue):
try:
if packetType == app_packet.SOLO_PANO_OPTIONS:
(self.panoType, _run, self.cylinder_fov, self.degSecondYaw, self.lensFOV) = struct.unpack('<BBhff', packetValue)
logger.log("[PANO]: panoType %d" % self.panoType)
logger.log("[PANO]: state %d" % _run)
logger.log("[PANO]: cylinder_fov %d" % self.cylinder_fov)
logger.log("[PANO]: video degSecondYaw %f" % self.degSecondYaw)
logger.log("[PANO]: lens fov %f" % self.lensFOV)
# range limit lens
self.lensFOV = max(self.lensFOV, 60)
self.lensFOV = min(self.lensFOV, 180)
# tell main loop to run Pano
if _run == PANO_RUN:
self.state = PANO_RUN
else:
self.resetPano()
self.state = PANO_SETUP
self.setButtonMappings()
else:
return False
except Exception as e:
logger.log('[PANO]: Error handling packet. (%s)' % e)
return False
else:
return True
def setButtonMappings(self):
buttonMgr = self.shotmgr.buttonManager
if self.panoType == PANO_VIDEO:
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_PANO, 0, "\0")
else:
if self.state == PANO_RUN:
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_PANO, btn_msg.ARTOO_BITMASK_ENABLED, "Cancel\0")
else:
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_PANO, btn_msg.ARTOO_BITMASK_ENABLED, "Begin\0")
if self.state == PANO_SETUP:
if self.panoType == PANO_VIDEO:
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_PANO, btn_msg.ARTOO_BITMASK_ENABLED, "Video\0")
elif self.panoType == PANO_SPHERE:
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_PANO, btn_msg.ARTOO_BITMASK_ENABLED, "Sphere\0")
elif self.panoType == PANO_CYLINDER:
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_PANO, btn_msg.ARTOO_BITMASK_ENABLED, "Cylinder\0")
else:
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_PANO, 0, "\0")
# if we can handle the button we do
def handleButton(self, button, event):
if button == btn_msg.ButtonA and event == btn_msg.Press:
# don't use A button for video ever
if self.panoType != PANO_VIDEO:
if self.state == PANO_SETUP:
self.state = PANO_RUN
else:
# enter standby
self.resetPano()
self.state = PANO_SETUP
logger.log("[PANO]: Cancel Pano")
# if we are in video mode, stop Yawing
if self.panoType == PANO_VIDEO:
self.degSecondYaw = 0
self.setButtonMappings()
if button == btn_msg.ButtonLoiter and event == btn_msg.Press:
if self.panoType == PANO_VIDEO:
self.degSecondYaw = 0
# cycle through options
if self.state == PANO_SETUP and button == btn_msg.ButtonB and event == btn_msg.Press:
#clear Pano Video yaw speed
self.degSecondYaw = 0
if self.panoType == PANO_VIDEO:
self.panoType = PANO_SPHERE
elif self.panoType == PANO_SPHERE:
self.panoType = PANO_CYLINDER
elif self.panoType == PANO_CYLINDER:
self.panoType = PANO_VIDEO
self.setButtonMappings()
# tell the app what's happening
self.updateAppOptions()
def setupTargeting(self):
# set gimbal targeting mode
msg = self.vehicle.message_factory.mount_configure_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, #mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
self.vehicle.send_mavlink(msg)
def manualPitch(self, channels):
if abs(channels[THROTTLE]) > abs(channels[RAW_PADDLE]):
value = channels[THROTTLE]
else:
value = channels[RAW_PADDLE]
self.camPitch += value * PANO_PITCH_SPEED * UPDATE_TIME
if self.camPitch > 0.0:
self.camPitch = 0.0
elif self.camPitch < -90:
self.camPitch = -90
def manualYaw(self, stick):
if stick == 0:
return
self.camYaw += stick * PANO_YAW_SPEED * UPDATE_TIME
if stick > 0:
self.camDir = 1
else:
self.camDir = -1
self.camYaw = location_helpers.wrapTo360(self.camYaw)
def handlePitchYaw(self):
'''Send pitch and yaw commands to gimbal or fixed mount'''
# if we do have a gimbal, use mount_control to set pitch and yaw
if self.vehicle.mount_status[0] is not None:
msg = self.vehicle.message_factory.mount_control_encode(
0, 1, # target system, target component
# pitch is in centidegrees
self.camPitch * 100,
0.0, # roll
# yaw is in centidegrees
self.camYaw * 100,
0 # save position
)
else:
# if we don't have a gimbal, just set CONDITION_YAW
msg = self.vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
0, #confirmation
self.camYaw, # param 1 - target angle
YAW_SPEED, # param 2 - yaw speed
self.camDir, # param 3 - direction
0.0, # relative offset
0, 0, 0 # params 5-7 (unused)
)
self.vehicle.send_mavlink(msg)
def enterPhotoMode(self):
# switch into photo mode if we aren't already in it
self.shotmgr.goproManager.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, (CAPTURE_MODE_PHOTO, 0 ,0 , 0))

147
shotmanager/pathHandler.py Normal file
View File

@ -0,0 +1,147 @@
#
# Code common across shots to handle movement on paths
#
from pymavlink import mavutil
import location_helpers
import shotLogger
from shotManagerConstants import *
import math
import shots
logger = shotLogger.logger
#Path accel/decel constants
PATH_ACCEL = 6
ACCEL_PER_TICK = PATH_ACCEL * UPDATE_TIME
class PathHandler():
def __init__(self, vehicle, shotmgr):
self.vehicle = vehicle
self.shotMgr = shotmgr
# Automatic cruising of the cable
self.cruiseSpeed = 0.0
self.resumeSpeed = 0.0
def pause(self):
self.resumeSpeed = self.cruiseSpeed
self.setCruiseSpeed(0.0)
logger.log("[%s]: Pausing cruise" % shots.SHOT_NAMES[self.shotMgr.currentShot].lower())
def togglePause(self):
if self.isPaused():
self.resume()
else:
self.pause()
def resume(self):
self.setCruiseSpeed(0.0) # self.resumeSpeed to enable actual resume speed
logger.log("[%s]: Resuming cruise." % shots.SHOT_NAMES[self.shotMgr.currentShot].lower())
def isPaused(self):
return self.cruiseSpeed == 0.0
def setCruiseSpeed(self, speed):
self.cruiseSpeed = speed
# special case of PathHandler
class TwoPointPathHandler(PathHandler):
def __init__(self, pt1, pt2, vehicle, shotmgr):
PathHandler.__init__(self, vehicle, shotmgr)
self.pt1 = pt1
self.pt2 = pt2
self.curTarget = None
#for simulated acceleration
self.currentSpeed = 0.0
self.desiredSpeed = 0.0
# given RC input, calculate a speed and use it to
# move towards one of our endpoints
# return the speed we set for the copter
def MoveTowardsEndpt( self, channels ):
# allow both up/down and left/right to move along the cable
# use the max of them
if abs(channels[1]) > abs(channels[2]):
value = channels[1]
else:
value = -channels[2]
# user controls speed
if self.cruiseSpeed == 0.0:
self.desiredSpeed = value * MAX_SPEED
# cruise control
else:
speed = abs(self.cruiseSpeed)
# if sign of stick and cruiseSpeed don't match then...
if math.copysign(1,value) != math.copysign(1,self.cruiseSpeed): # slow down
speed *= (1.0 - abs(value))
else: # speed up
speed += (MAX_SPEED - speed) * abs(value)
# carryover user input sign
if self.cruiseSpeed < 0:
speed = -speed
# limit speed
if speed > MAX_SPEED:
speed = MAX_SPEED
elif -speed > MAX_SPEED:
speed = -MAX_SPEED
self.desiredSpeed = speed
# Synthetic acceleration
if self.desiredSpeed > self.currentSpeed:
self.currentSpeed += ACCEL_PER_TICK
self.currentSpeed = min(self.currentSpeed, self.desiredSpeed)
elif self.desiredSpeed < self.currentSpeed:
self.currentSpeed -= ACCEL_PER_TICK
self.currentSpeed = max(self.currentSpeed, self.desiredSpeed)
else:
self.currentSpeed = self.desiredSpeed
#Check direction of currentSpeed
goingForwards = self.currentSpeed > 0.0
if goingForwards:
target = self.pt2
else:
target = self.pt1
if target != self.curTarget: #switching target and logging
self.vehicle.simple_goto(target)
self.curTarget = target
logger.log("[%s]: Going to pt %d"%(shots.SHOT_NAMES[self.shotMgr.currentShot].lower(), 2 if goingForwards else 1 ) )
logger.log("[%s]: Target is %.12f, %.12f, %.12f."%
(shots.SHOT_NAMES[self.shotMgr.currentShot].lower(), target.lat, target.lon,
target.alt))
# should replace with a dronekit command when it gets in there
msg = self.vehicle.message_factory.command_long_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, abs(self.currentSpeed), -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
return abs(self.currentSpeed), goingForwards
# returns if we're near our target or not
def isNearTarget(self):
if self.desiredSpeed > 0.0 and \
location_helpers.getDistanceFromPoints3d( \
self.vehicle.location.global_relative_frame, \
self.pt2 ) < WAYPOINT_NEARNESS_THRESHOLD:
return True
elif self.desiredSpeed < 0.0 and \
location_helpers.getDistanceFromPoints3d( \
self.vehicle.location.global_relative_frame, \
self.pt1 ) < WAYPOINT_NEARNESS_THRESHOLD:
return True
return False

11
shotmanager/prereqs/.mavinit.scr Normal file
View File

@ -0,0 +1,11 @@
module load dronekit.module.api
module unload terrain
module unload log
module unload battery
module unload fence
module unload rally
module unload calibration
module unload relay
module unload misc
module unload rc
api start /usr/bin/main.py

47
shotmanager/prereqs/inittab Normal file
View File

@ -0,0 +1,47 @@
# /etc/inittab: init(8) configuration.
# $Id: inittab,v 1.91 2002/01/25 13:35:21 miquels Exp $
# The default runlevel.
id:3:initdefault:
# Boot-time system configuration/initialization script.
# This is run first except when booting in emergency (-b) mode.
si::sysinit:/etc/init.d/rcS
# What to do in single-user mode.
~~:S:wait:/sbin/sulogin
# /etc/init.d executes the S and K scripts upon change
# of runlevel.
#
# Runlevel 0 is halt.
# Runlevel 1 is single-user.
# Runlevel 2 is multi-user.
# Runlevel 3 is STANDBY (controller/solo not paired).
# Runlevel 4 is READY or FLIGHT (all flight code running).
# Runlevel 5 is MAINTENANCE.
# Runlevel 6 is reboot.
l0:0:wait:/etc/init.d/rc 0
l1:1:wait:/etc/init.d/rc 1
l2:2:wait:/etc/init.d/rc 2
l3:3:wait:/etc/init.d/rc 3
l4:4:wait:/etc/init.d/rc 4
l5:5:wait:/etc/init.d/rc 5
l6:6:wait:/etc/init.d/rc 6
# Normally not reached, but fallthrough in case of emergency.
z6:6:respawn:/sbin/sulogin
LOGD:345:respawn:logd.py
PAIR:3:respawn:pair_solo.py
CONN:45:respawn:pair_solo_connected.py
RCRX:4:respawn:pixrc
VID:4:respawn:video_send.sh
MAV:4:respawn:telem_forwarder
API:4:respawn:runMavproxy.sh
2:2345:respawn:/sbin/getty 115200 ttyGS0 vt100
BLNK:2345:once:`echo phy0assoc >> /sys/class/leds/user2/trigger`
mxc::respawn:/etc/init.d/rc_mxc.S

218
shotmanager/rcManager.py Normal file
View File

@ -0,0 +1,218 @@
#
# Handles RC (radio control) connection state and IO.
# Created by Will Silva on 3/5/2016.
# Updated by Jason Short 4/6/2016.
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import os
import socket
import sys
import shotLogger
from sololink import rc_pkt
sys.path.append(os.path.realpath(''))
from shotManagerConstants import *
from sololink import rc_ipc
from sololink import rc_pkt
import shots
import rcManager
logger = shotLogger.logger
DEFAULT_RC_MIN = 1000
DEFAULT_RC_MAX = 2000
DEFAULT_RC_MID = ( DEFAULT_RC_MAX - DEFAULT_RC_MIN ) / 2.0 + DEFAULT_RC_MIN
# These channels have different ranges
CHANNEL6_MIN = 1000
CHANNEL6_MAX = 1520
CHANNEL6_MID = ( CHANNEL6_MAX - CHANNEL6_MIN ) / 2.0 + CHANNEL6_MIN
CHANNEL8_MIN = 0
CHANNEL8_MAX = 1000
CHANNEL8_MID = ( CHANNEL8_MAX - CHANNEL8_MIN ) / 2.0 + CHANNEL8_MIN
THROTTLE_FAILSAFE = 950
DEAD_ZONE = 0.009
TICKS_UNTIL_RC_STALE = 20 # ticks
RC_TCP_BUFSIZE = 1024
class rcManager():
def __init__(self, shotmgr):
self.shotmgr = shotmgr
self.connected = False
self.bindServer()
# Manage flow of RC data
self.numTicksSinceRCUpdate = 0
self.timestamp = 0
self.sequence = 0
self.channels = None
# True if we have RC link
self.failsafe = False
if os.path.exists( "/run/rc_uplink_cmd" ):
self.server.sendto("attach", "/run/rc_uplink_cmd")
# only log this once
self.loggedRC_ipc = False
# We are sending the RC data to PixRC and disabling CH Yaw
# Force off on startup
self.remappingSticks = True
self.enableRemapping(False)
def bindServer(self):
# set up a socket to receive rc inputs from pixrc
if os.path.exists( "/tmp/shotManager_RCInputs_socket" ):
os.remove( "/tmp/shotManager_RCInputs_socket" )
self.server = socket.socket( socket.AF_UNIX, socket.SOCK_DGRAM )
self.server.bind("/tmp/shotManager_RCInputs_socket")
self.server.setblocking(0)
# This is called whenever we have data on the rc socket, which should be
# at 50 hz, barring any drops
# We remap/normalize the RC and then store it away for use in Tick()
def parse(self):
datagram = self.server.recv(RC_TCP_BUFSIZE)
if datagram == None or len(datagram) != rc_pkt.LENGTH:
self.channels = None
return
self.timestamp, self.sequence, self.channels = rc_pkt.unpack(datagram)
if self.channels[THROTTLE] > THROTTLE_FAILSAFE:
self.numTicksSinceRCUpdate = 0
else:
# we are in failsafe so don't cache data - we'll send defaults
self.channels = None
def rcCheck(self):
self.numTicksSinceRCUpdate += 1
if self.numTicksSinceRCUpdate > TICKS_UNTIL_RC_STALE:
if self.failsafe == False:
logger.log( "[RC] Enter failsafe")
self.triggerFailsafe(True)
else:
if self.failsafe == True:
logger.log( "[RC] Exit failsafe")
self.triggerFailsafe(False)
def isRcConnected(self):
return self.numTicksSinceRCUpdate < TICKS_UNTIL_RC_STALE
"""
This remaps all of our RC input into (-1.0, 1.0) ranges.
The RC channels come in as PWM values of (1000, 2000)
Filtered camera paddle is in channel 6 (index 5), and has range (1000, 1520) for some odd reason.
Raw camera paddle is channel 8 (index 7) and is of range (0, 1000)
This function is responsible for sending RC data back to PixRC for remapping purposes.
However, it will only do this if the sendToPixRC param is set to True and self.remappingSticks is set
"""
def remap(self):
if self.failsafe or self.channels == None:
# send default values to the Pixhawk
self.channels = [DEFAULT_RC_MID, DEFAULT_RC_MID, DEFAULT_RC_MID, DEFAULT_RC_MID, DEFAULT_RC_MIN, CHANNEL6_MAX, DEFAULT_RC_MIN, CHANNEL8_MID ]
normChannels = [0]*8
# channels 1-4
for i in range(4):
normChannels[i] = self.normalizeRC( self.channels[i], DEFAULT_RC_MIN, DEFAULT_RC_MAX )
#logger.log("FP %d, RP %d" % (self.channels[FILTERED_PADDLE], self.channels[RAW_PADDLE]))
# channel 6 (index 5) is the filtered gimbal paddle value
# its values go from CHANNEL6_MIN - CHANNEL6_MAX
# this value is used directly to point the gimbal
# 1520 = level, 1000 = straight down
normChannels[FILTERED_PADDLE] = self.normalizeRC( self.channels[FILTERED_PADDLE], CHANNEL6_MIN, CHANNEL6_MAX )
# channel 8 (index 7) is the raw gimbal paddle and is a special case
# its values go from CHANNEL8_MIN - CHANNEL8_MAX
# this value is used to understand user input
# >500 = tilt up, 500 = no tilt, < 500 tilt down
normChannels[RAW_PADDLE] = self.normalizeRC( self.channels[RAW_PADDLE], CHANNEL8_MIN, CHANNEL8_MAX)
if self.remappingSticks:
# never allow Yaw to rotate in guided shots to prevent shot confusion
self.channels[YAW] = 1500
if not rc_ipc.put((self.timestamp, self.sequence, self.channels)):
if not self.loggedRC_ipc:
logger.log( "ERROR returned from rc_ipc.put" )
self.loggedRC_ipc = True
return normChannels
# convert from RC input values to (-1.0, 1.0) floating point value
# min/max is customizable to handle inputs of different ranges
def normalizeRC(self, value, min, max):
# failsafe is out of these bounds
if value < min or value > max:
return 0.0
halfrange = (max - min) / 2.0
midpt = halfrange + min
# this is our range
effectiveRange = halfrange
# scale the input to (-1.0, 1.0),
result = float( value - midpt ) / effectiveRange
if abs(result) < DEAD_ZONE:
return 0.0
else:
return result
def triggerFailsafe(self, doFailsafe):
self.failsafe = doFailsafe
if self.failsafe:
self.shotmgr.enterFailsafe()
# enable/Disable remapping of sticks
def enableRemapping(self, doEnable):
# only act on change
if self.remappingSticks != doEnable:
self.remappingSticks = doEnable
if os.path.exists( "/run/rc_uplink_cmd" ):
if doEnable:
logger.log("[RC] Enabling stick remapping")
rc_ipc.attach()
self.server.sendto("detach uplink", "/run/rc_uplink_cmd")
else:
logger.log("[RC] Disabling stick remapping")
self.server.sendto("attach uplink", "/run/rc_uplink_cmd")
rc_ipc.detach()
def detach(self):
logger.log( "[RC] detach from rc_ipc.put" )
if os.path.exists( "/run/rc_uplink_cmd" ):
self.server.sendto("detach", "/run/rc_uplink_cmd")
self.enableRemapping(False)

8
shotmanager/requirements.txt Normal file
View File

@ -0,0 +1,8 @@
posix_ipc
nose
mock
numpy
enum34
dronekit>=2.0.0
dronekit-solo
-e git+ssh://git@github.com/3drobotics/sololink-python.git#egg=sololink

349
shotmanager/returnHome.py Normal file
View File

@ -0,0 +1,349 @@
#
# rewind.py
# shotmanager
#
# The Rewind RTL shot controller.
# Runs as a DroneKit-Python script under MAVProxy.
#
# Created by Jason Short
# Copyright (c) 2015 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from dronekit import Vehicle, LocationGlobalRelative, VehicleMode
from pymavlink import mavutil
import os
from os import sys, path
import math
import struct
import time
from vector3 import Vector3
sys.path.append(os.path.realpath(''))
import app_packet
import camera
import location_helpers
import shotLogger
from shotManagerConstants import *
from flyController import FlyController
import shots
# on host systems these files are located here
sys.path.append(os.path.realpath('../../flightcode/stm32'))
from sololink import btn_msg
logger = shotLogger.logger
# state management
RISING = 0
TRAVELING = 1
LANDING = 2
RTL_ALT = 2500 # store in cm
RTL_CONE_SLOPE = 1.5
RTL_MIN_RISE = 1
RTL_CLIMB_MIN = 10
RTL_LAND_SPEED_SLOW = .75
RTL_LAND_SPEED_FAST = 2.5
RTL_LAND_REPOSITION_SPEED = 2
MIN_HOME_DISTANCE = 1
# Accel/decel constants
ALT_ACCEL = .1
ALT_ACCEL_PER_TICK = ALT_ACCEL * UPDATE_TIME
# cam control
YAW_SPEED = 60.0
PITCH_SPEED = 60.0
class returnHomeShot():
def __init__(self, vehicle, shotmgr):
# assign the vehicle object
self.vehicle = vehicle
# assign the shotManager object
self.shotmgr = shotmgr
# grab a copy of home
self.homeLocation = self.shotmgr.getHomeLocation()
# enable stick remappings
self.shotmgr.rcMgr.enableRemapping( True )
# enter GUIDED mode
logger.log("[RTL] Try Guided")
self.setButtonMappings()
self.vehicle.mode = VehicleMode("GUIDED")
# sets desired climb altitude
self.rtlAltParam = shotmgr.getParam( "RTL_ALT", RTL_ALT ) / 100.0
self.returnAlt = max((self.vehicle.location.global_relative_frame.alt + RTL_CLIMB_MIN), self.rtlAltParam)
self.returnAlt = self.coneOfAwesomeness(self.returnAlt)
# open loop alt target
self.targetAlt = self.vehicle.location.global_relative_frame.alt
# manages acceleration of alt target
self.desiredClimbRate = 0
self.currentClimbRate = 0
# init state machine
self.state = RISING
# Camera control
self.camYaw = camera.getYaw(self.vehicle)
self.camPitch = camera.getPitch(self.vehicle)
self.camDir = 1
self.manualGimbalTargeting()
def handleRCs(self, channels):
# Freelook during all phases of RTL
self.manualPitch(channels)
self.manualYaw(channels)
self.handleFreeLookPointing()
# if we don't have a home, just land;
# should never happen, but...
if self.homeLocation == None:
self.setupLanding()
if self.state is RISING:
# rise to minimum altitude
self.rise()
if self.vehicle.location.global_relative_frame.alt > self.returnAlt:
logger.log("[RTL] Completed Rise, Travel Home")
self.state = TRAVELING
self.comeHome()
elif self.state is TRAVELING:
if self.isNearHome():
if self.shotmgr.rewindManager.hover == True:
#exit to fly
logger.log("[RTL] Landing disabled")
self.shotmgr.enterShot(shots.APP_SHOT_NONE)
else:
logger.log("[RTL] Landing at home")
self.setupLanding()
elif self.state is LANDING:
# XXX hack until JC fixes Landing in Pos/Vel
#self.land(channels)
return
def setupLanding(self):
self.state = LANDING
# re-init alt to deal with loss of alt during transit
self.targetAlt = self.vehicle.location.global_relative_frame.alt
# Initialize the feed-forward Fly controller
temp = LocationGlobalRelative(self.homeLocation.lat, self.homeLocation.lon, self.targetAlt)
self.pathController = FlyController(temp, 0, 0, 0, self.camYaw)
self.pathController.setOptions(RTL_LAND_SPEED_FAST, 400)
self.pathController.maxSpeed = RTL_LAND_REPOSITION_SPEED
# XXX hack until JC fixes landing
self.vehicle.mode = VehicleMode("LAND")
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 comeHome(self):
# travel to home
aboveHome = LocationGlobalRelative(self.homeLocation.lat, self.homeLocation.lon, self.returnAlt)
self.vehicle.simple_goto(aboveHome)
# should replace with a dronekit command when it gets in there
msg = self.vehicle.message_factory.command_long_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, RTL_SPEED, -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
def rise(self):
self.desiredClimbRate = RTL_LAND_SPEED_FAST * UPDATE_TIME
self.accelerateZ()
self.targetAlt += self.currentClimbRate
# 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.vehicle.location.global_relative_frame.lat * 10000000), # latitude (degrees*1.0e7)
int(self.vehicle.location.global_relative_frame.lon * 10000000), # longitude (degrees*1.0e7)
self.targetAlt, # altitude (meters)
0, 0, -self.currentClimbRate, # 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 land(self, channels):
pos, vel = self.pathController.move(channels, newHeading = self.camYaw)
# if we are high, come down faster
if self.vehicle.location.global_relative_frame.alt > 10:
self.desiredClimbRate = -RTL_LAND_SPEED_FAST * UPDATE_TIME
else:
self.desiredClimbRate = -RTL_LAND_SPEED_SLOW * UPDATE_TIME
self.accelerateZ()
self.targetAlt += self.currentClimbRate
# 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(pos.lat * 10000000), # latitude (degrees*1.0e7)
int(pos.lon * 10000000), # longitude (degrees*1.0e7)
self.targetAlt, # altitude (meters)
vel.x, vel.y, -self.currentClimbRate, # 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 accelerateZ(self):
# Synthetic acceleration
if self.desiredClimbRate > self.currentClimbRate:
self.currentClimbRate += ALT_ACCEL_PER_TICK
self.currentClimbRate = min(self.currentClimbRate, self.desiredClimbRate)
elif self.desiredClimbRate < self.currentClimbRate:
self.currentClimbRate -= ALT_ACCEL_PER_TICK
self.currentClimbRate = max(self.currentClimbRate, self.desiredClimbRate)
else:
self.currentClimbRate = self.desiredClimbRate
def isNearHome(self):
mydist = location_helpers.getDistanceFromPoints(self.vehicle.location.global_relative_frame, self.homeLocation)
return (mydist < MIN_HOME_DISTANCE)
def handleButton(self, button, event):
# any Pause button press or release should get out of RTL
if button == btn_msg.ButtonLoiter and event == btn_msg.Press:
#exit to fly
self.shotmgr.enterShot(shots.APP_SHOT_NONE)
if button == btn_msg.ButtonRTL and event == btn_msg.LongHold and self.state is RISING:
self.state = TRAVELING
self.returnAlt = self.vehicle.location.global_relative_frame.alt
self.comeHome()
def handlePacket(self, packetType, packetLength, packetValue):
return False
def setButtonMappings(self):
buttonMgr = self.shotmgr.buttonManager
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_REWIND, 0, "\0")
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_REWIND, 0, "\0")
def manualGimbalTargeting(self):
'''set gimbal targeting mode to manual'''
msg = self.vehicle.message_factory.mount_configure_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, # mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
self.vehicle.send_mavlink(msg)
# init the open loop gimbal pointing vars
self.camYaw = camera.getYaw(self.vehicle)
self.camPitch = camera.getPitch(self.vehicle)
def manualPitch(self, channels):
if abs(channels[RAW_PADDLE]) > abs(channels[THROTTLE]):
value = channels[RAW_PADDLE]
else:
value = channels[THROTTLE]
self.camPitch += PITCH_SPEED * UPDATE_TIME * value
if self.camPitch > 0.0:
self.camPitch = 0.0
elif self.camPitch < -90:
self.camPitch = -90
def manualYaw(self, channels):
if channels[YAW] == 0:
return
self.camYaw += channels[YAW] * YAW_SPEED * UPDATE_TIME
if self.camYaw > 360:
self.camYaw -= 360
if self.camYaw < 0:
self.camYaw += 360
# required for gimbals w/o Yaw
if channels[YAW] > 0:
self.camDir = 1
else:
self.camDir = -1
def handleFreeLookPointing(self):
'''Handle free look'''
# if we do have a gimbal, use mount_control to set pitch and yaw
if self.vehicle.mount_status[0] is not None:
msg = self.vehicle.message_factory.mount_control_encode(
0, 1, # target system, target component
# pitch is in centidegrees
self.camPitch * 100.0,
0.0, # roll
# yaw is in centidegrees
self.camYaw * 100.0,
0 # save position
)
else:
# if we don't have a gimbal, just set CONDITION_YAW
msg = self.vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command
0, # confirmation
self.camYaw, # param 1 - target angle
YAW_SPEED, # param 2 - yaw speed
self.camDir, # param 3 - direction XXX
0.0, # relative offset
0, 0, 0 # params 5-7 (unused)
)
self.vehicle.send_mavlink(msg)

340
shotmanager/rewind.py Normal file
View File

@ -0,0 +1,340 @@
#
# rewind.py
# shotmanager
#
# The Rewind RTL shot controller.
# Runs as a DroneKit-Python script under MAVProxy.
#
# Created by Jason Short
# Copyright (c) 2015 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from dronekit import Vehicle, LocationGlobalRelative, VehicleMode
from pymavlink import mavutil
import os
from os import sys, path
import math
import struct
import time
from vector3 import Vector3
sys.path.append(os.path.realpath(''))
import app_packet
import camera
import location_helpers
import shotLogger
from shotManagerConstants import *
import shots
# spline
import cableController
from cableController import CableController
import monotonic
# on host systems these files are located here
sys.path.append(os.path.realpath('../../flightcode/stm32'))
from sololink import btn_msg
# for manual control of the camera during rewind
YAW_SPEED = 60.0
PITCH_SPEED = 60.0
# spline speed control
REWIND_SPEED = 3.5
REWIND_MIN_SPEED = 0.5
# distance to exit rewind if we are near home
REWIND_MIN_HOME_DISTANCE = 6.0
logger = shotLogger.logger
# spline
ACCEL_LIMIT = 2.5 #m/s^2
NORM_ACCEL_LIMIT = 2.25 #m/s^2
TANGENT_ACCEL_LIMIT = math.sqrt(ACCEL_LIMIT**2-NORM_ACCEL_LIMIT**2) #m/s^2
class RewindShot():
def __init__(self, vehicle, shotmgr):
# assign the vehicle object
self.vehicle = vehicle
# assign the shotManager object
self.shotmgr = shotmgr
# data manager for breadcrumbs
self.rewindManager = shotmgr.rewindManager
# defines how we exit
self.exitToRTL = True
# enable stick remappings
self.shotmgr.rcMgr.enableRemapping( True )
# enter GUIDED mode
logger.log("[Rewind] Try Guided")
self.setButtonMappings()
self.vehicle.mode = VehicleMode("GUIDED")
# grab a copy of home
self.homeLocation = self.shotmgr.getHomeLocation()
''' spline '''
# default targetP
self.targetP = 0.0
# initialize cable to None
self.cable = None
# last time that the controller was advanced
self.lastTime = None
self.splineOrigin = None
if not self.generateSplines():
logger.log("[Rewind]: Spline generation failed.")
if self.cable is not None:
# go to 1.0
self.cable.setTargetP(1.0)
# give cable controller our desired speed
self.cable.trackSpeed(REWIND_SPEED)
# Camera control
self.camYaw = camera.getYaw(self.vehicle)
self.camPitch = camera.getPitch(self.vehicle)
self.camDir = 1
self.manualGimbalTargeting()
def handleRCs(self, channels):
if self.cable is None:
if self.exitToRTL:
self.exitRewind()
else:
self.shotmgr.enterShot(shots.APP_SHOT_NONE)
return
self.travel()
# Freelook
self.manualPitch(channels)
self.manualYaw(channels)
self.handleFreeLookPointing()
if self.cable.reachedTarget():
self.cable.trackSpeed(0)
if self.exitToRTL:
logger.log("[Rewind] exiting at end of Spline")
self.exitRewind()
if self.isNearHome():
if self.exitToRTL:
logger.log("[Rewind] Exiting Near Home")
self.exitRewind()
def exitRewind(self):
self.rewindManager.resetSpline()
self.shotmgr.enterShot(shots.APP_SHOT_RTL)
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 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 handleButton(self, button, event):
# any Pause button press or release should get out of Rewind
if button == btn_msg.ButtonLoiter and (event == btn_msg.Release or event == btn_msg.Press):
#exit to fly
self.shotmgr.enterShot(shots.APP_SHOT_NONE)
return
def handlePacket(self, packetType, packetLength, packetValue):
return False
def setButtonMappings(self):
buttonMgr = self.shotmgr.buttonManager
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_REWIND, 0, "\0")
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_REWIND, 0, "\0")
def updateAppOptions(self):
return
def addLocation(self, loc):
return
def generateSplines(self):
'''Generate the multi-point spline'''
logger.log("[Rewind] generateSplines")
# store the Lat,lon,alt locations
ctrlPtsLLA = []
# store vectors for relative offsets
ctrlPtsCart = []
# set initial control point as origin
ctrlPtsCart.append(Vector3(0, 0, 0))
# try and load a point
loc = self.rewindManager.queueNextloc()
if loc is None:
return False
logger.log("[Rewind] read loc: %f %f %f" % (loc.lat, loc.lon, loc.alt))
ctrlPtsLLA.append(loc)
# store as spline origin
self.splineOrigin = ctrlPtsLLA[0]
# read all available locations
while (loc is not None):
loc = self.rewindManager.queueNextloc()
if loc is not None:
logger.log("[Rewind] read loc: %f %f %f" % (loc.lat, loc.lon, loc.alt))
ctrlPtsLLA.append(loc)
else:
print "loc: None"
# try and have a 3 point spline or longer:
if len(ctrlPtsLLA) < 2:
return False
# Save offsets from home for spline
for n in range(1, len(ctrlPtsLLA)):
ctrlPtsCart.append(location_helpers.getVectorFromPoints(self.splineOrigin, ctrlPtsLLA[n]))
ctrlPtsCart[-1].z *= -1. #NED
# Construct spline object
try:
self.cable = cableController.CableController(points = ctrlPtsCart, maxSpeed = REWIND_SPEED, minSpeed = REWIND_MIN_SPEED, tanAccelLim = TANGENT_ACCEL_LIMIT, normAccelLim = NORM_ACCEL_LIMIT, smoothStopP = 0.7, maxAlt = 400)
except ValueError, e:
logger.log("%s", e)
return False
#set the location to the start point
self.cable.setCurrentP(0)
return True
def manualGimbalTargeting(self):
'''set gimbal targeting mode to manual'''
msg = self.vehicle.message_factory.mount_configure_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, # mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
self.vehicle.send_mavlink(msg)
def manualPitch(self, channels):
if abs(channels[RAW_PADDLE]) > abs(channels[THROTTLE]):
value = channels[RAW_PADDLE]
else:
value = channels[THROTTLE]
self.camPitch += PITCH_SPEED * UPDATE_TIME * value
if self.camPitch > 0.0:
self.camPitch = 0.0
elif self.camPitch < -90:
self.camPitch = -90
def manualYaw(self, channels):
if channels[YAW] == 0:
return
self.camYaw += channels[YAW] * YAW_SPEED * UPDATE_TIME
if self.camYaw > 360:
self.camYaw -= 360
if self.camYaw < 0:
self.camYaw += 360
# required for gimbals w/o Yaw
if channels[YAW] > 0:
self.camDir = 1
else:
self.camDir = -1
def handleFreeLookPointing(self):
'''Handle free look'''
# if we do have a gimbal, use mount_control to set pitch and yaw
if self.vehicle.mount_status[0] is not None:
msg = self.vehicle.message_factory.mount_control_encode(
0, 1, # target system, target component
# pitch is in centidegrees
self.camPitch * 100.0,
0.0, # roll
# yaw is in centidegrees
self.camYaw * 100.0,
0 # save position
)
else:
# if we don't have a gimbal, just set CONDITION_YAW
msg = self.vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command
0, # confirmation
self.camYaw, # param 1 - target angle
YAW_SPEED, # param 2 - yaw speed
self.camDir, # param 3 - direction XXX
0.0, # relative offset
0, 0, 0 # params 5-7 (unused)
)
self.vehicle.send_mavlink(msg)

244
shotmanager/rewindManager.py Normal file
View File

@ -0,0 +1,244 @@
#
# rewindManager.py
# shotmanager
#
# The section smart shot controller.
# Runs as a DroneKit-Python script.
#
# Created by Jason Short
# Copyright (c) 2015 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from dronekit import Vehicle, LocationGlobalRelative, LocationGlobal
from pymavlink import mavutil
import shots
import location_helpers
import struct
import app_packet
import math
import shotLogger
RTL_STEP_DIST = 1
RTL_MIN_DISTANCE = 10
RTL_DEFAULT_DISTANCE = 20
RTL_MAX_DISTANCE = 60
LOOP_LIMITER = 4
logger = shotLogger.logger
class RewindManager():
def __init__(self, vehicle, shotmgr):
logger.log("[RewindManager] Init")
# assign the vehicle object
self.vehicle = vehicle
self.shotmgr = shotmgr
# should we hover after RTL (True) or Land (False)
self.hover = False
# should we rewind or just RTL
self.enabled = True
# length of breadcrumb trail
self.rewindDistance = RTL_DEFAULT_DISTANCE
# Size of location storage
self.bufferSize = int(math.floor(self.rewindDistance / RTL_STEP_DIST))
# location buffer and pointer
self.buffer = None
self.currentIndex = 0
# Flag to fill buffer once we get good locations
self.did_init = False
# compute limiting
self.counter = 0
# proxy for the vehcile home
self.homeLocation = None
# manages behavior in Auto
self.fs_thr = self.shotmgr.getParam( "FS_THR_ENABLE", 2 )
def resetSpline(self):
logger.log("[RewindManager] reset Spline to size %d" % self.bufferSize)
vehicleLocation = self.vehicle.location.global_relative_frame
if vehicleLocation is None or vehicleLocation.lat is None or vehicleLocation.lon is None or vehicleLocation.alt is None:
self.did_init = False
return
else:
self.buffer = [None for i in range(self.bufferSize)]
self.did_init = True
self.buffer[0] = vehicleLocation
self.currentIndex = 0
def loadHomeLocation(self):
''' Hack method to avoid Dronekit issues with loading home from vehicle'''
if self.vehicle is None:
logger.log("no Vehicle!")
#load home from vehicle
# total grossness for getting HOME_LOCATION! Should be done properly in dronekit instead
self.vehicle.add_message_listener('MISSION_ITEM', self.handleMissionItem)
# formulate mavlink message to request home position
self.vehicle.message_factory.mission_request_send(0, 1, 0) # target_system, target_component, seq
#logger.log("loading home from vehicle2")
def handleMissionItem(self, vehicle, name, msg):
''' Handles callback for home location from vehicle '''
self.vehicle.remove_message_listener('MISSION_ITEM', self.handleMissionItem)
self.homeLocation = LocationGlobal(msg.x, msg.y, msg.z)
logger.log("[RewindManager] loaded home %f %f, alt %f" % (self.homeLocation.lat, self.homeLocation.lon, self.homeLocation.alt))
# Send home to the App
self.updateAppOptions()
def updateAppOptions(self):
''' send home loc to App '''
# f = float_32
# d = float_64
packet = struct.pack('<IIddf', app_packet.SOLO_HOME_LOCATION, 20, self.homeLocation.lat, self.homeLocation.lon, self.homeLocation.alt)
self.shotmgr.appMgr.sendPacket(packet)
def updateLocation(self):
''' Store locations for Rewind Spline'''
# load limiter
self.counter += 1
if self.counter > LOOP_LIMITER:
self.counter = 0
else:
return
if not self.vehicle.armed or self.vehicle.system_status != 'ACTIVE':
# we don't want to reset every cycle while on ground
if self.currentIndex != 0:
self.resetSpline()
return
vehicleLocation = self.vehicle.location.global_relative_frame
if vehicleLocation is None or vehicleLocation.lat is None or vehicleLocation.lon is None or vehicleLocation.alt is None:
return
# reset buffer with the current location if needed
if self.did_init is False or self.buffer[self.currentIndex] is None:
self.resetSpline()
return
try:
# calc distance from home
dist = location_helpers.getDistanceFromPoints3d(self.buffer[self.currentIndex], vehicleLocation)
except Exception as e:
logger.log('[RewindManager]: loc was None, %s' % e)
#logger.log("dist %f"% dist)
if dist >= RTL_STEP_DIST:
# store new location in next index
self.currentIndex += 1
# mind the size of the buffer
if(self.currentIndex >= self.bufferSize):
self.currentIndex = 0
# store the location
self.buffer[self.currentIndex] = vehicleLocation
# keep for testing
#logger.log("[RWMGR]: Save %d %f %f %f" % (self.currentIndex,
# vehicleLocation.lat,
# vehicleLocation.lon,
# vehicleLocation.alt))
def queueNextloc(self):
''' Called by Rewind shot '''
if self.buffer is None:
return None
# get ref to current buffered location
temp = self.buffer[self.currentIndex]
# put a None in the array to clear it
self.buffer[self.currentIndex] = None
# decrement index
self.currentIndex -= 1
if self.currentIndex < 0:
self.currentIndex = self.bufferSize - 1
#logger.log("[RewindManager] next loc index %d %d" % (self.currentIndex, self.bufferSize))
if temp is None:
#logger.log("[RewindManager] next loc is None")
self.resetSpline()
return temp
def handlePacket(self, packetType, packetLength, packetValue):
'''handle incoming data from the client app'''
try:
if packetType == app_packet.SOLO_REWIND_OPTIONS:
# don't read packet if we are in Rewind
if self.shotmgr.currentShot != shots.APP_SHOT_REWIND:
#(self.enabled) = struct.unpack('<B', packetValue)
(self.enabled, self.hover, _rewindDistance) = struct.unpack('<BBf', packetValue)
logger.log("[RWMGR]: Rewind enabled: %d" % self.enabled)
logger.log("[RWMGR]: Rewind RTL hover: %d" % self.hover)
logger.log("[RWMGR]: Rewind distance: %d" % _rewindDistance)
if _rewindDistance != self.rewindDistance:
self.rewindDistance = max(min(_rewindDistance, RTL_MAX_DISTANCE), RTL_MIN_DISTANCE)
self.bufferSize = int(math.floor(self.rewindDistance / RTL_STEP_DIST))
logger.log("[RWMGR]: self.bufferSize: %d" % self.bufferSize)
self.resetSpline()
elif packetType == app_packet.SOLO_HOME_LOCATION:
if self.homeLocation is None:
return True
''' Store new home location for Return to Me'''
(lat, lon, alt) = struct.unpack('<ddf', packetValue)
# only respond to the app data when we are armed and in the air
if self.vehicle.armed == 1 and self.vehicle.system_status == 'ACTIVE':
self.homeLocation = LocationGlobal(lat, lon, self.homeLocation.alt)
#logger.log("[RWMGR]: New Home loc set: %f, %f, %f" % (lat, lon, self.homeLocation.alt))
# let the app know we repsoned to the data
self.updateAppOptions()
else:
return False
except Exception as e:
logger.log('[RWMGR]: Error handling packet. (%s)' % e)
return False
else:
return True

4
shotmanager/run_shotmanager.sh Executable file
View File

@ -0,0 +1,4 @@
#!/bin/sh
date > /log/python_stderr.log
python /usr/bin/main.py udpout:127.0.0.1:14560 udpout:127.0.0.1:14550 >> /log/python_stderr.log 2>&1

291
shotmanager/selfie.py Normal file
View File

@ -0,0 +1,291 @@
# selfie.py
# shotmanager
#
# The selfie smart shot.
# Press Fly Out in the app and the drone flies up and out, keeping the camera pointed at the ROI
# Runs as a DroneKit-Python script.
#
# Created by Will Silva and Eric Liao in 2015
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from dronekit import VehicleMode, LocationGlobalRelative
from pymavlink import mavutil
import os
from os import sys, path
import struct
import math
sys.path.append(os.path.realpath(''))
import app_packet
import location_helpers
import pathHandler
import shotLogger
import shots
from shotManagerConstants import *
# on host systems these files are located here
sys.path.append(os.path.realpath('../../flightcode/stm32'))
from sololink import btn_msg
YAW_SPEED = 60.0
PITCH_SPEED = 60.0
logger = shotLogger.logger
class SelfieShot():
def __init__(self, vehicle, shotmgr):
self.vehicle = vehicle
self.shotmgr = shotmgr
self.waypoints = []
self.roi = None
self.pathHandler = None
# Camera control
self.camPitch = 0
self.camYaw = 0
self.camDir = 1
self.canResetCam = False
self.setButtonMappings()
# get current altitude limit
self.altLimit = self.shotmgr.getParam( "FENCE_ALT_MAX", DEFAULT_FENCE_ALT_MAX ) # in meters
# check APM params to see if Altitude Limit should apply
if self.shotmgr.getParam( "FENCE_ENABLE", DEFAULT_FENCE_ENABLE ) == 0:
self.altLimit = None
logger.log("[Selfie]: Altitude Limit is disabled.")
# channels are expected to be floating point values in the (-1.0, 1.0) range
def handleRCs( self, channels ):
#selfie needs 2 waypoints and an ROI to be ready
if len(self.waypoints) != 2 or not self.roi:
return
self.pathHandler.MoveTowardsEndpt(channels)
self.manualPitch(channels)
self.manualYaw(channels)
self.handleFreeLookPointing()
if self.pathHandler.isNearTarget():
# if we reached the end, flip
if self.pathHandler.cruiseSpeed > 0.0:
self.pathHandler.cruiseSpeed = -self.pathHandler.cruiseSpeed
logger.log("[Selfie]: reached end of selfie path, flipping")
elif self.pathHandler.cruiseSpeed < 0.0:
logger.log("[Selfie]: reached end of selfie path, pausing")
self.pathHandler.pause()
self.updateAppOptions()
def setButtonMappings(self):
buttonMgr = self.shotmgr.buttonManager
if self.canResetCam:
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_SELFIE, btn_msg.ARTOO_BITMASK_ENABLED, "Look At Me\0")
else:
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_SELFIE, 0, "\0")
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_SELFIE, 0, "\0")
# if we can handle the button we do
def handleButton(self, button, event):
if button == btn_msg.ButtonA and event == btn_msg.Press:
self.pointAtROI()
if button == btn_msg.ButtonLoiter and event == btn_msg.Press:
if self.pathHandler:
self.shotmgr.notifyPause(True)
self.pathHandler.togglePause()
self.updateAppOptions()
else:
# notify autopilot of pause press (technically not in shot)
self.shotmgr.notifyPause(False)
# send our current set of options to the app
def updateAppOptions(self):
speed = 0.0
if self.pathHandler:
speed = self.pathHandler.cruiseSpeed
packet = struct.pack('<IIf', app_packet.SOLO_SHOT_OPTIONS, 4, speed)
self.shotmgr.appMgr.sendPacket(packet)
def handlePacket(self, packetType, packetLength, packetValue):
try:
if packetType == app_packet.SOLO_MESSAGE_LOCATION:
(lat, lon, alt) = struct.unpack('<ddf', packetValue)
logger.log("[selfie]: Location received from app: %f, %f, %f." %( lat, lon, alt ) )
self.addLocation(LocationGlobalRelative(lat, lon, alt))
elif packetType == app_packet.SOLO_SHOT_OPTIONS:
(cruiseSpeed,) = struct.unpack('<f', packetValue)
if self.pathHandler:
self.pathHandler.setCruiseSpeed(cruiseSpeed)
logger.log("[selfie]: Cruise speed set to %.2f." % (cruiseSpeed,))
else:
return False
except Exception as e:
logger.log('[selfie]: Error handling packet. (%s)' % e)
return False
else:
return True
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 manualGimbalTargeting(self):
'''set gimbal targeting mode to manual'''
msg = self.vehicle.message_factory.mount_configure_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, # mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
self.vehicle.send_mavlink(msg)
self.pointAtROI()
def pointAtROI(self):
logger.log("[Selfie]: Point at ROI")
# calcs the yaw and pitch to point the gimbal
self.camYaw, self.camPitch = location_helpers.calcYawPitchFromLocations(self.vehicle.location.global_relative_frame, self.roi)
if self.canResetCam:
self.canResetCam = False
self.setButtonMappings()
def updateCanResetCam(self, resetCam):
# only send button label updates on change
if resetCam != self.canResetCam:
logger.log("[Selfie]: can reset to ROI")
self.canResetCam = resetCam
self.setButtonMappings()
def manualPitch(self, channels):
if abs(channels[RAW_PADDLE]) > abs(channels[THROTTLE]):
value = channels[RAW_PADDLE]
else:
value = channels[THROTTLE]
if value == 0:
return
self.updateCanResetCam(True)
self.camPitch += value * PITCH_SPEED * UPDATE_TIME
if self.camPitch > 0.0:
self.camPitch = 0.0
elif self.camPitch < -90:
self.camPitch = -90
def manualYaw(self, channels):
if channels[YAW] == 0:
return
self.updateCanResetCam(True)
self.camYaw += channels[YAW] * YAW_SPEED * UPDATE_TIME
if self.camYaw > 360:
self.camYaw -= 360
if self.camYaw < 0:
self.camYaw += 360
# required for gimbals w/o Yaw
if channels[YAW] > 0:
self.camDir = 1
else:
self.camDir = -1
def handleFreeLookPointing(self):
'''Handle free look'''
# if we do have a gimbal, use mount_control to set pitch and yaw
if self.vehicle.mount_status[0] is not None:
msg = self.vehicle.message_factory.mount_control_encode(
0, 1, # target system, target component
# pitch is in centidegrees
self.camPitch * 100.0,
0.0, # roll
# yaw is in centidegrees
self.camYaw * 100.0,
0 # save position
)
else:
# if we don't have a gimbal, just set CONDITION_YAW
msg = self.vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command
0, # confirmation
self.camYaw, # param 1 - target angle
YAW_SPEED, # param 2 - yaw speed
self.camDir, # param 3 - direction XXX
0.0, # relative offset
0, 0, 0 # params 5-7 (unused)
)
self.vehicle.send_mavlink(msg)

76
shotmanager/settings.py Normal file
View File

@ -0,0 +1,76 @@
#
# This file handles reading/writing settings from shotmanager.conf
#
import os
import threading
import shotLogger
import ConfigParser
logger = shotLogger.logger
settingsLock = threading.Lock()
if 'SOLOLINK_SANDBOX' in os.environ:
CONFIG_FILE = os.path.join(os.path.dirname(__file__), 'sim/shotmanager.sandbox.conf')
CONFIG_FILE_BACKUP = os.path.join(os.path.dirname(__file__), 'sim/shotmanager.back')
else:
CONFIG_FILE = "/etc/shotmanager.conf"
CONFIG_FILE_BACKUP = "/etc/shotmanager.back"
def writeSettingsThread(name, value):
settingsLock.acquire()
# write to the config file
config = ConfigParser.SafeConfigParser()
config.optionxform=str
config.read(CONFIG_FILE)
try:
config.set("shotManager", name, value)
except:
logger.log("Failed to write setting")
# back up the file
os.system("cp %s %s" % (CONFIG_FILE, CONFIG_FILE_BACKUP))
os.system("md5sum %s > %s.md5" % (CONFIG_FILE_BACKUP, CONFIG_FILE_BACKUP))
os.system("sync")
# modify config file and set md5
with open(CONFIG_FILE, 'wb') as configfile:
config.write(configfile)
os.system("md5sum %s > %s.md5" % (CONFIG_FILE, CONFIG_FILE))
os.system("sync")
os.system("rm %s %s.md5" % (CONFIG_FILE_BACKUP, CONFIG_FILE_BACKUP))
os.system("sync")
logger.log("wrote setting: %s: %s"%(name, value))
settingsLock.release()
# reads and returns setting of the given name
# perhaps we shouldn't read the file for each setting, but that's something we can address
# when we have more settings
def readSetting(name):
# get our saved button mappings
config = ConfigParser.SafeConfigParser()
settingsLock.acquire()
# if the config file is not found, an empty list is returned and the "get"
# operations below fail
config.read(CONFIG_FILE)
settingsLock.release()
try:
return config.get("shotManager", name)
except:
logger.log("error reading %s"%(CONFIG_FILE,))
raise
return 0
# starts our thread which writes out our setting
# note both name and value should be strings
def writeSetting(name, value):
thread = threading.Thread(name = "writeSettingsThread", target = writeSettingsThread, args = (name, value))
thread.daemon = True
thread.start()

25
shotmanager/setupShots.sh Executable file
View File

@ -0,0 +1,25 @@
# install shots code to a vehicle and reboots python interpreter
function control_c {
exit $?
}
function update_version {
VERSION=$(git describe --tags)
echo Installing ${VERSION} ...
>"./shotManager_version.py"
echo VERSION = \"${VERSION}\" >> "./shotManager_version.py"
}
trap control_c SIGINT
trap control_c SIGTERM
update_version
SCPARGS="-o StrictHostKeyChecking=no"
scp $SCPARGS *.py root@10.1.1.10:/usr/bin
echo Rebooting all Python
ssh $SCPARGS root@10.1.1.10 "killall python"

32
shotmanager/shotFactory.py Normal file
View File

@ -0,0 +1,32 @@
import shots
import selfie
import orbit
import cable_cam
import zipline
import follow
import pano
import rewind
import multipoint
import transect
import returnHome
class ShotFactory(object):
__shot_classes = {
shots.APP_SHOT_SELFIE : selfie.SelfieShot,
shots.APP_SHOT_ORBIT: orbit.OrbitShot,
shots.APP_SHOT_CABLECAM : cable_cam.CableCamShot,
shots.APP_SHOT_ZIPLINE : zipline.ZiplineShot,
shots.APP_SHOT_FOLLOW : follow.FollowShot,
shots.APP_SHOT_MULTIPOINT : multipoint.MultipointShot,
shots.APP_SHOT_PANO : pano.PanoShot,
shots.APP_SHOT_REWIND : rewind.RewindShot,
shots.APP_SHOT_TRANSECT : transect.TransectShot,
shots.APP_SHOT_RTL : returnHome.returnHomeShot,
}
@staticmethod
def get_shot_obj(shot, *args, **kwargs):
shot_class = ShotFactory.__shot_classes.get(shot, None)
if shot_class:
return shot_class(args[0], args[1])
raise NotImplementedError("[shot]: ShotFactory failed to generate the requested shot.")

18
shotmanager/shotLogger.py Normal file
View File

@ -0,0 +1,18 @@
#!/usr/bin/env python
import os
import logging
import logging.config
class Logger():
def __init__(self):
if 'SOLOLINK_SANDBOX' in os.environ:
logging.config.fileConfig(os.path.join(os.path.dirname(__file__), 'sim/shotmanager.sandbox.conf'))
else:
logging.config.fileConfig("/etc/shotmanager.conf")
self.xlog = logging.getLogger("shot")
def log(self, data):
self.xlog.info(str(data).replace("\0", ""))
logger = Logger()

505
shotmanager/shotManager.py Normal file
View File

@ -0,0 +1,505 @@
#
# This is the entry point for shotmanager on Solo using Dronekit-Python
# Python native imports
import os
from os import sys, path
import select
import struct
import time
import traceback
# Dronekit/Mavlink imports
from dronekit.lib import VehicleMode
from pymavlink import mavutil
sys.path.append(path.realpath(''))
# Managers imports
import buttonManager
import appManager
import rcManager
from GoProConstants import *
import GoProManager
import rewindManager
import GeoFenceManager
# Loggers imports
import shotLogger
# Constants imports
from shotManagerConstants import *
# Shotmanager imports
from shotFactory import ShotFactory
import app_packet
import shots
import modes
import monotonic
from location_helpers import rad2deg, wrapTo360, wrapTo180
try:
from shotManager_version import VERSION
except ImportError:
VERSION = "[shot]: Unknown Shotmanager version"
logger = shotLogger.logger
class ShotManager():
def __init__(self):
# see the shotlist in app/shots/shots.py
self.currentShot = shots.APP_SHOT_NONE
self.currentModeIndex = DEFAULT_APM_MODE
self.curController = None
def Start(self, vehicle):
logger.log("+-+-+-+-+-+-+ Starting up %s +-+-+-+-+-+-+" % VERSION)
### initialize dronekit vehicle ###
self.vehicle = vehicle
### switch vehicle to loiter mode ###
self.vehicle.mode = VehicleMode("LOITER")
### initialize rc manager ###
self.rcMgr = rcManager.rcManager(self)
### initialize app manager ###
self.appMgr = appManager.appManager(self)
### initialize button manager ###
self.buttonManager = buttonManager.buttonManager(self)
### initialize gopro manager ###
self.goproManager = GoProManager.GoProManager(self)
### Initialize GeoFence manager ###
self.geoFenceManager = GeoFenceManager.GeoFenceManager(self)
# instantiate rewindManager
self.rewindManager = rewindManager.RewindManager(self.vehicle, self)
### init APM stream rates ###
self.initStreamRates()
### register callbacks ###
self.registerCallbacks()
# Try to maintain a constant tick rate
self.timeOfLastTick = monotonic.monotonic()
# how many ticks have we performed since an RC update?
# register all connections (gopro manager communicates via appMgr's socket)
self.inputs = [self.rcMgr.server, self.appMgr.server]
self.outputs = []
#check if gimbal is present
if self.vehicle.gimbal.yaw is not None:
logger.log("[shot]: Gimbal detected.")
# Initialize gimbal to RC targeting
self.vehicle.gimbal.release()
else:
logger.log("[shot]: No gimbal detected.")
# mark first tick time
self.timeOfLastTick = monotonic.monotonic()
# check for In-Air start from Shotmanager crash
if self.vehicle.system_status == 'ACTIVE':
logger.log("[shot]: Restart in air.")
# load vehicle home
self.rewindManager.loadHomeLocation()
# not yet enabled until this check proves effective
#self.vehicle.mode = VehicleMode("RTL")
def Run(self):
while True:
try:
#print "in shotManager server loop"
# handle TCP/RC packets
# we set a timeout of UPDATE_TIME, so we roughly do this every UPDATE_TIME
rl, wl, el = select.select( self.inputs, self.outputs, [], UPDATE_TIME )
# handle reads
for s in rl:
if s is self.appMgr.server: # if read is a connection attempt
self.appMgr.connectClient()
elif s is self.appMgr.client: # if read is from app
self.appMgr.parse()
elif s is self.rcMgr.server: # if read is an RC packet
self.rcMgr.parse()
elif s is self.buttonManager.client: # if read is from buttons
self.buttonManager.parse()
# now handle writes (sololink.btn_msg handles all button writes)
for s in wl:
if s is self.appMgr.client: # if write is for app
self.appMgr.write()
# exceptions
for s in el:
if s is self.appMgr.client: # if its the app socket throwing an exception
self.appMgr.exception()
else:
# otherwise remove whichever socket is excepting
if s in self.inputs:
self.inputs.remove(s)
if s in self.outputs:
self.outputs.remove(s)
s.close()
self.buttonManager.checkButtonConnection()
# Check if copter is outside fence or will be
self.geoFenceManager.activateGeoFenceIfNecessary()
# call main control/planning loop at UPDATE_RATE
if time.time() - self.timeOfLastTick > UPDATE_TIME:
self.Tick()
except Exception as ex:
# reset any RC timeouts and stop any stick remapping
self.rcMgr.detach()
# try to put vehicle into LOITER
self.vehicle.mode = VehicleMode("LOITER")
exceptStr = traceback.format_exc()
print exceptStr
strlist = exceptStr.split('\n')
for i in strlist:
logger.log(i)
if self.appMgr.isAppConnected():
# send error to app
packet = struct.pack('<II%ds' % (len(exceptStr)), app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR, len(exceptStr), exceptStr)
self.appMgr.client.send(packet)
# sleep to make sure the packet goes out (for some reason
# setting client.setblocking(1) doesn't work)
time.sleep(0.4)
# cleanup
for socket in self.inputs:
socket.close()
os._exit(1)
def enterShot(self, shot):
if shot not in shots.SHOT_NAMES:
logger.log("[shot]: Shot not recognized. (%d)" % shot)
return
if shot == shots.APP_SHOT_NONE:
pass
# check our EKF - if it's bad and that we can't init the shot prior to EKF being OK, reject shot entry attempt
elif self.last_ekf_ok is False and shot not in shots.CAN_START_BEFORE_EKF:
logger.log('[shot]: Vehicle EKF quality is poor, shot entry into %s disallowed.' % shots.SHOT_NAMES[shot])
# set shot to APP_SHOT_NONE
shot = shots.APP_SHOT_NONE
# notify the app of shot entry failure
packet = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_BAD_EKF)
self.appMgr.sendPacket(packet)
# check vehicle system status - if it's CRITICAL or EMERGENCY, reject shot entry attempt
elif self.vehicle.system_status in ['CRITICAL', 'EMERGENCY']:
logger.log('[shot]: Vehicle in %s, shot entry into %s disallowed.' % (self.vehicle.system_status, shots.SHOT_NAMES[shot]))
# set shot to APP_SHOT_NONE
shot = shots.APP_SHOT_NONE
# notify the app of shot entry failure
packet = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_RTL)
self.appMgr.sendPacket(packet)
# check if vehicle is not armed or in STANDBY and that we can't init the shot prior to arm, reject shot entry attempt
elif (self.vehicle.armed is False or self.vehicle.system_status == 'STANDBY') and shot not in shots.CAN_START_BEFORE_ARMING:
logger.log('[shot]: Vehicle is unarmed, shot entry into %s disallowed.' % shots.SHOT_NAMES[shot])
self.vehicle.mode = VehicleMode("LOITER")
# set shot to APP_SHOT_NONE
shot = shots.APP_SHOT_NONE
# notify the app of shot entry failure
packet = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_UNARMED)
self.appMgr.sendPacket(packet)
# OK fine, you get to start the shot.
if self.currentShot != shot:
logger.log('[shot]: Entering shot %s.' % shots.SHOT_NAMES[shot])
if self.currentShot == shots.APP_SHOT_REWIND:
# we are exiting Rewind
self.rewindManager.resetSpline()
# APP_SHOT_NONE
if shot == shots.APP_SHOT_NONE:
# mark curController for garbage collection
del self.curController
# set curController to None (should also mark for garbage collection)
self.curController = None
# re-enable manual gimbal controls (RC Targeting mode)
self.vehicle.gimbal.release()
# disable the stick re-mapper
self.rcMgr.enableRemapping( False )
# if the Rewind shot put us into RTL, lets stay there
if self.vehicle.mode.name == 'RTL':
logger.log("[shot]: Leaving vehicle in mode RTL")
# if vehicle mode is in another mode such as GUIDED or AUTO, then switch to LOITER
elif self.vehicle.mode.name in shots.SHOT_MODES:
logger.log("[shot]: Changing vehicle mode to LOITER.")
self.vehicle.mode = VehicleMode("LOITER")
else:
self.curController = ShotFactory.get_shot_obj(shot, self.vehicle, self)
# update currentShot
self.currentShot = shot
logger.log("[shot]: Successfully entered %s." % shots.SHOT_NAMES[shot])
# already in that shot
else:
logger.log('[shot]: Already in shot %s.' % shots.SHOT_NAMES[shot])
# let the world know
if self.appMgr.isAppConnected():
self.appMgr.broadcastShotToApp(shot)
# let Artoo know too
self.buttonManager.setArtooShot(shot)
# set new button mappings appropriately
self.buttonManager.setButtonMappings()
def mode_callback(self, vehicle, name, mode):
try:
if mode.name != self.lastMode:
logger.log("[callback]: Mode changed from %s -> %s"%(self.lastMode, mode.name))
if mode.name == 'RTL':
logger.log("[callback]: System entered RTL, switch to shot!")
self.enterShot(shots.APP_SHOT_RTL)
elif self.currentShot != shots.APP_SHOT_NONE:
# looks like somebody switched us out of guided! Exit our current shot
if mode.name not in shots.SHOT_MODES:
logger.log("[callback]: Detected that we are not in the correct apm mode for this shot. Exiting shot!")
self.enterShot(shots.APP_SHOT_NONE)
self.lastMode = mode.name
# don't do the following for guided, since we're in a shot
if self.lastMode == 'GUIDED' or mode.name == 'RTL':
return
modeIndex = modes.getAPMModeIndexFromName( self.lastMode, self.vehicle)
if modeIndex < 0:
logger.log("couldn't find this mode index: %s" % self.lastMode)
return
if self.currentShot == shots.APP_SHOT_NONE:
self.buttonManager.setArtooShot( -1, modeIndex )
self.currentModeIndex = modeIndex
except Exception as e:
logger.log('[shot]: mode callback error, %s' % e)
def ekf_callback(self, vehicle, name, ekf_ok):
try:
if ekf_ok != self.last_ekf_ok:
self.last_ekf_ok = ekf_ok
logger.log("[callback]: EKF status changed to %d" % (ekf_ok))
self.buttonManager.setButtonMappings()
# if we regain EKF and are landing - just push us into fly
if ekf_ok and self.vehicle.mode.name == 'LAND':
self.enterShot(shots.APP_SHOT_NONE)
# only store home in the air when no home loc exists
if self.rewindManager:
if ekf_ok and self.last_armed and self.rewindManager.homeLocation is None:
self.rewindManager.loadHomeLocation()
except Exception as e:
logger.log('[callback]: ekf callback error, %s' % e)
def armed_callback(self, vehicle, name, armed):
try:
if armed != self.last_armed:
self.last_armed = armed
logger.log("[callback]: armed status changed to %d"%(armed))
self.buttonManager.setButtonMappings()
# clear Rewind manager cache
self.rewindManager.resetSpline()
if not armed and self.currentShot not in shots.CAN_START_BEFORE_ARMING:
self.enterShot(shots.APP_SHOT_NONE)
self.vehicle.mode = VehicleMode("LOITER")
# stop recording upon disarm (landing, hopefully)
if not armed:
self.goproManager.handleRecordCommand(self.goproManager.captureMode, RECORD_COMMAND_STOP)
# read home loc from vehicle
if armed:
self.rewindManager.loadHomeLocation()
except Exception as e:
logger.log('[callback]: armed callback error, %s' % e)
def camera_feedback_callback(self, vehicle, name, msg):
try:
if self.currentShot in shots.SITE_SCAN_SHOTS or self.vehicle.mode.name == 'AUTO':
# issue GoPro record command
self.goproManager.handleRecordCommand(CAPTURE_MODE_PHOTO, RECORD_COMMAND_START)
except Exception as e:
logger.log('[callback]: camera feedback callback error, %s.' % e)
def initStreamRates(self):
STREAM_RATES = {
mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS: 2,
mavutil.mavlink.MAV_DATA_STREAM_EXTRA1: 10,
mavutil.mavlink.MAV_DATA_STREAM_EXTRA2: 10,
mavutil.mavlink.MAV_DATA_STREAM_EXTRA3: 2,
mavutil.mavlink.MAV_DATA_STREAM_POSITION: 10,
mavutil.mavlink.MAV_DATA_STREAM_RAW_SENSORS: 2,
mavutil.mavlink.MAV_DATA_STREAM_RAW_CONTROLLER: 3,
mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS: 5,
}
for stream, rate in STREAM_RATES.items():
msg = self.vehicle.message_factory.request_data_stream_encode(
0, 1, # target system, target component
stream, # requested stream id
rate, # rate
1 # start it
)
self.vehicle.send_mavlink(msg)
def notifyPause(self, inShot=0):
'''notify the autopilot that we would like to pause'''
if inShot:
return
msg = self.vehicle.message_factory.command_long_encode(
0, # target system
1, # target component
mavutil.mavlink.MAV_CMD_SOLO_BTN_PAUSE_CLICK, # frame
0, # confirmation
int(inShot), # param 1: 1 if Solo is in a shot mode, 0 otherwise
0, 0, 0, 0, 0, 0) # params 2-7 (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
# This fetches and returns the value of the parameter matching the given name
# If the parameter is not found, returns the given default value instead
def getParam(self, name, default=None):
return self.vehicle.parameters.get(name, wait_ready=False) or default
# we call this at our UPDATE_RATE
# drives the shots as well as anything else timing-dependent
def Tick(self):
self.timeOfLastTick = time.time()
self.rcMgr.rcCheck()
# update rewind manager
if (self.currentShot == shots.APP_SHOT_REWIND or self.currentShot == shots.APP_SHOT_RTL or self.vehicle.mode.name == 'RTL') is False:
self.rewindManager.updateLocation()
# Always call remap
channels = self.rcMgr.remap()
if self.curController:
self.curController.handleRCs(channels)
def getHomeLocation(self):
if self.rewindManager.homeLocation is None or self.rewindManager.homeLocation.lat == 0:
return None
else:
return self.rewindManager.homeLocation
def enterFailsafe(self):
''' called when we loose RC link or have Batt FS event '''
# dont enter failsafe on the ground
if not self.vehicle.armed or self.vehicle.system_status != 'ACTIVE':
return
# dont enter failsafe if we are already rewinding home
if self.currentShot == shots.APP_SHOT_REWIND:
self.curController.exitToRTL = True
return
if self.currentShot == shots.APP_SHOT_RTL:
return
if self.last_ekf_ok is False:
# no GPS - force an emergency land
self.vehicle.mode = VehicleMode("LAND")
return
# ignore FS while in Auto mode
if self.vehicle.mode.name == 'AUTO' and self.rewindManager.fs_thr == 2:
return
if self.rewindManager.enabled:
self.enterShot(shots.APP_SHOT_REWIND)
self.curController.exitToRTL = True
else:
self.enterShot(shots.APP_SHOT_RTL)
def registerCallbacks(self):
self.last_ekf_ok = False
self.last_armed = False
self.lastMode = self.vehicle.mode.name
# MODE
self.vehicle.add_attribute_listener('mode', self.mode_callback) #register with vehicle class (dronekit)
# EKF
# call ekf back first
self.ekf_callback(self.vehicle, 'ekf_ok', self.vehicle.ekf_ok)
self.vehicle.add_attribute_listener('ekf_ok', self.ekf_callback) #register with vehicle class (dronekit)
# ARMED
self.vehicle.add_attribute_listener('armed', self.armed_callback) #register with vehicle class (dronekit)
# CAMERA FEEDBACK
self.vehicle.add_message_listener('CAMERA_FEEDBACK', self.camera_feedback_callback) #register with vehicle class (dronekit)
# gopro manager callbacks (defined in gopro manager)
self.vehicle.add_attribute_listener('gopro_status', self.goproManager.state_callback)
self.vehicle.add_attribute_listener('gopro_get_response', self.goproManager.get_response_callback)
self.vehicle.add_attribute_listener('gopro_set_response', self.goproManager.set_response_callback)

39
shotmanager/shotManagerConstants.py Normal file
View File

@ -0,0 +1,39 @@
SERVER_PORT = 5507
RC_FREQUENCY = 50.0
# if this is 0, don't skip any RC packets.
# if 1, skip every other one, and so forth
RC_SKIP_MULTIPLE = 1
UPDATE_RATE = RC_FREQUENCY / (RC_SKIP_MULTIPLE + 1)
UPDATE_TIME = 1.0 / UPDATE_RATE
MAX_SPEED = 8.0
# don't allow waypoints this close to each other
WAYPOINT_NEARNESS_THRESHOLD = 2.5
# cm / s from APM
DEFAULT_WPNAV_SPEED_VALUE = 500.0
# in seconds
BRAKE_LENGTH = 1.5
NUM_TICKS_FOR_BRAKE = int( BRAKE_LENGTH * UPDATE_RATE )
# this is FLY mode - see pymavlink/mavutil.py:mode_mapping_acm
DEFAULT_APM_MODE = 5
# default altitude limit
DEFAULT_FENCE_ALT_MAX = 46 #meters, ~150 feet
DEFAULT_FENCE_ENABLE = 0 # Disabled by default, as the failure mode is safer. Better to not stop the copter from climbing than to force it to descend when entering a shot.
# Spektrum channels order
THROTTLE = 0
ROLL = 1
PITCH = 2
YAW = 3
FILTERED_PADDLE = 5
RAW_PADDLE = 7
RTL_SPEED = 10

40
shotmanager/shots.py Executable file
View File

@ -0,0 +1,40 @@
#!/usr/bin/env python
# definitions of shots
# NOTE: Make sure this stays in sync with the app's definitions! Those are in iSolo/UI/ShotLibrary.swift
APP_SHOT_NONE = -1
APP_SHOT_SELFIE = 0
APP_SHOT_ORBIT = 1
APP_SHOT_CABLECAM = 2
APP_SHOT_ZIPLINE = 3
APP_SHOT_RECORD = 4
APP_SHOT_FOLLOW = 5
APP_SHOT_MULTIPOINT = 6
APP_SHOT_PANO = 7
APP_SHOT_REWIND = 8
APP_SHOT_TRANSECT = 9
APP_SHOT_RTL = 10
# NULL terminated for sending to Artoo
SHOT_NAMES = {
APP_SHOT_NONE : "FLY\0",
APP_SHOT_SELFIE : "Selfie\0",
APP_SHOT_ORBIT : "Orbit\0",
APP_SHOT_CABLECAM : "Cable Cam\0",
APP_SHOT_ZIPLINE : "Zip Line\0",
APP_SHOT_FOLLOW : "Follow\0",
APP_SHOT_MULTIPOINT: "Cable Cam\0",
APP_SHOT_PANO: "Pano\0",
APP_SHOT_REWIND: "Rewind\0",
APP_SHOT_TRANSECT: "Transect\0",
APP_SHOT_RTL: "Return Home\0",
}
CAN_START_BEFORE_ARMING = []
CAN_START_BEFORE_EKF = []
CAN_START_FROM_ARTOO = [APP_SHOT_ORBIT, APP_SHOT_CABLECAM, APP_SHOT_MULTIPOINT, APP_SHOT_PANO, APP_SHOT_ZIPLINE, APP_SHOT_REWIND, APP_SHOT_TRANSECT, APP_SHOT_RTL]
ALWAYS_NEEDS_APP_CONNECTION = [APP_SHOT_CABLECAM, APP_SHOT_SELFIE, APP_SHOT_ORBIT, APP_SHOT_FOLLOW, APP_SHOT_MULTIPOINT, APP_SHOT_PANO, APP_SHOT_ZIPLINE]
ALWAYS_NEEDS_RC_CONNECTION = [APP_SHOT_CABLECAM, APP_SHOT_SELFIE, APP_SHOT_ORBIT, APP_SHOT_FOLLOW, APP_SHOT_MULTIPOINT, APP_SHOT_PANO, APP_SHOT_ZIPLINE, APP_SHOT_TRANSECT]
SHOT_MODES = ['AUTO', 'GUIDED']
SITE_SCAN_SHOTS = [] # XXX remove

49
shotmanager/sim/StressPerformance.py Normal file
View File

@ -0,0 +1,49 @@
#
# This is the entry point for MavProxy running DroneAPI on the vehicle
# Usage:
# * mavproxy.py
# * module load api
# * api start TestPerformance.py
#
import time
from pymavlink import mavutil
UPDATE_RATE = 50.0
class ShotManager():
def __init__(self):
# First get an instance of the API endpoint
api = local_connect()
# get our vehicle - when running with mavproxy it only knows about one vehicle (for now)
self.vehicle = api.get_vehicles()[0]
self.vehicle.wait_ready()
while True:
msg = self.vehicle.message_factory.command_long_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
0, # confirmation
1, 1.0, -1, # params 1-3
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
msg2 = self.vehicle.message_factory.command_long_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_SET_ROI, #command
0, #confirmation
0, 0, 0, 0, #params 1-4
33.0,
100.9,
40.6
)
self.vehicle.send_mavlink(msg2)
time.sleep( 1 / UPDATE_RATE )
ShotManager()

302
shotmanager/sim/TestRC.py Executable file
View File

@ -0,0 +1,302 @@
#!/usr/bin/env python
"""
Test script to test sending RC input to shotManager
(Used for testing shotManager in SITL)
Also emulates the button server in stm32 running on Artoolink
This takes user input to change rc values (use the arrow keys)
Only meant for debugging purposes, so it's not production code
"""
#Imports
import sys
import os
import curses
import Queue
import select
import socket
import struct
import threading
import time
sys.path.append(os.path.realpath('..'))
import app_packet
sys.path.append(os.path.realpath('../../../net/usr/bin'))
from sololink import rc_pkt
sys.path.append(os.path.realpath('../../../flightcode/stm32'))
from sololink import btn_msg
#Globals
state = "starting" #state of artoo emulator
stringsUpdated = False
Amapping = "unset"
Bmapping = "unset"
shotName = "unset"
NUM_CHANNELS = 8
channelValues = [1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500]
PORT = 5016
outputLock = threading.Lock()
def main(stdscr):
'''Main function that contains while loop, wrapped by curses'''
#import globals
global state
global stringsUpdated
global Amapping
global Bmapping
global shotName
global clientQueue
stringsUpdated = True
activeChannel = 0
stdscr.nodelay(1)
stdscr.addstr(0,10,"Hit 'q' to quit")
for i in range(NUM_CHANNELS):
stdscr.addstr(i+2, 20, "Channel %d : %d" % (i+1, channelValues[i]))
stdscr.addstr(activeChannel+2, 18, ">>")
stdscr.addstr(12, 10, "Hit up/down to change active channel")
stdscr.addstr(13, 10, "Hit left/right to change stick value")
stdscr.addstr(14, 10, "Hit 'c' to release sticks")
stdscr.refresh()
#start RC thread
rc = threading.Thread(name = "sendRC", target = sendRC, args = ())
rc.daemon = True
rc.start()
#start artoo thread
artooThread = threading.Thread(name = "ArtooButtonThread", target = ArtooButtonThread, args = ())
artooThread.daemon = True
artooThread.start()
key = ''
while key != ord('q'):
key = stdscr.getch()
stdscr.refresh()
if key == curses.KEY_UP:
stdscr.addstr(activeChannel+2, 18, " Channel %d : %d"%(activeChannel + 1, channelValues[activeChannel]))
activeChannel -= 1
if activeChannel < 0:
activeChannel = 0
stdscr.addstr(activeChannel+2, 18, ">>")
if key == curses.KEY_DOWN:
stdscr.addstr(activeChannel+2, 18, " Channel %d : %d"%(activeChannel + 1, channelValues[activeChannel]))
activeChannel += 1
if activeChannel >= NUM_CHANNELS:
activeChannel = NUM_CHANNELS - 1
stdscr.addstr(activeChannel+2, 18, ">>")
if key == curses.KEY_RIGHT:
channelValues[activeChannel] += 10
if channelValues[activeChannel] > 2000:
channelValues[activeChannel] = 2000
stdscr.addstr(activeChannel+2, 18, " Channel %d : %d"%(activeChannel + 1, channelValues[activeChannel]))
if key == curses.KEY_LEFT:
channelValues[activeChannel] -= 10
if channelValues[activeChannel] < 1000:
channelValues[activeChannel] = 1000
stdscr.addstr(activeChannel+2, 18, " Channel %d : %d"%(activeChannel + 1, channelValues[activeChannel]))
if key == ord('c'):
for i in range(NUM_CHANNELS):
channelValues[i] = 1500
stdscr.addstr(i+2, 20, "Channel %d : %d" % (i+1, channelValues[i]))
# hit the A button
if key == ord('a'):
# (timestamp_us, button_id, button_event, pressed_mask)
pkt = struct.pack("<QBBH", 0, btn_msg.ButtonA, btn_msg.Press, 0)
sendPacket(pkt)
if key == ord('b'):
# (timestamp_us, button_id, button_event, pressed_mask)
pkt = struct.pack("<QBBH", 0, btn_msg.ButtonB, btn_msg.Press, 0)
sendPacket(pkt)
if key == ord('p'):
# (timestamp_us, button_id, button_event, pressed_mask)
pkt = struct.pack("<QBBH", 0, btn_msg.ButtonLoiter, btn_msg.Press, 0)
sendPacket(pkt)
if key == ord('f'):
# (timestamp_us, button_id, button_event, pressed_mask)
pkt = struct.pack("<QBBH", 0, btn_msg.ButtonFly, btn_msg.Press, 0)
sendPacket(pkt)
if stringsUpdated:
stdscr.addstr(16, 10, 'Current shot: ' + shotName + ' ')
stdscr.addstr(17, 10, "Hit 'a' to do " + Amapping + ' ')
stdscr.addstr(18, 10, "Hit 'b' to do " + Bmapping + ' ')
stdscr.addstr(19, 10, "Hit 'p' to hit pause button")
stdscr.addstr(20, 10, "Hit 'f' to hit fly button")
stdscr.addstr(21, 10, state + ' ')
stdscr.refresh()
stringsUpdated = False
time.sleep(0.01)
def sendRC():
'''Sends RC to Sololink'''
#import globals
global channelValues
rcSock = None
#Main RC loop
while True:
if not rcSock:
#print "trying to connect"
if os.path.exists( "/tmp/shotManager_RCInputs_socket" ):
rcSock = socket.socket( socket.AF_UNIX, socket.SOCK_DGRAM )
try:
rcSock.connect( "/tmp/shotManager_RCInputs_socket" )
except:
rcSock = None
else:
#print "connected, sending rc info"
try:
pkt = rc_pkt.pack((0, 0, [channelValues[2], channelValues[0],
channelValues[1], channelValues[3], channelValues[4],
channelValues[5], channelValues[6], channelValues[7]]))
rcSock.send(pkt)
#print "sending rc data"
except socket.error:
#print "failed to send"
rcSock = None
time.sleep(1.0 / 50.0)
rcSock.close()
def sendPacket(pkt):
'''Sends packet to client (Sololink)'''
if clientQueue:
global outputPacket
outputLock.acquire()
clientQueue.put(pkt)
outputLock.release()
def ArtooButtonThread():
'''this thread simulates the server running in the stm32 process in Artoolink'''
#import globals
global state
global stringsUpdated
global Amapping
global Bmapping
global shotName
global clientQueue
#establish server
server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
server.setblocking(0)
# Bind the socket to the port
server_address = ('', PORT)
stringsUpdated = True
currentPacket = ""
currentPacketLength = 9999
currentPacketType = None
#blocking connect loop
while True:
try:
server.bind(server_address)
except:
time.sleep(1.0)
pass
else:
break
#update emulator state
state = "bound"
stringsUpdated = True
server.listen(2)
inputs = [server,]
outputs = []
#main emulator loop
while True:
#grab data on pipe
rl, wl, el = select.select( inputs, outputs, [] )
for s in rl:
if s is server:
client, address = server.accept()
client.setblocking(0)
inputs.append(client)
outputs.append(client)
clientQueue = Queue.Queue()
state = "accepted"
stringsUpdated = True
else:
data = s.recv( 1 )
if data:
currentPacket += data
state = "got a packet"
# parse the packet
if len(currentPacket) == app_packet.SOLO_MESSAGE_HEADER_LENGTH:
(currentPacketType, currentPacketLength) = struct.unpack('<II', currentPacket)
if len(currentPacket) == app_packet.SOLO_MESSAGE_HEADER_LENGTH + currentPacketLength:
value = currentPacket[app_packet.SOLO_MESSAGE_HEADER_LENGTH:]
if currentPacketType == btn_msg.ARTOO_MSG_SET_BUTTON_STRING:
state = "Button String"
(button_id, event, shot, mask) = struct.unpack('<BBbb', value[:4])
artooString = value[4:]
if button_id == btn_msg.ButtonA:
# remove null terminator
Amapping = artooString[:-1]
if mask & btn_msg.ARTOO_BITMASK_ENABLED:
Amapping += "(enabled)"
else:
Amapping += "(disabled)"
elif button_id == btn_msg.ButtonB:
# remove null terminator
Bmapping = artooString[:-1]
if mask & btn_msg.ARTOO_BITMASK_ENABLED:
Bmapping += "(enabled)"
else:
Bmapping += "(disabled)"
elif currentPacketType == btn_msg.ARTOO_MSG_SET_SHOT_STRING:
# remove null terminator
shotName = value[:-1]
currentPacket = ""
else:
inputs.remove(s)
outputs.remove(s)
s.close()
client = None
clientQueue = None
stringsUpdated = True
# now handle writes
for s in wl:
outputLock.acquire()
try:
if clientQueue:
msg = clientQueue.get_nowait()
except Queue.Empty:
pass
else:
try:
s.send(msg)
except Exception as e:
state = 'Shotmanager disconnected.'
shotName = 'unset'
Amapping = 'unset'
Bmapping = 'unset'
outputLock.release()
#Calls main function inside a curses wrapper for graceful exit
if __name__ == '__main__':
curses.wrapper(main)

197
shotmanager/sim/TestSocket.py Normal file
View File

@ -0,0 +1,197 @@
#!/usr/bin/env python
"""
Connects to shotManager so you can send it messages. Fun times!
"""
import os
import socket
import struct
import sys
import threading
import logging
import time
sys.path.append(os.path.realpath('..'))
import app_packet
# create socket
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server_address = ('10.1.48.157', 5507)
sock.connect(server_address)
# start listen thread
def listener(sock, verbose):
currentPacket = ""
currentPacketLength = 9999
while True:
data = sock.recv(1)
currentPacket += data
if len(currentPacket) == app_packet.SOLO_MESSAGE_HEADER_LENGTH:
(currentPacketType, currentPacketLength) = struct.unpack('<II', currentPacket)
#print "received packet type is ", currentPacketType, currentPacketLength
if len(currentPacket) == app_packet.SOLO_MESSAGE_HEADER_LENGTH + currentPacketLength:
value = currentPacket[app_packet.SOLO_MESSAGE_HEADER_LENGTH:]
if currentPacketType == app_packet.SOLO_MESSAGE_GET_CURRENT_SHOT:
(shot,) = struct.unpack('<i', value)
print "[App]: Received SOLO_MESSAGE_GET_CURRENT_SHOT"
if verbose:
print "Shot ID: %d" % shot
elif currentPacketType == app_packet.SOLO_MESSAGE_LOCATION:
(lat, lon, alt) = struct.unpack('<ddf', value)
print "[App]: Received SOLO_MESSAGE_LOCATION"
if verbose:
print "Location: %f, %f, %f" % (lat, lon, alt)
elif currentPacketType == app_packet.SOLO_SPLINE_RECORD:
print "[App]: Received SOLO_SPLINE_RECORD"
elif currentPacketType == app_packet.SOLO_SPLINE_PLAY:
print "[App]: Received SOLO_SPLINE_PLAY"
elif currentPacketType == app_packet.SOLO_SPLINE_POINT:
(index, lat, lon, alt, pitch, yaw, uPosition, status) = struct.unpack('<Iddffffh', value)
print "[App]: Received SOLO_SPLINE_POINT"
if verbose:
print "Index: %d" % index
print "Location: %f, %f, %f" % (lat, lon, alt)
print "Pitch, Yaw: %f, %f" % (pitch, yaw)
print "uPosition: %f" % uPosition
print "Status: %d" % status
elif currentPacketType == app_packet.SOLO_SPLINE_ATTACH:
(index,) = struct.unpack('<I', value)
print "[App]: Received SOLO_SPLINE_ATTACH"
if verbose:
print "Index: %d" % index
elif currentPacketType == app_packet.SOLO_SPLINE_PLAYBACK_STATUS:
(uPosition,uVelocity) = struct.unpack('<ff', value)
print "[App]: Received SOLO_SPLINE_PLAYBACK_STATUS"
if verbose:
print "uPosition: %f" % uPosition
print "uVelocity: %f" % uVelocity
elif currentPacketType == app_packet.SOLO_SPLINE_PATH_STATUS:
(minTime,maxTime) = struct.unpack('<ff', value)
print "[App]: Received SOLO_SPLINE_PATH_STATUS"
if verbose:
print "minTime: %f" % minTime
print "maxTime: %f" % maxTime
elif currentPacketType == app_packet.GOPRO_V1_STATE:
#(a,b,c,d,e,f,g,h,i,j,k,l,m,n,o,p,q,r,s,t,u,v,w,x,y,z,aa,bb,cc,dd,ee,ff,gg,hh,ii,jj,kk) = struct.unpack('<BBBBBBBBBBBBBBBBBBBBBBBBBBHHHHH', value)
print "[App]: Received GOPRO_V1_STATE"
if verbose:
pass
elif currentPacketType == app_packet.GOPRO_V2_STATE:
#(a,b,c,d,e,f,g,h,i,j,k,l,m,n,o,p,q,r,s,t,u,v,w,x,y,z,aa,bb,cc,dd,ee,ff,gg,hh,ii,jj,kk) = struct.unpack('<BBBBBBBBBBBBBBBBBBBBBBBBBBHHHHH', value)
print "[App]: Received GOPRO_V2_STATE"
if verbose:
pass
# # Gopromanager handles these messages
# elif self.currentPacketType in GoProManager.GOPROMESSAGES:
# self.goproManager.handlePacket( self.currentPacketType, value )
else:
print "[App]: Got an unknown packet type: %d" % (currentPacketType)
# reset packet parsing
currentPacket = ""
print "Verbose? y/n"
verbose = raw_input(">")
if verbose == "y":
verbose = True
else:
verbose = False
w = threading.Thread(name='listener', target=listener,args=(sock,verbose))
w.setDaemon(True)
w.start()
print "Please enter desired shot."
print "0 = Selfie"
print "1 = Orbit"
print "2 = MP Cable Cam"
print "5 = Follow Me"
print "6 = Classic Cable Cam"
print "7 = Load a MP Cable"
shot = raw_input(">")
shot = int(shot)
step = 0
# main "app" loop
while True:
if shot == 0:
# send shot request to shotManager
packet = struct.pack('<III', app_packet.SOLO_MESSAGE_SET_CURRENT_SHOT, 4, shot)
sock.sendall(packet)
elif shot == 1:
# send shot request to shotManager
packet = struct.pack('<III', app_packet.SOLO_MESSAGE_SET_CURRENT_SHOT, 4, shot)
sock.sendall(packet)
elif shot == 2:
if step == 0:
aCount = 0
bCount = 0
# send shot request to shotManager
packet = struct.pack('<III', app_packet.SOLO_MESSAGE_SET_CURRENT_SHOT, 4, shot)
sock.sendall(packet)
step += 1
elif step == 1:
print "Please press A or B button."
button = raw_input(">")
if button == "A":
packet = struct.pack('<II', app_packet.SOLO_RECORD_POSITION, 0)
sock.sendall(packet)
aCount += 1
elif button == "B":
packet = struct.pack('<II', app_packet.SOLO_RECORD_POSITION, 0)
sock.sendall(packet)
packet = struct.pack('<II', app_packet.SOLO_SPLINE_PLAY, 0)
sock.sendall(packet)
packet = struct.pack('<III', app_packet.SOLO_SPLINE_ATTACH, 4, aCount)
sock.sendall(packet)
packet = struct.pack('<IIhf', app_packet.SOLO_SPLINE_PATH_SETTINGS, 6, 0, 20)
sock.sendall(packet)
step += 1
elif step == 2:
_ = raw_input("Hit enter to send SEEK.")
packet = struct.pack('<IIf', app_packet.SOLO_SPLINE_SEEK, 4, 0.5)
sock.sendall(packet)
step += 1
elif shot == 5:
# send shot request to shotManager
packet = struct.pack('<III', app_packet.SOLO_MESSAGE_SET_CURRENT_SHOT, 4, shot)
sock.sendall(packet)
elif shot == 6:
# send shot request to shotManager
packet = struct.pack('<III', app_packet.SOLO_MESSAGE_SET_CURRENT_SHOT, 4, shot)
sock.sendall(packet)
elif shot == 7:
if step == 0:
# send shot request to shotManager
packet = struct.pack('<III', app_packet.SOLO_MESSAGE_SET_CURRENT_SHOT, 4, 2)
sock.sendall(packet)
#print "[App]: Sent SOLO_MESSAGE_SET_CURRENT_SHOT"
packet = struct.pack('<IIIddffffh', app_packet.SOLO_SPLINE_POINT, 38, 0, 37.330674, -122.028759, 15, 0, 90, 0, 0)
sock.sendall(packet)
#print "[App]: Sent SOLO_SPLINE_POINT"
packet = struct.pack('<IIIddffffh', app_packet.SOLO_SPLINE_POINT, 38, 1, 37.331456, -122.027785, 15, 0, 45, 0, 0)
sock.sendall(packet)
#print "[App]: Sent SOLO_SPLINE_POINT"
packet = struct.pack('<IIIddffffh', app_packet.SOLO_SPLINE_POINT, 38, 2, 37.332216, -122.027842, 15, 0, 0, 0, 0)
sock.sendall(packet)
#print "[App]: Sent SOLO_SPLINE_POINT"
packet = struct.pack('<II', app_packet.SOLO_SPLINE_PLAY, 0)
sock.sendall(packet)
#print "[App]: Sent SOLO_SPLINE_PLAY"
packet = struct.pack('<III', app_packet.SOLO_SPLINE_ATTACH, 4, 0)
sock.sendall(packet)
#print "[App]: Sent SOLO_SPLINE_ATTACH"
step += 1

1
shotmanager/sim/__init__.py Normal file
View File

@ -0,0 +1 @@
import rc_ipc_shim

80
shotmanager/sim/rc_ipc_shim.py Normal file
View File

@ -0,0 +1,80 @@
import socket
import time
import struct
from threading import Thread
from sololink import rc_pkt
rc_sock = None
rc_attached = False
rc_actual = [1500, 1500, 900, 1500, 0, 0, 0, 0]
rc_override = [1500, 1500, 900, 1500, 0, 0, 0, 0]
def attach():
global rc_attached
rc_attached = True
def detach():
global rc_attached
rc_attached = False
def put(arg):
global rc_override
(timestamp, sequence, chan) = arg
rc_override = [chan[2], chan[1], chan[0], chan[3], chan[4], chan[5], chan[6], chan[7]]
return True
def pixrc_start():
global rc_sock
global rc_actual
if not rc_sock:
rc_sock = socket.socket( socket.AF_INET, socket.SOCK_DGRAM )
rc_sock.setblocking(0)
rc_actual = [1500, 1500, 900, 1500, 0, 0, 0, 0]
def listener():
global rc_sock, rc_actual
sock = socket.socket( socket.AF_INET, socket.SOCK_DGRAM )
sock.bind(('0.0.0.0', 13341))
while True:
try:
data = sock.recv( 1000 )
if data == None or len(data) != rc_pkt.LENGTH:
continue
(timestamp, sequence, chan) = rc_pkt.unpack(data)
rc_actual = [chan[2], chan[1], chan[0], chan[3], chan[4], chan[5], chan[6], chan[7]]
except Exception as e:
print(e)
if rc_sock == None:
return
t_l = Thread(target=listener)
t_l.daemon = True
t_l.start()
def sender():
global rc_sock, rc_override, rc_attached
while True:
time.sleep(.020)
pkt = struct.pack('<HHHHHHHH', *(rc_override if rc_attached else rc_actual))
try:
# print('--->', rc_attached, rc_override if rc_attached else rc_actual)
rc_sock.sendto(pkt, ('127.0.0.1', 5501))
except Exception as e:
print(e)
if rc_sock == None:
return
t_s = Thread(target=sender)
t_s.daemon = True
t_s.start()
def pixrc_stop():
global rc_sock
if rc_sock:
rc_sock.close()
rc_sock = None

97
shotmanager/sim/shotmanager.conf Normal file
View File

@ -0,0 +1,97 @@
[solo]
artooip = 10.1.1.1
soloip = 10.1.1.10
rcdestport = 5005
sysdestip = %(artooIp)s
sysdestport = 5012
pairreqdestport = 5013
pairresdestport = 5014
mavdestport = 5015
buttoneventport = 5016
buttonfunctionconfigdestport = 5017
setshotinfodestport = 5018
telemdestport = 14550
appserverport = 5502
appaddressfile = /var/run/solo_app.ip
stm32dev = /dev/ttymxc1
stm32baud = 115200
telemdev = /dev/ttymxc1
telembaud = 57600
rcdsmdev = /dev/ttymxc2
rcdsmbaud = 115200
rcsourceips = 10.1.1.1,127.0.0.1
usegpstime = True
[pairing]
user_confirmation_timeout = 30.0
controller_link_port = 5501
ssid_pattern = SoloLink*
wifi_connect_timeout = 5.0
connect_request_interval = 1.0
connect_request_max = 3
connect_ack_timeout = 0.5
solo_address_file = /var/run/solo.ip
button_filename = /dev/input/event0
wifi_reverse = False
[net]
apenable = True
stationenable = False
[loggers]
keys = root,shot
[handlers]
keys = consoleHandler,sysLogHandler,sysLog2Handler
[formatters]
keys = simpleFormatter,syslogFormatter
[logger_root]
level = INFO
handlers = consoleHandler
[logger_shot]
level=INFO
handlers=sysLog2Handler
qualname=shot
propagate=0
[logger_vid]
level = INFO
handlers = varLogHandler,consoleHandler
qualname = vid
propagate = 0
[handler_consoleHandler]
class = StreamHandler
level = ERROR
formatter = simpleFormatter
args = (sys.stdout,)
[handler_sysLogHandler]
class=handlers.SysLogHandler
level=DEBUG
formatter=syslogFormatter
args=("/var/run/syslog", handlers.SysLogHandler.LOG_LOCAL2)
[handler_sysLog2Handler]
class=handlers.SysLogHandler
level=DEBUG
formatter=syslogFormatter
args=("/var/run/syslog", handlers.SysLogHandler.LOG_LOCAL2)
[formatter_simpleFormatter]
format = %(asctime)s %(name)s %(levelname)-8s %(message)s
datefmt =
[formatter_syslogFormatter]
format=%(name)s: %(message)s
datefmt=
[shotManager]
a = -1, 5
b = -1, 2
A = 2, -1
B = -1, 0
GoProEnabled=1

184
shotmanager/sim/shotmanager.sandbox.conf Normal file
View File

@ -0,0 +1,184 @@
[solo]
artooIp=10.1.1.1
soloIp=10.1.1.10
# Address to which RC packets are sent
rcDestPort=5005
# Address to which 'sysinfo' packet for the STM32 is sent
sysDestIp=%(artooIp)s
sysDestPort=5012
# Port to which 'pair request' packet for the STM32 is sent
pairReqDestPort=5013
# Port to which 'pair result' packet for the STM32 is sent
pairResDestPort=5014
# Port to which MAVLink packets for the STM32 are sent
mavDestPort=5015
# Port to connect to for button events (TCP)
buttonEventPort=5016
# Port to send button function config messages to
buttonFunctionConfigDestPort=5017
# Port to send set shot info messages to
setShotInfoDestPort=5018
# Port to send updater messages to
updaterDestPort=5019
# Port to which MAVLink packets are sent for all external systems
telemDestPort=14550
# TCP port where app server listens for connections
appServerPort=5502
# File where app_server saves connect app's IP address
appAddressFile=/var/run/solo_app.ip
# Artoo's serial ports
# Console is /dev/ttymxc0
# STM32
stm32Dev=/dev/ttymxc1
stm32Baud=115200
# Solo's serial ports
# Console is /dev/ttymxc0
# Pixhawk telemetry
telemDev=/dev/ttymxc1
telemBaud=921600
telemFlow=True
# Telemetry logging control
telemLogGap=1000000
telemLogDelayMax=100000
#telemLogDelayFile=/tmp/pkt_delays.csv
# Pixhawk RC
rcDsmDev=/dev/ttymxc2
rcDsmBaud=115200
# IP addresses from which Solo accepts RC packets
rcSourceIps=10.1.1.1,127.0.0.1
# Set system time from GPS when available
useGpsTime=True
# Throttle PWM mapping
pwmInMinThrottle=1000
pwmInMaxThrottle=2000
pwmOutMinThrottle=1000
pwmOutMaxThrottle=1900
# Rc timeout max
rcTimeoutUS=400000
# Telemetry display units (metric or imperial)
uiUnits=metric
[pairing]
user_confirmation_timeout = 30.0
controller_link_port = 5501
wifi_connect_timeout = 5.0
connect_request_interval = 1.0
connect_ack_timeout = 0.5
solo_address_file = /var/run/solo.ip
button_filename = /dev/input/event0
[net]
ApEnable=True
StationEnable=False
[loggers]
keys=root,stm32,pix,pair,net,app,tlm,shot
[handlers]
keys=consoleHandler
[formatters]
keys=simpleFormatter,syslogFormatter
[logger_root]
level=INFO
handlers=consoleHandler
[logger_stm32]
level=INFO
handlers=consoleHandler
qualname=stm32
propagate=0
[logger_pix]
level=INFO
handlers=consoleHandler
qualname=pix
propagate=0
[logger_pair]
level=INFO
handlers=consoleHandler
qualname=pair
propagate=0
[logger_net]
level=INFO
handlers=consoleHandler
qualname=net
propagate=0
[logger_app]
level=INFO
handlers=consoleHandler
qualname=app
propagate=0
[logger_tlm]
level=INFO
handlers=consoleHandler
qualname=tlm
propagate=0
[logger_shot]
level=INFO
handlers=consoleHandler
qualname=shot
propagate=0
[handler_consoleHandler]
class=StreamHandler
level=DEBUG
formatter=simpleFormatter
args=(sys.stdout,)
[formatter_simpleFormatter]
format=%(asctime)s %(name)-4s %(levelname)-8s %(message)s
datefmt=
[formatter_syslogFormatter]
format=%(name)s: %(message)s
datefmt=
[shotManager]
# button mappings
# shot, apmMode
A=2, -1
B=1, -1
goproenabled=0
[video]
vidTargetBitrate=2000000
vidOptimalBitrateRatio=100
vidFramerate=24
vidQOSValue=32
vidPosGain=1000
vidNegGain=2000
vidSysGain=120

293
shotmanager/transect.py Normal file
View File

@ -0,0 +1,293 @@
#
# transect.py
# shotmanager
#
# The transect smart shot controller.
# Runs as a DroneKit-Python script under MAVProxy.
#
# Created by Jason Short
# Copyright (c) 2015 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from dronekit import Vehicle, LocationGlobalRelative, VehicleMode
from pymavlink import mavutil
import os
from os import sys, path
import math
import struct
sys.path.append(os.path.realpath(''))
import app_packet
import camera
import location_helpers
import pathHandler
import shotLogger
import shots
from shotManagerConstants import *
import GoProManager
from GoProConstants import *
from sololink import btn_msg
# length of the transect line (meters)
MAX_TRANSECT_LENGTH = 500
MIN_TRANSECT_LENGTH = 50
#speed of vehicle m/s
TRANSECT_SPEED = 1.5
# we can take a photo every 2 seconds
MIN_TRIGGER_DIST = TRANSECT_SPEED * 2
OVERLAP = .8
STARTUP_DELAY = UPDATE_RATE * 2
RETURN_TO_START = 4
FINISH = 3
RUN = 2
TILT = 1
SETUP = 0
YAW_SPEED = 60.0
logger = shotLogger.logger
class TransectShot():
def __init__(self, vehicle, shotmgr):
# assign the vehicle object
self.vehicle = vehicle
# assign the shotManager object
self.shotmgr = shotmgr
# default pathHandler to none
self.pathHandler = None
# default gimbal into mavlink targeting mode
self.setupTargeting()
# initialize roi object to none
self.roi = None
# default camYaw to current pointing
self.camYaw = camera.getYaw(self.vehicle)
# enter GUIDED mode
self.vehicle.mode = VehicleMode("GUIDED")
#disable stick deadzones for throttle and yaw
self.shotmgr.rcMgr.enableRemapping( True )
#state machine
self.state = SETUP
#state machine
self.lastShot = -1
# distance traveled
self.dist = 0
# distance between photos in M
self.triggerDist = 10
# length of cable
self.transectLength = MIN_TRANSECT_LENGTH
self.start_loc = None
# used for timing
self.ticks = 0
# set Camera to Photo
self.enterPhotoMode()
# channels are expected to be floating point values in the (-1.0, 1.0) range
def handleRCs(self, channels):
if self.state == SETUP:
# point stright ahead
self.camPitch = 0
# allow user to point Solo
self.manualYaw(channels)
self.ticks = 0
elif self.state == TILT:
# point straight down
self.camPitch = -90
self.ticks += 1
if self.ticks > STARTUP_DELAY:
# take first shot
self.shotmgr.goproManager.handleRecordCommand(self.shotmgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE)
self.pathHandler.setCruiseSpeed(TRANSECT_SPEED) #temp
self.state = RUN
elif self.state == RUN:
self.ticks = 0
self.calcPhoto()
self.pathHandler.MoveTowardsEndpt(channels)
# pause at the end of "infinity"!
if self.pathHandler.isNearTarget():
self.pathHandler.pause()
#take 1 more photo
self.shotmgr.goproManager.handleRecordCommand(self.shotmgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE)
self.state = FINISH
elif self.state == FINISH:
self.ticks += 1
if self.ticks > STARTUP_DELAY:
logger.log("[Transect] Finished")
self.state = RETURN_TO_START
self.pathHandler.setCruiseSpeed(-6)
elif self.state == RETURN_TO_START:
self.pathHandler.MoveTowardsEndpt(channels)
if self.pathHandler.isNearTarget():
self.pathHandler.currentSpeed = 0.0
self.pathHandler.pause()
self.shotmgr.enterShot(shots.APP_SHOT_NONE)
self.handlePitchYaw()
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 setupWaypoints(self):
'''setup our two waypoints'''
# we want two points that are far apart
# both in the direction we're looking and the opposite direction
# get vehicle state
self.start_loc = self.vehicle.location.global_relative_frame
self.triggerDist = self.start_loc.alt * OVERLAP
self.triggerDist = max(self.triggerDist, MIN_TRIGGER_DIST)
# calculate the foward point
forwardPt = location_helpers.newLocationFromAzimuthAndDistance(self.start_loc, self.camYaw, self.transectLength)
# create a new pathHandler obejct with our new points
# this will automatically reset resumeSpeed, cruiseSpeed etc...
self.pathHandler = pathHandler.TwoPointPathHandler(self.start_loc, forwardPt, self.vehicle, self.shotmgr)
#self.addLocation(forwardPt)
def handleButton(self, button, event):
if self.state == SETUP and button == btn_msg.ButtonA and event == btn_msg.Press:
self.state = TILT
self.setupWaypoints()
self.setButtonMappings()
if self.state == SETUP and button == btn_msg.ButtonB and event == btn_msg.Press:
self.transectLength += 25
if self.transectLength > MAX_TRANSECT_LENGTH:
self.transectLength = MIN_TRANSECT_LENGTH
# redo path
self.setButtonMappings()
if button == btn_msg.ButtonLoiter and event == btn_msg.Press:
if self.pathHandler:
self.pathHandler.togglePause()
self.updateAppOptions()
def setButtonMappings(self):
buttonMgr = self.shotmgr.buttonManager
if self.state == SETUP:
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_TRANSECT, btn_msg.ARTOO_BITMASK_ENABLED, "Begin\0")
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_TRANSECT, btn_msg.ARTOO_BITMASK_ENABLED, str(self.transectLength)+"m\0")
else:
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_TRANSECT, 0, "\0")
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_TRANSECT, btn_msg.ARTOO_BITMASK_ENABLED, "("+str(self.transectLength)+"m)\0")
def updateAppOptions(self):
'''send our current set of options to the app'''
return
def addLocation(self, loc):
'''called by shot manager to set new ROI from App'''
return
def manualYaw(self, channels):
if channels[YAW] == 0:
return
self.camYaw += channels[YAW] * YAW_SPEED * UPDATE_TIME
if self.camYaw > 360:
self.camYaw -= 360
if self.camYaw < 0:
self.camYaw += 360
def setupTargeting(self):
'''set gimbal targeting mode'''
msg = self.vehicle.message_factory.mount_configure_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, # mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
logger.log("setting gimbal to mavlink mode")
self.vehicle.send_mavlink(msg)
def handlePitchYaw(self):
'''Handle free look'''
# if we do have a gimbal, use mount_control to set pitch and yaw
if self.vehicle.mount_status[0] is not None:
msg = self.vehicle.message_factory.mount_control_encode(
0, 1, # target system, target component
# pitch is in centidegrees
self.camPitch * 100,
0.0, # roll
# yaw is in centidegrees
self.camYaw * 100,
0 # save position
)
self.vehicle.send_mavlink(msg)
def enterPhotoMode(self):
# switch into photo mode if we aren't already in it
if self.shotmgr.goproManager.captureMode != CAPTURE_MODE_PHOTO:
self.shotmgr.goproManager.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, (CAPTURE_MODE_PHOTO, 0 ,0 , 0))
def handlePacket(self, packetType, packetLength, packetValue):
try:
return False
except Exception as e:
logger.log('[selfie]: Error handling packet. (%s)' % e)
return False
else:
return True

64
shotmanager/vector2.py Normal file
View File

@ -0,0 +1,64 @@
# vector2.py
# shotmanager
#
# A two-dimensional vector class.
#
# Created by Will Silva on 11/22/2015.
# Copyright (c) 2015 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import math
class Vector2:
def __init__(self, x, y):
self.x = x
self.y = y
# normalizes self and returns the length
def normalize(self):
length2 = self.x * self.x + self.y * self.y
length = math.sqrt(length2)
if length != 0: # if our vector is a zero vector
self.x /= length
self.y /= length
return length
def length(self):
length2 = self.x * self.x + self.y * self.y
return math.sqrt(length2)
def dot(a, b):
return a.x * b.x + a.y * b.y
def __add__(self, vector):
return Vector2(self.x + vector.x, self.y + vector.y)
def __sub__(self, vector):
return Vector2(self.x - vector.x, self.y - vector.y)
def __mul__(self, scalar):
return Vector2(self.x * scalar, self.y * scalar)
def __rmul__(self, scalar):
return Vector2(scalar * self.x, scalar * self.y)
def __repr__(self):
return "<%f,%f>" % (self.x, self.y)
def __str__(self):
return "<%f,%f>" % (self.x, self.y)
def __iter__(self):
return iter([self.x, self.y])

63
shotmanager/vector3.py Executable file
View File

@ -0,0 +1,63 @@
#!/usr/bin/env python
# Vector3 class - super minimal; just adding things as I need them
import math
class Vector3:
def __init__(self, x=0, y=0, z=0):
self.x = x
self.y = y
self.z = z
# normalizes self and returns the length
def normalize(self):
length2 = self.x * self.x + self.y * self.y + self.z * self.z
length = math.sqrt(length2)
if length != 0: #if our vector is a zero vector
self.x /= length
self.y /= length
self.z /= length
return length
def length(self):
length2 = self.x * self.x + self.y * self.y + self.z * self.z
return math.sqrt(length2)
def cross(a,b):
return Vector3(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y-a.y*b.x)
def dot(a,b):
return a.x*b.x + a.y*b.y + a.z*b.z
def __eq__(self,vector):
return (self.x == vector.x and self.y == vector.y and self.z == vector.z)
def __ne__(self,vector):
return (self.x != vector.x or self.y != vector.y or self.z != vector.z)
def __add__(self, vector):
return Vector3(self.x + vector.x, self.y + vector.y, self.z + vector.z)
def __sub__(self, vector):
return Vector3(self.x - vector.x, self.y - vector.y, self.z - vector.z)
def __neg__(self):
return Vector3(-self.x, -self.y, -self.z)
def __mul__(self, scalar):
return Vector3(self.x * scalar, self.y * scalar, self.z * scalar)
def __rmul__(self, scalar):
return Vector3(scalar * self.x, scalar * self.y, scalar * self.z)
def __div__(self, scalar):
return Vector3(self.x/scalar, self.y/scalar, self.z/scalar)
def __repr__(self):
return "<%f,%f,%f>" % (self.x,self.y,self.z)
def __str__(self):
return "<%f,%f,%f>" % (self.x,self.y,self.z)
def __iter__(self):
return iter([self.x,self.y,self.z])

183
shotmanager/vectorPathHandler.py Normal file
View File

@ -0,0 +1,183 @@
#
# Code common across shots to handle movement on paths
#
from pymavlink import mavutil
import location_helpers
import shotLogger
from pathHandler import PathHandler
from shotManagerConstants import *
import math
from vector3 import Vector3
logger = shotLogger.logger
#Path accel/decel constants
WPNAV_ACCEL = 3.4
WPNAV_ACCEL_Z = 1.6
# for 3D max speed
HIGH_PATH_SPEED = 5.0
LOW_PATH_SPEED = 1.5
MAX_PATH_SPEED = HIGH_PATH_SPEED + LOW_PATH_SPEED
# used to correct for drag or other factors
ERROR_P = .01
# special case of PathHandler
class VectorPathHandler(PathHandler):
def __init__(self, vehicle, shotManager, heading, pitch):
PathHandler.__init__(self, vehicle, shotManager)
# the initial reference position
self.initialLocation = vehicle.location.global_relative_frame
self.heading = heading
# creates a unit vector from telemetry data
self.unitVector = self.getUnitVectorFromHeadingAndTilt(heading, pitch)
# limit speed based on vertical component
# We can't go full speed vertically
# this section should be 2.0 to 8.0 m/s
# to generate a nice speed limiting curve we scale it.
# pitch is used to generate the vertical portion of the 3d Vector
pitch = min(pitch, 0) # level
pitch = max(pitch, -90) # down
accelXY = shotManager.getParam( "WPNAV_ACCEL", WPNAV_ACCEL ) / 100.0
accelZ = shotManager.getParam( "WPNAV_ACCEL_Z", WPNAV_ACCEL_Z ) / 100.0
cos_pitch = math.cos(math.radians(pitch))
self.maxSpeed = LOW_PATH_SPEED + (cos_pitch**3 * HIGH_PATH_SPEED)
self.maxSpeed = min(self.maxSpeed, MAX_PATH_SPEED)
self.accel = accelZ + (cos_pitch**3 * (accelXY - accelZ))
self.accel *= UPDATE_TIME
# the current distance from the intitial location
self.distance = 0.0
#for synthetic acceleration
self.currentSpeed = 0.0
self.desiredSpeed = 0.0
self.distError = 0.0
# given RC input, calculate a speed to move along vector
def move(self, channels):
# allows travel along the vector
# use the max of them
if abs(channels[ROLL]) > abs(channels[PITCH]):
userInput = channels[ROLL]
else:
userInput = -channels[PITCH]
# user controls speed
if self.cruiseSpeed == 0.0:
self.desiredSpeed = userInput * self.maxSpeed
# cruise control
else:
speed = abs(self.cruiseSpeed)
# if sign of stick and cruiseSpeed don't match then...
if math.copysign(1, userInput) != math.copysign(1, self.cruiseSpeed): # slow down
speed *= (1.0 - abs(userInput))
else: # speed up
speed += (self.maxSpeed - speed) * abs(userInput)
# carryover user input sign
if self.cruiseSpeed < 0:
speed = -speed
# limit speed
if speed > self.maxSpeed:
speed = self.maxSpeed
elif -speed > self.maxSpeed:
speed = -self.maxSpeed
self.desiredSpeed = speed
# Synthetic acceleration
if self.desiredSpeed > self.currentSpeed:
self.currentSpeed += self.accel
self.currentSpeed = min(self.currentSpeed, self.desiredSpeed)
elif self.desiredSpeed < self.currentSpeed:
self.currentSpeed -= self.accel
self.currentSpeed = max(self.currentSpeed, self.desiredSpeed)
else:
self.currentSpeed = self.desiredSpeed
# the distance to fly along the vectorPath
self.distance += self.currentSpeed * UPDATE_TIME
self.distance += self.distError * ERROR_P
# generate Guided mode commands to move the copter
self.travel()
# report speed output
return abs(self.currentSpeed)
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 getUnitVectorFromHeadingAndTilt(self, heading, tilt):
''' generate a vector from the camera gimbal '''
angle = math.radians(90 - heading)
tilt = math.radians(tilt)
# create a vector scaled by tilt
x = math.cos(tilt)
# Rotate the vector
nx = x * math.cos(angle)
ny = x * math.sin(angle)
# Up
z = math.sin(tilt)
return Vector3(ny, nx, z)

152
shotmanager/yawPitchOffsetter.py Normal file
View File

@ -0,0 +1,152 @@
#
# Code common across shots to offset yaw or pitch
#
from shotManagerConstants import *
# in degrees per second
# The user will be able to offset yaw from the interpolated position at this speed
# this is the speed the user is able to offset yaw at
YAW_OFFSET_SPEED = 60.0
# this is the speed that the yaw offset corrects itself
# should be less than YAW_OFFSET_SPEED
YAW_CORRECT_SPEED = 9.0
# amount a user is able to offset yaw (in degrees)
YAW_OFFSET_THRESHOLD = 120.0
#pitch offset constants
# The user will be able to offset pitch from the interpolated position at this speed
# this is the speed the user is able to offset pitch at
PITCH_OFFSET_SPEED = 90.0
# this is the speed that the pitch offset corrects itself
# should be less than PITCH_OFFSET_SPEED
PITCH_CORRECT_SPEED = 9.0
# amount a user is able to offset pitch (in degrees)
PITCH_OFFSET_THRESHOLD = 30.0
# our gimbal threshold in the no nudge case
PITCH_OFFSET_NO_NUDGE_THRESHOLD = -90.0
class YawPitchOffsetter():
# pass in False for handlePitch if you don't want it overriding pitch
def __init__(self, handlePitch = True):
self.yawOffset = 0.0
self.pitchOffset = 0.0
# 1 is clockwise, -1 is counter clockwise
self.yawDir = 1
self.handlePitch = handlePitch
"""
We default to nudge mode. In this mode, we only calculate offsets to
a real yaw/pitch. We're capped by the above thresholds.
If this is off, then our yaw/pitch offsets become actual yaw/pitch values
"""
self.isNudge = True
self.freeLook = False
# given RC input, update our pitch/yaw offsets
# returns whether we changed yaw/pitch offset or not
def Update(self, channels):
lastYaw = self.yawOffset
lastPitch = self.pitchOffset
self.offsetYaw( channels[3] )
if self.handlePitch:
if self.freeLook:
value = channels[0]
else:
if abs(channels[7]) > abs(channels[0]):
value = channels[7]
else:
value = channels[0]
self.offsetPitch( value )
if lastYaw != self.yawOffset:
return True
if lastPitch != self.pitchOffset:
return True
return False
def offsetYaw( self, yawStick ):
# if stick is centered, return yaw offset to 0.0
if yawStick == 0.0:
# only if we're in nudge mode
if self.isNudge:
if self.yawOffset > 0.0:
self.yawOffset -= YAW_CORRECT_SPEED * UPDATE_TIME
if self.yawOffset < 0.0:
self.yawOffset = 0.0
self.yawDir = -1
else:
self.yawOffset += YAW_CORRECT_SPEED * UPDATE_TIME
if self.yawOffset > 0.0:
self.yawOffset = 0.0
self.yawDir = 1
return
self.yawOffset += YAW_OFFSET_SPEED * yawStick * UPDATE_TIME
if self.isNudge:
if self.yawOffset > YAW_OFFSET_THRESHOLD:
self.yawOffset = YAW_OFFSET_THRESHOLD
elif self.yawOffset < -YAW_OFFSET_THRESHOLD:
self.yawOffset = -YAW_OFFSET_THRESHOLD
else:
# otherwise, the only constraints we have are to keep it in a (0.0, 360.0) range
if self.yawOffset < 0.0:
self.yawOffset += 360.0
elif self.yawOffset > 360.0:
self.yawOffset -= 360.0
if yawStick > 0.0:
self.yawDir = 1
else:
self.yawDir = -1
def offsetPitch( self, gimbalPaddle ):
# if stick is centered, return pitch offset to 0.0
if gimbalPaddle == 0.0:
# only if we're in nudge mode
if self.isNudge:
if self.pitchOffset > 0.0:
self.pitchOffset -= PITCH_CORRECT_SPEED * UPDATE_TIME
if self.pitchOffset < 0.0:
self.pitchOffset = 0.0
else:
self.pitchOffset += PITCH_CORRECT_SPEED * UPDATE_TIME
if self.pitchOffset > 0.0:
self.pitchOffset = 0.0
return
self.pitchOffset += PITCH_OFFSET_SPEED * gimbalPaddle * UPDATE_TIME
if self.isNudge:
if self.pitchOffset > PITCH_OFFSET_THRESHOLD:
self.pitchOffset = PITCH_OFFSET_THRESHOLD
elif self.pitchOffset < -PITCH_OFFSET_THRESHOLD:
self.pitchOffset = -PITCH_OFFSET_THRESHOLD
else:
# In the non nudge case, we have a different set of constraints
# These are just the gimbal constraints (-90.0, 0.0)
if self.pitchOffset > 0.0:
self.pitchOffset = 0.0
elif self.pitchOffset < PITCH_OFFSET_NO_NUDGE_THRESHOLD:
self.pitchOffset = PITCH_OFFSET_NO_NUDGE_THRESHOLD
# This turns on nudge mode.
# It also clears out yaw/pitch offsets
def enableNudge(self):
self.isNudge = True
self.pitchOffset = 0.0
self.yawOffset = 0.0
# This turns off nudge mode,
# initializing yaw/pitch offsets to the given values
def disableNudge(self, pitch, yaw):
self.isNudge = False
self.pitchOffset = pitch
self.yawOffset = yaw

391
shotmanager/zipline.py Normal file
View File

@ -0,0 +1,391 @@
#
# zipline.py
# shotmanager
#
# The zipline smart shot controller.
# Runs as a DroneKit-Python script.
#
# Created by Jason Short
# Copyright (c) 2016 3D Robotics.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from dronekit import Vehicle, LocationGlobalRelative, VehicleMode
from pymavlink import mavutil
import os
from os import sys, path
import math
import struct
sys.path.append(os.path.realpath(''))
import app_packet
import camera
import location_helpers
import vectorPathHandler
import shotLogger
import shots
from shotManagerConstants import *
# on host systems these files are located here
sys.path.append(os.path.realpath('../../flightcode/stm32'))
from sololink import btn_msg
SHALLOW_ANGLE_THRESHOLD = -5
ROI_ALT_MAX_DELTA = 50
# States
ZIPLINE_SETUP = 0
ZIPLINE_RUN = 1
# Camera modes
FREE_LOOK = 0
SPOT_LOCK = 1
YAW_SPEED = 60.0
PITCH_SPEED = 60.0
YAW_NUDGE_SPEED = 35.0
FOLLOW_ALT_NUDGE_SPEED = 6
MIN_PADDLE_THRESHOLD = 0.02
ROI_ALT_MARGIN = 5
logger = shotLogger.logger
class ZiplineShot():
def __init__(self, vehicle, shotmgr):
self.vehicle = vehicle
self.shotmgr = shotmgr
# Limit ziplines to a plane parallel with Earth surface
self.is3D = False
# default pathHandler to none
self.pathHandler = None
# track cruise speed between pathHandler instances
self.cruiseSpeed = 0
# tracks the camera control mode
self.camPointing = FREE_LOOK
# ROI will update when this is True
self.needsUpdate = True
# Camera control
self.camYaw = camera.getYaw(self.vehicle)
self.camPitch = camera.getPitch(self.vehicle)
self.camDir = 1
# initialize roi object to none
# roi used to store spot lock location object
self.roi = None
self.state = ZIPLINE_SETUP
# channels are expected to be floating point values in the (-1.0, 1.0) range
def handleRCs(self, channels):
if self.state == ZIPLINE_SETUP:
return
# handle camera per camera mode
if self.camPointing == SPOT_LOCK:
self.handleSpotLock(channels)
else:
# Freelook
self.manualPitch(channels)
self.manualYaw(channels)
self.handleFreeLookPointing()
# Vechicle control on the zipline
if self.pathHandler is not None:
self.pathHandler.move(channels)
def setupZipline(self):
if self.state == ZIPLINE_SETUP:
# enter GUIDED mode
self.vehicle.mode = VehicleMode("GUIDED")
# Take over RC
self.shotmgr.rcMgr.enableRemapping( True )
# default camera mode to FREE LOOK
self.camPointing = -1
self.initCam(FREE_LOOK)
self.state = ZIPLINE_RUN
# re-init Yaw
self.camYaw = camera.getYaw(self.vehicle)
# null pitch if we only want 2D ziplines
if self.is3D == 0:
self.camPitch = 0
# create a new pathHandler obejct with our new points
self.pathHandler = vectorPathHandler.VectorPathHandler(self.vehicle, self.shotmgr, self.camYaw, self.camPitch)
self.pathHandler.setCruiseSpeed(self.cruiseSpeed)
# re-init Pitch
self.camPitch = camera.getPitch(self.vehicle)
# update the app
self.updateAppOptions()
self.updateAppStart()
def setButtonMappings(self):
buttonMgr = self.shotmgr.buttonManager
buttonMgr.setArtooButton(
btn_msg.ButtonA, shots.APP_SHOT_ZIPLINE, btn_msg.ARTOO_BITMASK_ENABLED, "New Zipline\0")
if self.camPointing == SPOT_LOCK:
buttonMgr.setArtooButton(
btn_msg.ButtonB, shots.APP_SHOT_ZIPLINE, btn_msg.ARTOO_BITMASK_ENABLED, "Free Look\0")
else:
buttonMgr.setArtooButton(
btn_msg.ButtonB, shots.APP_SHOT_ZIPLINE, btn_msg.ARTOO_BITMASK_ENABLED, "Spot Lock\0")
def handleButton(self, button, event):
if button == btn_msg.ButtonA and event == btn_msg.Press:
self.setupZipline()
if button == btn_msg.ButtonB and event == btn_msg.Press:
# Toggle between free look and spot lock
if self.camPointing is FREE_LOOK:
self.initCam(SPOT_LOCK)
else:
self.initCam(FREE_LOOK)
if button == btn_msg.ButtonLoiter and event == btn_msg.Press:
if self.pathHandler:
self.pathHandler.togglePause()
self.cruiseSpeed = self.pathHandler.cruiseSpeed
self.updateAppOptions()
def initCam(self, _camPointing):
if _camPointing == self.camPointing:
return
if _camPointing is SPOT_LOCK:
self.spotLock()
self.camPointing = _camPointing
else:
self.manualGimbalTargeting()
self.camPointing = _camPointing
self.setButtonMappings()
self.updateAppOptions()
def updateAppOptions(self):
'''send our current set of options to the app'''
# B = uint_8
if self.pathHandler is None:
return
packet = struct.pack('<IIfBB', app_packet.SOLO_ZIPLINE_OPTIONS, 6, self.pathHandler.cruiseSpeed, self.is3D, self.camPointing)
self.shotmgr.appMgr.sendPacket(packet)
def updateAppStart(self):
'''Let app know we've started'''
if self.pathHandler is None:
return
logger.log("[ZIPLINE]: send App Start")
packet = struct.pack('<II', app_packet.SOLO_ZIPLINE_LOCK, 0)
self.shotmgr.appMgr.sendPacket(packet)
def handlePacket(self, packetType, packetLength, packetValue):
'''handle incoming data from the client app'''
try:
if packetType == app_packet.SOLO_MESSAGE_LOCATION:
(lat, lon, alt) = struct.unpack('<ddf', packetValue)
logger.log("[ZIPLINE]: Location received from app: %f, %f, %f." %( lat, lon, alt ) )
# dont read alt from App - it has no way to set it from UI
self.addLocation(LocationGlobalRelative(lat, lon, self.roi.alt))
elif packetType == app_packet.SOLO_ZIPLINE_OPTIONS:
(self.cruiseSpeed, self.is3D, _camPointing) = struct.unpack('<fBB', packetValue)
logger.log( "[ZIPLINE]: Set cruise speed to %f"% (self.cruiseSpeed,))
logger.log( "[ZIPLINE]: Set 3D path %d"% (self.is3D,))
logger.log( "[ZIPLINE]: Cam pointing %d"% (_camPointing,))
self.setButtonMappings()
self.initCam(_camPointing)
if self.pathHandler:
self.pathHandler.setCruiseSpeed(self.cruiseSpeed)
elif packetType == app_packet.SOLO_ZIPLINE_LOCK:
self.setupZipline()
else:
return False
except Exception as e:
logger.log('[ZIPLINE]: Error handling packet. (%s)' % e)
return False
else:
return True
def addLocation(self, loc):
'''called by shot manager to set new ROI from App'''
# replaces our ROI
self.roi = loc
# send this ROI to the app
packet = struct.pack('<IIddf', app_packet.SOLO_MESSAGE_LOCATION, 20, loc.lat, loc.lon, loc.alt)
self.shotmgr.appMgr.sendPacket(packet)
def spotLock(self):
'''take the angle of the copter and lock onto a ground level target'''
if self.camPointing == SPOT_LOCK:
return
self.needsUpdate = True
# don't use a shallow angle resulting in massively distant ROIs
pitch = min(camera.getPitch(self.vehicle), SHALLOW_ANGLE_THRESHOLD)
# Get ROI for the vehicle to look at
spotLock = location_helpers.getSpotLock(self.vehicle.location.global_relative_frame, pitch, camera.getYaw(self.vehicle))
self.addLocation(spotLock)
def manualGimbalTargeting(self):
'''set gimbal targeting mode to manual'''
msg = self.vehicle.message_factory.mount_configure_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, # mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
self.vehicle.send_mavlink(msg)
# init the open loop gimbal pointing vars
self.camYaw = camera.getYaw(self.vehicle)
self.camPitch = camera.getPitch(self.vehicle)
def manualPitch(self, channels):
if abs(channels[RAW_PADDLE]) > abs(channels[THROTTLE]):
value = channels[RAW_PADDLE]
else:
value = channels[THROTTLE]
self.camPitch += PITCH_SPEED * UPDATE_TIME * value
if self.camPitch > 0.0:
self.camPitch = 0.0
elif self.camPitch < -90:
self.camPitch = -90
def manualYaw(self, channels):
if channels[YAW] == 0:
return
self.camYaw += channels[YAW] * YAW_SPEED * UPDATE_TIME
if self.camYaw > 360:
self.camYaw -= 360
if self.camYaw < 0:
self.camYaw += 360
# required for gimbals w/o Yaw
if channels[YAW] > 0:
self.camDir = 1
else:
self.camDir = -1
def handleFreeLookPointing(self):
'''Handle free look'''
# if we do have a gimbal, use mount_control to set pitch and yaw
if self.vehicle.mount_status[0] is not None:
msg = self.vehicle.message_factory.mount_control_encode(
0, 1, # target system, target component
# pitch is in centidegrees
self.camPitch * 100.0,
0.0, # roll
# yaw is in centidegrees
self.camYaw * 100.0,
0 # save position
)
else:
# if we don't have a gimbal, just set CONDITION_YAW
msg = self.vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command
0, # confirmation
self.camYaw, # param 1 - target angle
YAW_SPEED, # param 2 - yaw speed
self.camDir, # param 3 - direction XXX
0.0, # relative offset
0, 0, 0 # params 5-7 (unused)
)
self.vehicle.send_mavlink(msg)
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)
# moves an offset of the ROI altitude up or down
def updateROIAlt(self, rawPaddleValue):
# no gimbal, no reason to change ROI
if self.vehicle.mount_status[0] == None:
return
if abs(rawPaddleValue) > MIN_PADDLE_THRESHOLD:
self.roi.alt += (rawPaddleValue * FOLLOW_ALT_NUDGE_SPEED * UPDATE_TIME)
self.roi.alt = min(self.vehicle.location.global_relative_frame.alt + ROI_ALT_MARGIN, self.roi.alt)
self.roi.alt = max(self.vehicle.location.global_relative_frame.alt - ROI_ALT_MAX_DELTA, self.roi.alt)