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

shotmanager/.gitignore

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

shotmanager/.travis.yml

@@ -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/

shotmanager/COPYRIGHT-3DR

@@ -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

shotmanager/GeoFenceHelper.py

@@ -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)

shotmanager/GeoFenceManager.py

@@ -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

shotmanager/GoProConstants.py

@@ -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

shotmanager/GoProManager.py

@@ -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))

shotmanager/LICENSE-APACHE

@@ -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.

shotmanager/README.md

@@ -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)

shotmanager/SOURCE

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

shotmanager/Test/TestAppManager.py

@@ -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()

shotmanager/Test/TestButtonManager.py

@@ -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)])
+

shotmanager/Test/TestCableCam.py

@@ -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)
+                                            )

shotmanager/Test/TestCableController.py

@@ -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.)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

shotmanager/Test/TestCamera.py

@@ -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 )

shotmanager/Test/TestCatmullRom.py

@@ -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)

shotmanager/Test/TestFlyController.py

@@ -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)
+

shotmanager/Test/TestFollow.py

@@ -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

shotmanager/Test/TestGoProManager.py

@@ -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()

shotmanager/Test/TestLeashController.py

@@ -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)        
+

shotmanager/Test/TestLocationHelpers.py

@@ -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 )

shotmanager/Test/TestLookAtController.py

@@ -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)        
+
+

shotmanager/Test/TestModes.py

@@ -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 )

shotmanager/Test/TestOrbit.py

@@ -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)    
+
+

shotmanager/Test/TestOrbitController.py

@@ -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)
+
+
+
+

shotmanager/Test/TestPano.py

@@ -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)
+
+        
+        
+

shotmanager/Test/TestPathHandler.py

@@ -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)

shotmanager/Test/TestRCManager.py

@@ -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)
+

shotmanager/Test/TestRewind.py

@@ -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)
+#        
+#

shotmanager/Test/TestRewindManager.py

@@ -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)
+
+

shotmanager/Test/TestSelfie.py

@@ -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)

shotmanager/Test/TestSettings.py

@@ -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)

shotmanager/Test/TestShotManager.py

@@ -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)

shotmanager/Test/TestVectorPathHandler.py

@@ -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)
+

shotmanager/Test/TestYawPitchOffsetter.py

@@ -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)

shotmanager/Test/TestZipline.py

@@ -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)
+        
+        
+        

shotmanager/Test/shotmanager.conf.md5

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

shotmanager/appManager.py

@@ -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

shotmanager/app_packet.py

@@ -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

shotmanager/buttonManager.py

@@ -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
+
+

shotmanager/cableController.py

@@ -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

shotmanager/cable_cam.py

@@ -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

shotmanager/camera.py

@@ -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

shotmanager/catmullRom.py

@@ -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,)

shotmanager/config/shotmanager.orig

@@ -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=

shotmanager/flyController.py

@@ -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
+

shotmanager/follow.py

@@ -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

shotmanager/leashController.py

@@ -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

shotmanager/location_helpers.py

@@ -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

shotmanager/lookAtController.py

@@ -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)
+

shotmanager/main.py

@@ -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()

shotmanager/modes.py

@@ -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

shotmanager/multipoint.py

@@ -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

shotmanager/orbit.py

@@ -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

shotmanager/orbitController.py

@@ -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

shotmanager/pano.py

@@ -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))
+

shotmanager/pathHandler.py

@@ -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

shotmanager/prereqs/.mavinit.scr

@@ -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

shotmanager/prereqs/inittab

@@ -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

shotmanager/rcManager.py

@@ -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)
+

shotmanager/requirements.txt

@@ -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

shotmanager/returnHome.py

@@ -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)
+

shotmanager/rewind.py

@@ -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)

shotmanager/rewindManager.py

@@ -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
+
+

shotmanager/run_shotmanager.sh

@@ -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

shotmanager/selfie.py

@@ -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)

shotmanager/settings.py

@@ -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()
+

shotmanager/setupShots.sh

@@ -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"

shotmanager/shotFactory.py

@@ -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.")

shotmanager/shotLogger.py

@@ -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()

shotmanager/shotManager.py

@@ -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)

shotmanager/shotManagerConstants.py

@@ -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

shotmanager/shots.py

@@ -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

shotmanager/sim/StressPerformance.py

@@ -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()

shotmanager/sim/TestRC.py

@@ -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)

shotmanager/sim/TestSocket.py

@@ -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

shotmanager/sim/__init__.py

@@ -0,0 +1 @@
+import rc_ipc_shim

shotmanager/sim/rc_ipc_shim.py

@@ -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

shotmanager/sim/shotmanager.conf

@@ -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

shotmanager/sim/shotmanager.sandbox.conf

@@ -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

shotmanager/transect.py

@@ -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

shotmanager/vector2.py

@@ -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])

shotmanager/vector3.py

@@ -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])

shotmanager/vectorPathHandler.py

@@ -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)
+

shotmanager/yawPitchOffsetter.py

@@ -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

shotmanager/zipline.py

@@ -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)
+