diff --git a/shotmanager/.gitignore b/shotmanager/.gitignore new file mode 100644 index 0000000..7055e31 --- /dev/null +++ b/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 diff --git a/shotmanager/.travis.yml b/shotmanager/.travis.yml new file mode 100644 index 0000000..dfb79a1 --- /dev/null +++ b/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/OpenSolo/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/ diff --git a/shotmanager/COPYRIGHT-3DR b/shotmanager/COPYRIGHT-3DR new file mode 100644 index 0000000..f67af11 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/shotmanager/GeoFenceHelper.py b/shotmanager/GeoFenceHelper.py new file mode 100644 index 0000000..fda5a04 --- /dev/null +++ b/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) diff --git a/shotmanager/GeoFenceManager.py b/shotmanager/GeoFenceManager.py new file mode 100644 index 0000000..eaac281 --- /dev/null +++ b/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 "" % (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(' 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('= 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 diff --git a/shotmanager/GoProConstants.py b/shotmanager/GoProConstants.py new file mode 100644 index 0000000..cc5f8a5 --- /dev/null +++ b/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 diff --git a/shotmanager/GoProManager.py b/shotmanager/GoProManager.py new file mode 100644 index 0000000..0da2a5a --- /dev/null +++ b/shotmanager/GoProManager.py @@ -0,0 +1,580 @@ +# +# 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) + + if self.captureMode == CAPTURE_MODE_PHOTO: + if command == mavutil.mavlink.GOPRO_COMMAND_SHUTTER: + if value[0] == 1: + self.sendPhotoEvent() + + # 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(' 0) + elif type == app_packet.GOPRO_SET_REQUEST: + (command, value) = struct.unpack(' 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. + + + +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/OpenSolo/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/OpenSolo/SoloLink). +* [mavlink-solo](https://github.com/OpenSolo/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/OpenSolo/ardupilot-solo) to your home directory. +``` +git clone https://github.com/OpenSolo/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/OpenSolo/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) diff --git a/shotmanager/SOURCE b/shotmanager/SOURCE new file mode 100644 index 0000000..093651d --- /dev/null +++ b/shotmanager/SOURCE @@ -0,0 +1 @@ +This source code was released by 3DR in the file: shotmanager_v2.4.1-1_2d69b277.tar.gz diff --git a/shotmanager/Test/TestAppManager.py b/shotmanager/Test/TestAppManager.py new file mode 100644 index 0000000..665375e --- /dev/null +++ b/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(' 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 diff --git a/shotmanager/Test/TestGoProManager.py b/shotmanager/Test/TestGoProManager.py new file mode 100644 index 0000000..28d320b --- /dev/null +++ b/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(' 0''' + + self.shot.cruiseSpeed = 4 + self.shot.handleRCs(self.channels) + self.assertEqual(self.shot.targetP, 1.0) + + def testSetTargetZeroDesiredSpeed(self): + '''Test that targetP is set to 1.0 if desiredSpeed is = 0''' + + self.shot.handleRCs(self.channels) + self.assertEqual(self.shot.targetP, 0.0) + + def testSetTargetNegativeDesiredSpeed(self): + '''Test that targetP is set to 0.0 if desiredSpeed is < 0''' + + self.shot.cruiseSpeed = -4 + self.shot.handleRCs(self.channels) + self.assertEqual(self.shot.targetP, 0.0) + + def testUpdatingCableController(self): + '''Tests that the cableController update functions are called''' + + self.shot.handleRCs(self.channels) + self.shot.cable.setTargetP.assert_called_with(0.0) + self.shot.cable.trackSpeed.assert_called_with(0.0) + self.shot.cable.update.assert_called_with(UPDATE_TIME) + + def testPosVelMessage(self): + '''Test that pos-vel message is formed correctly''' + + 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 + int(37.873168 * 10000000), # latitude (degrees*1.0e7) + int(-122.302062 * 10000000), # longitude (degrees*1.0e7) + 0, # altitude (meters) + 0.0, 0.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.shot.vehicle.send_mavlink.assert_called_with(mock.ANY) + + def testInterpolateCamON(self): + ''' InterpolateCamera should be called if the option is on ''' + + self.shot.camInterpolation = 0 + self.shot.handleRCs(self.channels) + self.shot.interpolateCamera.assert_called_with() + + def testInterpolateCamOFF(self): + ''' InterpolateCamera should NOT be called if the option is off ''' + + self.shot.camInterpolation = 1 + self.shot.handleRCs(self.channels) + assert not self.shot.interpolateCamera.called + + def testCallingYawPitchOffsetterUpdate(self): + '''yawPitchOffsetter.Update() should always be called''' + + self.shot.handleRCs(self.channels) + self.shot.yawPitchOffsetter.Update.assert_called_with(self.channels) + + def testWithGimbal(self): + ''' Test message packaging if we have a gimbal''' + + self.shot.vehicle.mount_status = [0.0, ] + self.shot.handleRCs(self.channels) + self.shot.vehicle.message_factory.mount_control_encode.assert_called_with( + 0, 1, # target system, target component + mock.ANY, # pitch is in centidegrees + 0.0, # roll + mock.ANY, # yaw is in centidegrees + 0 # save position + ) + + def testWithNoGimbal(self): + ''' Test message packaging if we don't have a gimbal''' + + self.shot.vehicle.mount_status = [None, ] + self.shot.handleRCs(self.channels) + 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 + mock.ANY, # param 1 - target angle + mock.ANY, # param 2 - yaw speed + 0, # param 3 - direction, always shortest route for now... + 0.0, # relative offset + 0, 0, 0 # params 5-7 (unused) + ) + + +class TestInterpolateCamera(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 + + #Run the shot constructor + self.shot = multipoint.MultipointShot(vehicle, shotmgr) + + #Mock cableController + self.shot.cable = mock.create_autospec(CableController) + self.shot.cable.currentU = 0.5 # half-way through spline + self.shot.cable.currentSeg = 0 + + # create two waypoints + loc = LocationGlobalRelative(37.873168,-122.302062, 0) + self.shot.waypoints.append(Waypoint(loc,-90,0)) + self.shot.waypoints.append(Waypoint(loc,0,90)) + + self.shot.camSpline = CatmullRom([Vector2(-180, -90), Vector2(-90, 0), Vector2(0, 90), Vector2(90, 180)]) + + def testNewPitchYaw(self): + '''Make sure newYaw is linearly interpolating between two target yaws''' + + newPitch, newYaw = self.shot.interpolateCamera() + self.assertEqual(newYaw, 45) + self.assertEqual(newPitch, -45) + + @mock.patch('location_helpers.wrapTo360') + def testCurrentUGreaterThanOne(self, location_helpers_wrapTo360): + '''Test that if cable.currentU is greater than 1, then it is set to 1''' + + self.shot.cable.currentU = 1.2 + self.shot.camSpline.position = Mock() + self.shot.interpolateCamera() + self.shot.camSpline.position.assert_called_with(0,1.0) + + @mock.patch('location_helpers.wrapTo360') + def testCurrentULessThanZero(self, location_helpers_wrapTo360): + '''Test that if cable.currentU is less than zero, then it is set to 0''' + + self.shot.cable.currentU = -0.1 + self.shot.camSpline.position = Mock() + self.shot.interpolateCamera() + self.shot.camSpline.position.assert_called_with(0,0.0) + +class TestRecordLocation(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.appMgr = Mock() + + #Run the shot constructor + self.shot = multipoint.MultipointShot(vehicle, shotmgr) + + #Mock setButtonMappings() + self.shot.setButtonMappings = Mock() + + self.shot.duplicateCheck = Mock(return_value=False) + + def testRecordInPlayMode(self): + '''Try recording a location when in PLAY mode''' + self.shot.vehicle.location.global_relative_frame = LocationGlobalRelative(37.242124, -122.12841, 15.3) + self.shot.cableCamPlaying = True + self.shot.recordLocation() + assert not self.shot.duplicateCheck.called + + def testRecordingLocations(self): + ''' Test if recording locations ''' + self.shot.vehicle.location.global_relative_frame = LocationGlobalRelative(37.242124, -122.12841, 15.3) + self.shot.recordLocation() + self.shot.vehicle.location.global_relative_frame = LocationGlobalRelative(30.13241, 10.112135, 0.0) + self.shot.recordLocation() + self.shot.vehicle.location.global_relative_frame = LocationGlobalRelative(37.873115, -122.303307, 25.0) + self.shot.recordLocation() + self.assertTrue(len(self.shot.waypoints) == 3) + + @patch('camera.getPitch') + @patch('camera.getYaw') + def testRecordingSameLocations(self, mock_getPitch, mock_getYaw): + ''' Test if recording the same locations ''' + mock_getPitch.return_value = 0 + mock_getYaw.return_value = 0 + self.shot.vehicle.location.global_relative_frame = LocationGlobalRelative(37.242124, -122.12841, 15.3) + self.shot.vehicle.location.global_frame.alt = 0 + self.shot.recordLocation() + mock_getPitch.return_value = 45 + mock_getYaw.return_value = 45 + self.shot.duplicateCheck.return_value = True + self.shot.recordLocation() + self.assertTrue(len(self.shot.waypoints) == 1) + packet = struct.pack(' 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) diff --git a/shotmanager/Test/TestZipline.py b/shotmanager/Test/TestZipline.py new file mode 100644 index 0000000..d28f8a6 --- /dev/null +++ b/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) + + + diff --git a/shotmanager/Test/shotmanager.conf b/shotmanager/Test/shotmanager.conf new file mode 100644 index 0000000..e69de29 diff --git a/shotmanager/Test/shotmanager.conf.md5 b/shotmanager/Test/shotmanager.conf.md5 new file mode 100644 index 0000000..eb8b31d --- /dev/null +++ b/shotmanager/Test/shotmanager.conf.md5 @@ -0,0 +1 @@ +d41d8cd98f00b204e9800998ecf8427e Test/shotmanager.conf diff --git a/shotmanager/appManager.py b/shotmanager/appManager.py new file mode 100644 index 0000000..5b70300 --- /dev/null +++ b/shotmanager/appManager.py @@ -0,0 +1,255 @@ +# +# 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('= 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 + + ''' + Press, DoubleClick, ShortHold, Hold, and Release events are not used any + further in Open Solo. We dump those events here and now so they don't + clog up the rest of the button handling. Most aren't even sent from + Artoo anymore, but just in case someone uses a controller with stock + firmware, we need to dump these events. + ''' + if (event == btn_msg.Press or event == btn_msg.DoubleClick + or event == btn_msg.ShortHold or event == btn_msg.Hold + or event == btn_msg.Release): + return + + logger.log("[button]: Received button %r event %r" % (button, event)) + + '''POWER BUTTON + The power button is handled entirely by the Artoo STM32. It's only + purpose here in shotmanager is logging. + ''' + if button == btn_msg.ButtonPower: + if event == btn_msg.LongHold: + logger.log("[Button]: Controller power button held long.") + return + + '''FLY BUTTON + The fly button directly initiates a mode change to Fly (loiter) from + artoo on a single press. And directly initiates takeoff/land mode from + artoo on a long hold. It's only purpose here in shotmanager is logging + and to cleanly exit any smart shot that happens to be running. + ''' + if button == btn_msg.ButtonFly: + if event == btn_msg.ClickRelease: + logger.log("[Button]: Fly button pressed.") + if self.shotMgr.currentShot != shots.APP_SHOT_NONE: + self.shotMgr.enterShot(shots.APP_SHOT_NONE) + elif event == btn_msg.LongHold: + logger.log("[Button]: Fly button long hold.") + return + + '''HOME BUTTON + The home button is handled entirely by the Artoo STM32. It will directly + initiate a mode change to RTL if GPS available, or land mode is no GPS + available when pressed. Its only purpose here in shotmanager is logging. + ''' + if button == btn_msg.ButtonRTL: + if event == btn_msg.ClickRelease: + logger.log("[Button]: Home button pressed.") + return + + '''PAUSE BUTTON + The pause button is handled in both the Artoo STM32 and here in + shotmanager. It serves multiple purposes. + + On ClickRease: + The STM32 will send mavlink command MAV_CMD_SOLO_BTN_PAUSE_CLICK to + ArduCopter. ArduCopter will handle what to do with it as follows: + - If not in Guided Mode, ArduCopter will brake then go into + fly (loiter) mode. If no GPS, it will go into manual (alt hold). + - If in Guided Mode, ArduCopter will ignore it, assuming + we're in a smart shot. + Here in shot manager, if a smart shot is running, the pause button + single click is sent to the shot for handling. + + On HoldRelease (2 second hold), The Artoo STM32 will set RC CH7 + to low (1000). On LongHold, the Artoo STM32 will set RC CH7 to + high (2000). You can set the CH7 on/off options in a GCS like Mission + Planner, Tower, QGC, etc. + + No other functions should be assigned to the pause button here + or in extFunctions. + ''' + if button == btn_msg.ButtonLoiter: + if event == btn_msg.ClickRelease: + if self.shotMgr.currentShot != shots.APP_SHOT_NONE: + self.shotMgr.curController.handleButton(button, event) + return + + ''' + The upper and lower camera preset ClickRelease & HoldRelease events are + managed completely on Artoo. These button events should not be given any + other usage here because it will hose the Artoo handling. + The HoldRelease and LongHold events will be sent to extFunctions for + further handling. + ''' + if button == btn_msg.ButtonPreset1 or button == btn_msg.ButtonPreset2: + if event == btn_msg.ClickRelease or event == btn_msg.HoldRelease: + return + elif event == btn_msg.HoldRelease or event == btn_msg.LongHold: + self.shotMgr.extFunctions.handleExtButtons(buttonEvent) + return + + ''' A BUTTON & B BUTTON + The ClickRelease events of A & B are managed exclusively here in + shotmanager. These button events will be processed just as they always + were with flight modes or smart shots assigned from the apps. They will + also be sent to any active smart shot for their re-mapped handling. + The HoldRelease and LongHold events will be sent to extFunctions for + further handling. + ''' + if button == btn_msg.ButtonA or button == btn_msg.ButtonB: + if event == btn_msg.ClickRelease: + if self.shotMgr.currentShot == shots.APP_SHOT_NONE: + (shot, mode) = self.getFreeButtonMapping(button) + 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: + logger.log("[button]: Requested a mode change via Artoo button to %s." % (modes.MODE_NAMES[mode])) + self.shotMgr.vehicle.mode = VehicleMode(mavutil.mode_mapping_acm.get(mode)) + else: + self.shotMgr.curController.handleButton(button, event) + logger.log("[button]: A/B Button ClickRelease sent to smart shot.") + elif event == btn_msg.HoldRelease or event == btn_msg.LongHold: + self.shotMgr.extFunctions.handleExtButtons(buttonEvent) + return + + '''CAMERA TRIGGER BUTTON + The camera trigger button is handled entirely here in shotmanager. + The ClickRelease event triggers the camera shutter / record just as + it always has. The HoldRelease and LongHold events will be sent to + extFunctions for further handling. + ''' + if button == btn_msg.ButtonCameraClick: + if event == btn_msg.ClickRelease: + self.shotMgr.goproManager.handleRecordCommand(self.shotMgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE) + elif event == btn_msg.HoldRelease or event == btn_msg.LongHold: + self.shotMgr.extFunctions.handleExtButtons(buttonEvent) + return + return diff --git a/shotmanager/cableController.py b/shotmanager/cableController.py new file mode 100644 index 0000000..d44d629 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/shotmanager/cable_cam.py b/shotmanager/cable_cam.py new file mode 100644 index 0000000..f1f7dd1 --- /dev/null +++ b/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(' 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(' 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(' 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,) \ No newline at end of file diff --git a/shotmanager/config/shotmanager.orig b/shotmanager/config/shotmanager.orig new file mode 100644 index 0000000..7e57ff6 --- /dev/null +++ b/shotmanager/config/shotmanager.orig @@ -0,0 +1,44 @@ +[shotManager] +A=-1, 2 +B=-1, 16 +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= \ No newline at end of file diff --git a/shotmanager/docs/App-ShotManager communication.md b/shotmanager/docs/App-ShotManager communication.md new file mode 100644 index 0000000..0a25e06 --- /dev/null +++ b/shotmanager/docs/App-ShotManager communication.md @@ -0,0 +1,1934 @@ +# App-ShotManager Communication + +This document details App-Sololink communication for use in the Sololink behaviors. + + - [Setup](#setup) + - [Shot Flows](#shot-flows) + - [Multipoint cable cam](#multipoint-cable-cam) + - [Record Mode](#record-mode) + - [Play Mode](#play-mode) + - [Definition of a Path](#definition-of-a-path) + - [Selfie](#selfie) + - [Orbit](#orbit) + - [Follow](#follow) + - [Cable cam (legacy)](#cable-cam-legacy) + - [Button mapping](#button-mapping) + - [Protocol](#protocol) + - [Message definitions](#message-definitions) + - [General messages](#general-messages) + - [SOLO_MESSAGE_GET_CURRENT_SHOT](#solo_message_get_current_shot) + - [SOLO_MESSAGE_SET_CURRENT_SHOT](#solo_message_set_current_shot) + - [SOLO_MESSAGE_LOCATION](#solo_message_location) + - [SOLO_MESSAGE_RECORD_POSITION](#solo_message_record_position) + - [SOLO_CABLE_CAM_OPTIONS](#solo_cable_cam_options) + - [SOLO_GET_BUTTON_SETTING](#solo_get_button_setting) + - [SOLO_SET_BUTTON_SETTING](#solo_set_button_setting) + - [SOLO_PAUSE](#solo_pause) + - [SOLO_FOLLOW_OPTIONS](#solo_follow_options) + - [SOLO_SHOT_OPTIONS](#solo_shot_options) + - [SOLO_SHOT_ERROR](#solo_shot_error) + - [SOLO_MESSAGE_SHOTMANAGER_ERROR](#solo_message_shotmanager_error) + - [SOLO_CABLE_CAM_WAYPOINT](#solo_cable_cam_waypoint) + - [SOLO_SECOND_PHONE_NOTIFICATION](#solo_second_phone_notification) + - [SOLO_ZIPLINE_OPTIONS](#solo_zipline_options) + - [SOLO_PANO_OPTIONS](#solo_pano_options) + - [Multipoint cable cam (SOLO_SPLINE_) messages](#multipoint-cable-cam-solo_spline_-messages) + - [SOLO_SPLINE_RECORD](#solo_spline_record) + - [SOLO_SPLINE_PLAY](#solo_spline_play) + - [SOLO_SPLINE_POINT](#solo_spline_point) + - [SOLO_SPLINE_ATTACH](#solo_spline_attach) + - [SOLO_SPLINE_SEEK](#solo_spline_seek) + - [SOLO_SPLINE_PLAYBACK_STATUS](#solo_spline_playback_status) + - [SOLO_SPLINE_PATH_SETTINGS](#solo_spline_path_settings) + - [SOLO_SPLINE_DURATIONS](#solo_spline_durations) + - [GoPro messages](#gopro-messages) + - [GOPRO_SET_ENABLED](#gopro_set_enabled) + - [GOPRO_SET_REQUEST](#gopro_set_request) + - [GOPRO_RECORD](#gopro_record) + - [GOPRO_STATE](#gopro_state) + - [GOPRO_REQUEST_STATE](#gopro_request_state) + - [GOPRO_SET_EXTENDED_REQUEST](#gopro_set_extended_request) + - [GeoFence messages](#geofence-messages) + - [GEOFENCE_SET_DATA](#geofence_set_data) + - [GEOFENCE_SET_ACK](#geofence_set_ack) + - [GEOFENCE_UPDATE_POLY](#geofence_update_poly) + - [GEOFENCE_CLEAR](#geofence_clear) + - [GEOFENCE_ACTIVATED](#geofence_activated) + - [Site Scan Inspect (SOLO_INSPECT_) messages](#site-scan-inspect_-messages) + - [SOLO_INSPECT_START](#solo_inspect_start) + - [SOLO_INSPECT_SET_WAYPOINT](#solo_inspect_set_waypoint) + - [SOLO_INSPECT_MOVE_GIMBAL](#solo_inspect_move_gimbal) + - [SOLO_INSPECT_MOVE_VEHICLE](#solo_inspect_move_vehicle) + - [Site Scan Scan (SOLO_SCAN_) messages](#site-scan-scan_-messages) + - [SOLO_SCAN_START](#solo_scan_start) + - [Site Scan Survey (SOLO_SURVEY_) messages](#site-scan-survey_-messages) + - [SOLO_SURVEY_START](#solo_survey_start) + + + +## Setup + +*ShotManager* is the process that runs on Sololink that handles behaviors. + +App-ShotManager communication happens over TCP on port 5507. + + + + +## Shot Flows + +### Multipoint cable cam + +The *Multipoint Cable Cam* shot allows the copter to fly a [Path](#definition-of-a-path) described by a spline. + +There are two modes: Record and Play + +* In Record mode, *ShotManager* collects and assembles Keypoints into a Path. +* In Play mode, *ShotManager* controls the copter to fly along the Path defined in Record mode. + +##### Record Mode + +* When *ShotManager* enters Record mode, it clears the existing [Path](definition-of-a-path) (if there is one) and starts a new one. +* *ShotManager* attempts to create a new Keypoint when: + + * It receives a button press from Artoo + * It receives [SOLO_MESSAGE_RECORD_POSITION](#solo_message_record_position) from the app + * It receives [SOLO_SPLINE_POINT](#solo_spline_point) from the app + +* After creating a Keypoint, *ShotManager* sends [SOLO_SPLINE_POINT](#solo_spline_point) to the app. +* It's possible for Keypoint creation to fail. + +* *ShotManager* can coalesce multiple recording requests into a smaller number of Keypoints (i.e. to remove duplicates); however, it must respond with exactly one [SOLO_SPLINE_POINT](#solo_spline_point) message for each [SOLO_SPLINE_POINT](#solo_spline_point) or [SOLO_MESSAGE_RECORD_POSITION](#solo_message_record_position) message it receives from the app. This is to provide an ACK mechanism. + +##### Play Mode + +* *ShotManager* enters Play mode: + + * In response to an Artooo button press. + * When it receives a [SOLO_SPLINE_PLAY](#solo_spline_play) message from the app. + +* *ShotManager* will only enter Play mode after a valid Path has been constructed in Record mode. +* When *ShotManager* enters Play mode, it sends all Keypoints on the current Path to the app using [SOLO_SPLINE_POINT](#solo_spline_point) messages +* The vehicle doesn't fly or snap to the path until it receives a message from the app +* The app sends a [SOLO_SPLINE_ATTACH](#solo_spline_attach) message to tell *ShotManager* to fly to the Path and await further commands. +* Once attached, the app uses the [SOLO_SPLINE_SEEK](#solo_spline_seek) message to tell *ShotManager* to fly the path. +* As the vehicle is flying, *ShotManager* sends [SOLO_SPLINE_PLAYBACK_STATUS](#solo_spline_playback_status) messages to the allow the app to visualize Solo’s location on the flight path. + +##### Definition of a Path + +* A Path is an ordered collection of Keypoints. +* Path indices are unique. +* Valid Keypoints are constructed only by *ShotManager*; the validity of Keypoints must be confirmed by *ShotManager* if created or re-loaded by the app. + +* A Path is valid if and only if: + * It contains at least two Keypoints. + * All its Keypoints are valid. + * The indices of its Keypoints form a complete set; that is, a path with *n* Keypoints contains a Keypoint with each of the indices *0..n-1*. + +* Index 0 is the beginning of the Path. Index *n* is the end of the Path. +* When Path positions are expressed parametrically, index 0 corresponds to a parametric value of 0.0; index *n* corresponds to a parametric value of 1.0. + +Path Equivalence -- **TBD** + +Camera Direction -- **TBD** + +The direction in which the camera points with respect to the Path is *ShotManager* implementation-specific. Eventually, this API will provide options to control this; it is currently undefined. + + +### Selfie + +Selfie is much simpler than cable cam. + +The flow must be initiated by the app via a [SOLO_MESSAGE_SET_CURRENT_SHOT](#solo_message_set_current_shot). *ShotManager* then expects 3 locations sent using [SOLO_MESSAGE_LOCATION](#solo_message_location). They are: + +* 1st selfie waypoint (near point) +* 2nd selfie waypoint (far point) +* Selfie ROI point + +Upon receiving these 3 points, *ShotManager* will put Solo into guided mode and the selfie will automatically start. It’s controllable and pausable just like cable cam though. + +Locations cannot be changed after they are received. Instead, stop and start a new selfie. + +User can press ‘FLY’ to exit as in cable cam. + + + +### Orbit + +Either the app can request that orbit starts (via [SOLO_MESSAGE_SET_CURRENT_SHOT](#solo_message_set_current_shot)) or the user can initiate from Artoo, in which case *ShotManager* will tell the app via a [SOLO_MESSAGE_GET_CURRENT_SHOT](#solo_message_get_current_shot). This only works if an app is connected to *ShotManager*. + +To proceed, the user needs to lock onto a spot. This can be done in 2 ways: + +* Press ‘A’ on Artoo. Orbit will record its current ROI. +* Drag/move the map until the orbit point on the map aligns with the desired orbit ROI,and then select either the app banner or ‘A’ on Artoo. This will send a [SOLO_MESSAGE_LOCATION](#solo_message_location) to *ShotManager*, which will be the ROI. + +In any case, if an initial ROI is set for Orbit, it will send this location back to the app via a [SOLO_MESSAGE_LOCATION](#solo_message_location) message. + +At any point, if the user hits ‘FLY’, it will exit orbit and *ShotManager* will send a [SOLO_MESSAGE_GET_CURRENT_SHOT](#solo_message_get_current_shot) with -1 to the app. + +The app can send [SOLO_SHOT_OPTIONS](#solo_shot_options) to *ShotManager* to change cruise speed. If the user hits the "Pause" button on Artoo, *ShotManager* will adjust cruiseSpeed to 0.0 and send [SOLO_SHOT_OPTIONS](#solo_shot_options) to the app. Hitting “pause” again will set cruiseSpeed to the original speed and send it to the app. + +When an initial ROI is set, *ShotManager* will set Solo to Guided and the copter will be in orbit mode. + +During orbit mode, the app can send a [SOLO_MESSAGE_LOCATION](#solo_message_location) to update the ROI location and begin a new orbit around the new ROI. Note this differs from Follow mode, which sends new points but does not reset the orbit. + +Prior to the ROI being set, the app should show on the app the projected ROI of orbit. This is not passed from *ShotManager* to the app, so the app should calculate it. This calculation works as follows: + +```python +if camera.getPitch(self.vehicle) > SHALLOW_ANGLE_THRESHOLD (-60): + loc = location_helpers.newLocationFromAzimuthAndDistance(self.vehicle.location, camera.getYaw(self.vehicle), FAILED_ROI_DISTANCE (20.0)) +else: + loc = roi.CalcCurrentROI(self.vehicle) +``` + +Buttons for cruising work as they do in cable cam. + + +### Follow + +Follow is a special case of orbit. It works the same way except instead of setting or locking onto an roi, it uses the phone’s GPS. In order to do this, it needs to connect to a udp port on *ShotManager*. + +First the app should tell *ShotManager* to enter the Follow shot via [SOLO_MESSAGE_SET_CURRENT_SHOT](#solo_message_set_current_shot). Follow is not enterable via Artoo. + +Then the app should begin to stream positions in [SOLO_MESSAGE_LOCATION](#solo_message_location) packets to *ShotManager* on port 14558. It should stream locations at 25 Hz, because that is the update loop rate of *ShotManager*. + +Buttons for cruising work as they do in cable cam. + +### Cable cam (legacy) + +Either the app can request that cable cam starts (via [SOLO_MESSAGE_SET_CURRENT_SHOT](#solo_message_set_current_shot)) or the user can initiate from Artoo, in which case *ShotManager* will tell the app via a [SOLO_MESSAGE_GET_CURRENT_SHOT](#solo_message_get_current_shot). This only works if an app is connected to *ShotManager*. + +To proceed, the user needs to record two locations. + +The app can tell *ShotManager* to record a point using [SOLO_MESSAGE_RECORD_POSITION](#solo_message_record_position), but it should always wait to receive a [SOLO_CABLE_CAM_WAYPOINT](#solo_cable_cam_waypoint) before proceeding. + +Otherwise, a user can Press ‘A’ to record a point on Artoo. + +If a user tries to record two points on top of each other, the second one overwrites the first. To inform the app, *ShotManager* will send a [SOLO_MESSAGE_GET_CURRENT_SHOT](#solo_message_get_current_shot) with -1 as a shot which should tell the app to exit cable cam, then a [SOLO_MESSAGE_GET_CURRENT_SHOT](#solo_message_get_current_shot) with 2 to reenter cable cam, and then a [SOLO_CABLE_CAM_WAYPOINT](#solo_cable_cam_waypoint) to reflect the new starting point of the cable. + +At any point, if the user hits ‘FLY’, it will exit cable cam and *ShotManager* will send a [SOLO_MESSAGE_GET_CURRENT_SHOT](#solo_message_get_current_shot) with -1 to the app. + +After the second point is recorded, *ShotManager* will send a [SOLO_CABLE_CAM_OPTIONS](#solo_cable_cam_options) to the app so the app can retrieve the memorized yaw direction. The app can send [SOLO_CABLE_CAM_OPTIONS](#solo_cable_cam_options) to *ShotManager* to change any of the options. If the user hits the **Pause** button on Artoo, *ShotManager* will adjust cruiseSpeed to 0.0 and send [SOLO_CABLE_CAM_OPTIONS](#solo_cable_cam_options) to the app. Hitting “pause” again will set `cruiseSpeed` to the original speed and send it to the app. Two camera pointing modes are supported: interpolated camera ('interpolate cam') and user-controller camera ('free cam'). The 'camInterpolate' value in [SOLO_CABLE_CAM_OPTIONS](#solo_cable_cam_options) is a boolean that switches between these modes. + +When the second point is recorded, *ShotManager* will set Solo to Guided and the copter will be on the cable. + +### Zip Line + +Zip Line is an infinitly long cable generated from Yaw/Pitch of the camera and initial location of the vehicle. Once created the pilot and fly the cable with the right stick and aim the camera with the left stick. + +Pressing A will allow the pilot to create a new Zip Line based on the camera pose. Pressing B will toggle from the default Free Look camera to a spot lock camera. The ROI will be sent to the app as a location. + + +The [SOLO_ZIPLINE_OPTIONS](#solo_zipline_options) packet is sent to control the speed and the option to take the camera pitch into account when creating Zip Lines. + +### Pano + +Pano is constructed as 3 seperate modes - Video, Cylindical and Spherical. + +The Pano shot starts in a Setup state. Once the user chooses their point of view and options, the begin the shot. Once the shot is finished it exits to Fly or in the case of the Video Pan option, it will run indefinitely. + +The [SOLO_PANO_OPTIONS](#solo_pano_options) packet contains the desired type of Pano, the state (0 = setup, 1 = run), the Field of View in degrees for the cylinder shot, and the speed of the Video Pan for Pause/Resume functionality. + + +### Button mapping + +The button mappings for ‘A’ and ‘B’ are stored on Solo. The app should poll these settings using [SOLO_GET_BUTTON_SETTING](#solo_get_button_setting), upon which *ShotManager* will send the current setting back. + +If a user changes the setting, the app can set it with [SOLO_SET_BUTTON_SETTING](#solo_set_button_setting). + +We only use "Press" events at the moment. + + +## Protocol + +All communication follows this TLV format (little endian). + +```cpp +Packet +{ + messageType : UInt32 + messageLength: UInt32 + messageValue : +} +``` + + +## Message definitions + + +### General messages + +We only have a few message types at the moment. + +#### SOLO_MESSAGE_GET_CURRENT_SHOT + +* **Sent by:** *ShotManager* to App. +* **Valid:** ??? + +Sent from Solo to app when it enters a shot. + +**SL version:** 1.0 + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt320
messageLengthUInt324
messageValue???Index of shot (SELFIE = 0, ORBIT=1, CABLECAM=2, FOLLOW=5)
+ + +#### SOLO_MESSAGE_SET_CURRENT_SHOT + +* **Sent by:** App to *ShotManager*. +* **Valid:** ??? + +Sent from app to Solo to request that Solo begin a shot. + +**SL version:** 1.0 + + +
+ + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt321
messageLengthUInt324
messageValue???Index of shot (SELFIE = 0, ORBIT=1, CABLECAM=2, FOLLOW=5, MPCABLECAM=6)
+ + +#### SOLO_MESSAGE_LOCATION + +* **Sent by:** App to *ShotManager*. +* **Valid:** ??? + +Sent from app to Solo to communicate a location. + +**SL version:** 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt322
messageLengthUInt3220
latitudeDouble
longitudeDouble
altitudeFloatAltitude in meters.
+ + +#### SOLO_MESSAGE_RECORD_POSITION + +* **Sent by:** App to *ShotManager*. +* **Valid:** ??? + +Sent from app to Solo to request recording of a position. + +**SL version:** 1.0 + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt323
messageLengthUInt320
+ + +#### SOLO_CABLE_CAM_OPTIONS + +* **Sent by:** Bidirectional. +* **Valid:** Both App and *ShotManager* can send anytime after a legacy cable has been setup. + +Sent from app to Solo to set [Cable cam (legacy)](#cable-cam-legacy) options. Sent from Solo to app to update cruise speed. + +**SL version:** 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt324
messageLengthUInt328
camInterpolationShort1 - On, 0 - Off
yawDirectionShort1 - Clockwise, 0 - Counterclockwise
cruiseSpeedFloatCruise speed in meters/second.
+ + +#### SOLO_GET_BUTTON_SETTING + +* **Sent by:** Bidirectional. +* **Valid:** ??? + +Sent from app to Solo to request [Button mapping](#button-mapping) setting. Sent from Solo to app as a response. + +**SL version:** 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt325
messageLengthUInt3216
buttonInt32ButtonPower = 0 +ButtonFly = 1 +ButtonRTL = 2 +ButtonLoiter = 3 +ButtonA = 4 +ButtonB = 5 +ButtonPreset1 = 6 +ButtonPreset2 = 7 +ButtonCameraClick = 8 + +
eventInt32Press = 0 +Release = 1 +ClickRelease = 2 +Hold = 3 +LongHold = 4 +DoubleClick = 5
shotInt32shot index, -1 if none. One of shot/mode should be -1, and the other should have a value
modeInt32APM mode index, -1 if none
+ + +#### SOLO_SET_BUTTON_SETTING + +* **Sent by:** App to *ShotManager*. +* **Valid:** ??? + +Sent from app to Solo to set a [Button mapping](#button-mapping) setting. + +**SL version:** 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt326
messageLengthUInt3216
buttonInt32ButtonPower = 0 +ButtonFly = 1 +ButtonRTL = 2 +ButtonLoiter = 3 +ButtonA = 4 +ButtonB = 5 +ButtonPreset1 = 6 +ButtonPreset2 = 7 +ButtonCameraClick = 8 +
eventInt32Press = 0 +Release = 1 +ClickRelease = 2 +Hold = 3 +LongHold = 4 +DoubleClick = 5
shotInt32shot index, -1 if none. One of shot/mode should be -1, and the other should have a value
modeInt32APM mode index, -1 if none
+ + +#### SOLO_PAUSE + +* **Sent by:** *ShotManager* <-> App. +* **Valid:** Valid only in a shot. + +Used by *ShotManager* or app to transmit a pause request. + + +**SL version:** 2.4.0+ + + +| Field | Type | Value/Description +---------------- | ---- | ------------ +messageType | UInt32| 7 +messageLength | UInt32| 8 + + +#### SOLO_FOLLOW_OPTIONS + +* **Sent by:** Bidirectional. +* **Valid:** ??? + +Sent from app to Solo or vice versa to transmit follow options. + +**SL version:** 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt3219
messageLengthUInt328
cruiseSpeedFloatCruise speed in meters/second.
inLookAtModeint1 - yes, 0 - no
+ + +**SL version:** 2.x.x ??? + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt32119
messageLengthUInt3212
cruiseSpeedFloatCruise speed in meters / second.
inLookAtModeint1 - yes, 0 - no.
freeLookModeint1 - yes, 0 - no
+ + +#### SOLO_SHOT_OPTIONS + +* **Sent by:** ??? +* **Valid:** ??? + +Sent from app to Solo or vice versa to transmit selfie options. + +These messages only go from *ShotManager* -> App. + +**SL version:** 1.0 + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt3220
messageLengthUInt324
cruiseSpeedFloatCruise speed in meters/second.
+ + + + +#### SOLO_SHOT_ERROR + +* **Sent by:** *ShotManager* -> App. +* **Valid:** ??? + +Sent from Solo to app when entry into a shot was attempted but rejected due to poor EKF, being unarmed, or RTL. + +**SL version:** 1.0 + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt3221
messageLengthUInt324
errorTypeInt32BAD_EKF = 0, UNARMED = 1, RTL = 2
+ + +#### SOLO_MESSAGE_SHOTMANAGER_ERROR + +* **Sent by:** *ShotManager* -> App. +* **Valid:** ??? + +Debugging tool - *ShotManager* sends this to the app when it has hit an exception. + +**SL version:** 1.0 + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt321000
messageLengthUInt32N (length of exceptStr)
exceptStrException info and stacktrace
+ + +#### SOLO_CABLE_CAM_WAYPOINT + +* **Sent by:** *ShotManager* -> App. +* **Valid:** ??? + +Send the app our cable cam waypoint when it’s recorded. + +**SL version:** 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt321001
messageLengthUInt3228
latitudeDouble
longitudeDouble
altitudeFloatAltitude in metres
degreesYawFloatYaw in degrees.
pitchFloatCamera pitch in degrees.
+ + +#### SOLO_SECOND_PHONE_NOTIFICATION + +* **Sent by:** *ShotManager* -> App. +* **Valid:** ??? + +*ShotManager* sends this to the app when the app is not the first to connect to *ShotManager* (it’s connection will be closed). + +**SL version:** ??? + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt321002
messageLengthUInt320
+ + +#### SOLO_ZIPLINE_OPTIONS + +* **Sent by:** Bidirectional. +* **Valid:** ??? + +Sent from app to Solo or vice versa to transmit zip line options. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt32119
messageLengthUInt326
cruiseSpeedFloatCruise speed in meters / second.
is3DUInt81 - yes, 0 - no.
camPointingUInt81 -Spot Lock, 0 - Free Look
+ + + +#### SOLO_PANO_OPTIONS + +* **Sent by:** Bidirectional. +* **Valid:** ??? + +Sent from app to Solo or vice versa to transmit Pano options. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt32119
messageLengthUInt328
panoTypeUInt81 - yes, 0 - no.
run stateUInt81 - run, 0 - stop, reset
cylinder_fovInt1691 to 360
is3DUInt81 - yes, 0 - no.
degSecondYawFloat-60 to 60 (deg/sec)
+ + + +### Multipoint cable cam (SOLO_SPLINE_) messages + + +#### SOLO_SPLINE_RECORD + +* **Sent by:** App -> *ShotManager*. +* **Valid:** ??? + +Tells *ShotManager* to enter [Record](#record-mode) mode and clear the current [Path](#definition-of-a-path). + +**SL version:** ??? + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt3250
messageLengthUInt320
+ + + + +#### SOLO_SPLINE_PLAY + +* **Sent by:** Bidirectional. +* **Valid:** ??? + +Bidirectional message: sent by the app to tell *ShotManager* to enter [Play mode](#play-mode); sent by *ShotManager* to the app after entering Play mode in response to an Artoo button press. + +In both cases, *ShotManager* follows this message with a sequence of [SOLO_SPLINE_POINT](#solo_spline_point) messages to transmit the current Path to the app. + +*ShotManager* will only enter [Play mode](#play-mode) if a valid [Path](##definition-of-a-path) exists in [Record mode](#record-mode); otherwise, the behavior is undefined. This implies that the app must only send this message when it knows a valid Path exists. + +**SL version:** ??? + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt3251
messageLengthUInt320
+ + + +#### SOLO_SPLINE_POINT + +* **Sent by:** Bidirectional. +* **Valid:** ??? + +Transmits proposed or actual Keypoints on the current camera [Path](#definition-of-a-path). + +*ShotManager* uses this message for two things: + +1. To transmit Keypoints it creates in response to Artoo button presses or [SOLO_MESSAGE_RECORD_POSITION](#solo_message_record_position) messages. +1. To confirm the validity of Keypoints it receives from the app; to ACK those Keypoints. + +When *ShotManager* creates a Keypoint, it assigns an index. When the app receives this message and a Keypoint with the index already exists, it updates the Keypoint to the values in this message; it replaces the existing Keypoint. + +The app uses this message to load Keypoints from previously recorded, known valid Paths into *ShotManager*. + +In every case, *ShotManager* sends a [SOLO_SPLINE_POINT](#solo_spline_point) message back to the app to confirm it was able to create the Keypoint. If it can't create the Keypoint, it sends a failure status. + + +**SL version:** ??? + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt3252
messageLengthUInt3244
versionUInt160
absAltReferenceFloatAbsolute altitude of home location when cable was recorded, in meters.
indexInt32starting at 0
latitudeDoubleLatitude (decimal degrees).
longitudeDoubleLongitude (decimal degrees).
altitudeFloatRelative altitude in metres.
pitchFloatPitch (degrees).
yawFloatYaw (degrees)
uPositionFloatThe parametric offset of the Keypoint along the Path. In Record mode, these values have no meaning since they can't be assigned until the Path is complete. In Play mode, *ShotManager* assigns these values and sends messages to the app. The app never creates these values.
statusInt16ShotManager sends this value to indicate success or failure when creating a Keypoint. Negative values are failure; 0 or positive is success. + +

Error codes:

+
    +
  • -1 : mode error (tried setting a spline point when we were already in PLAY mode).
    • +
  • -2 : keypoint too close to a previous keypoint
    • +
  • -3 : duplicate index error (received multiple Keypoints for a single index)
    • +
  • -4..-MAXINT16 : unspecified failure
    • +
+
+ + + + +#### SOLO_SPLINE_ATTACH + +* **Sent by:** Bidirectional. +* **Valid:** Only after Path is loaded. + +This message directs *ShotManager* to fly to a Keypoint on the Path. After the vehicle reaches this point, *ShotManager* responds with a [SOLO_SPLINE_ATTACH](#solo_spline_attach) to indicate that it has reached the Path and is prepared to receive [SOLO_SPLINE_SEEK](#solo_spline_seek) messages. + +When *ShotManager* enters playback mode, the vehicle may or may not be positioned on the path -- from the app’s point of view, the actual position and velocity of the vehicle with respect to the Path are unknown. + +How *ShotManager* directs the vehicle to the Path is implementation specific and not defined. + +This message is only valid **once** after a Path is loaded. There is no corresponding "detach" message -- the vehicle stays attached until playback mode is exited. + + +**SL version:** ??? + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt3257
messageLengthUInt324
keypointIndexInt32The index of the Keypoint on the currently defined Path to which *ShotManager* will attach (or did attach, for ShotManager to App packets).
+ + + + +#### SOLO_SPLINE_SEEK + +* **Sent by:** App -> *ShotManager*. +* **Valid:** Valid only in [Play mode](#play-mode) when the vehicle is attached to the Path. Ignored at other times. + +This message tells *ShotManager* to fly the vehicle to a position along the normalized length of the Path. The cruiseState value indicates pause/play state of the vehicle. This does *not* overwrite the stored cruise speed set by [SOLO_SPLINE_PATH_SETTINGS](#solo_spline_path_settings). + +**SL version:** ??? + + + + + + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt3253
messageLengthUInt328
uPositionFloatA parametric offset along the Path normalized to (0,1).
cruiseStateInt32Used by the app to determine the state of the cruise play/pause buttons.
    +
  • -1: Cruising to the start of the cable(negative cruise speed).
    • +
  • 0 : Not moving/paused (cruise speed == 0). (DEFAULT)
    • +
  • 1 : Cruising to the end of the cable (positive cruise speed).
    • +
+ + + + +#### SOLO_SPLINE_PLAYBACK_STATUS + +* **Sent by:** *ShotManager* to App. +* **Valid:** Valid only in [Play mode](#play-mode). + +*ShotManager* sends this message to the app to indicate the vehicle's parametric position and cruise state along the Path. The source, precision, and accuracy of these values is solely determined by *ShotManager*. They are for visualization in the app, but not control; they are intended to convey the information, "the vehicle was at this point, trying to move at this cruise speed when the message was sent" with as much certainty as practically possible. But there are no guarantees. + +The frequency at which *ShotManager* sends these messages is currently 10Hz. At a minimum, in playback mode *ShotManager* will send this message: + +* When the vehicle attaches to the path in response to the first [SOLO_SPLINE_SEEK](#solo_spline_seek) message received after entering Play mode. +* Each time the vehicle passes a Keypoint, including the beginning and end of the path +* When the vehicle starts or stops moving. +* When the vehicle’s parametric acceleration exceeds a threshold value: + +The threshold is implementation-specific, exact value TBD (ideally, it should be "tuneable"). + + +**SL version:** ??? + + + + + + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt3254
messageLengthUInt328
uPositionFloatA parametric offset along the Path normalized to (0,1).
cruiseStateInt32Used by the app to determine the state of the cruise play/pause buttons.
    +
  • -1: Cruising to the start of the cable (negative cruise speed).
    • +
  • 0 : Not moving/paused (cruise speed == 0). (DEFAULT)
    • +
  • 1 : Cruising to the end of the cable (positive cruise speed).
    • +
+ + +#### SOLO_SPLINE_PATH_SETTINGS + +* **Sent by:** App -> *ShotManager*. Optional. +* **Valid:** Valid only in [Play mode](#play-mode). + +The app sends this message to configure various aspects of how *ShotManager* interprets the current Path. This message is optional -- if the app never sends it, *ShotManager* will use the assumed default values listed below. Values stay in effect until the Path is reset by a [SOLO_SPLINE_RECORD](#solo_spline_record) message. + +**SL version:** ??? + + + + + + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt3255
messageLengthUInt328
cameraControlInt32
    +
  • 0 : *ShotManager* controls camera interpolation; automatically points camera
    • +
  • 1 : No camera interpolation - camera is controlled with Artoo only.
    • +(DEFAULT 0)
desiredTimeFloatThe app-requested total cable completion time, in seconds.
+ + + +#### SOLO_SPLINE_DURATIONS + +* **Sent by:** *ShotManager* -> App. +* **Valid:** Valid only in [Play mode](#play-mode). + +Used by *ShotManager* to transmit time information about the currently defined Path. It is sent **at least once** after *ShotManager* enters play mode to define the time bounds for the Path. + +Optionally, *ShotManager* can also resend the information any time previous values become invalid. For example, high winds or a low battery might mean that the previously reported maxUVelocity isn’t realistic; *ShotManager* would re-send the message to advise the app to change its estimates. + + +**SL version:** ??? + + +| Field | Type | Value/Description +---------------- | ---- | ------------ +messageType | UInt32| 56 +messageLength | UInt32| 8 +minTime | Float | The estimated time (in seconds) it will take to fly the entire path at maximum speed. +maxTime | Float | The estimated time (in seconds) it will take to fly the entire path at minimum speed. + + + + +### GoPro messages + +#### GOPRO_SET_ENABLED + +* **Sent by:** ??? +* **Valid:** ??? + +Enable/disable GoPro communications altogether. This can be useful in dealing with a camera model that is incapable of handling communications. This value will be stored by *ShotManager* and persist between boots. + +**SL version:** ??? + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt325000
messageLengthUInt324
enabledUInt320 off, 1 on
+ + +#### GOPRO_SET_REQUEST + +* **Sent by:** App -> *ShotManager* +* **Valid:** ??? + +Wrapper for the MAVLink version. See [here](https://docs.google.com/document/d/1CcYOCZRw9C4sIQu4xDXjPMkxZYROmTLB0EtpZamnq74/edit#) for reference on this message. The app will send this to *ShotManager* via TCP, so that *ShotManager* can handle this without using the lossy MAVLink connection. This is a legacy request that will only be able to set the first byte of the request payload. + +**SL version:** 1.1.12 + + + + + + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt325001
messageLengthUInt324
commandunsigned short
valueunsigned short
+ + +#### GOPRO_RECORD + +* **Sent by:** ??? +* **Valid:** ??? + +Higher level call, allowing the app/Artoo to issue a record command in either video or stills mode. *ShotManager* will handle issuing the command + +**SL version:** 1.1.12 + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt325003
messageLengthUInt324
on/offUInt320: Stop recording +1: Start recording +2: Toggle recording
+ + +#### GOPRO_STATE + +* **Sent by:** *ShotManager* -> App +* **Valid:** ??? + +Attempt to encapsulate all state that an app might want to know about the GoPro in one packet. This is automatically sent from *ShotManager* to the app any time a change in its contents occurs. Otherwise the app can explicitly request it to be sent by sending *ShotManager* a [GOPRO_REQUEST_STATE](#gopro_request_state) packet (see below). + +Because of a bug in earlier versions of the iOS app, the new *ShotManager* will actually send two packets: a V1 packet with the original field contents and a new V2 packet with the extended GoPro state. + +**SL version:** 1.1.12 (Incomplete handling in SL version 1.1.12) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt325005: V1 spec +5006: V2 spec
messageLengthUInt3236
VersionUInt8Which version of the spec? + +Version 1 only supplies data through capture mode. Version 2 adds additional fields.
ModelUInt8Model of the camera. Currently not supported.
StatusUInt8Status: +0: STATUS_NO_GOPRO +1: STATUS_INCOMPATIBLE_GOPRO +2: STATUS_GOPRO_CONNECTED +3: STATUS_ERROR
RecordingUInt80: not recording +1: recording
Capture ModeUInt8CAPTURE_MODE_VIDEO = 0 +CAPTURE_MODE_PHOTO = 1 +CAPTURE_MODE_BURST = 2 (Hero3+ only) +CAPTURE_MODE_TIMELAPSE = 3 +CAPTURE_MODE_MULTISHOT = 4 (Hero4 only)
NTSC/PALUInt80: NTSC +1: PAL
Video ResolutionUInt8
Video FPSUInt8
Video FOVUInt8
Video Low LightUInt80: Off +1: On +requires compatible video settings
Photo ResolutionUInt8
Photo Burst RateUInt8
Video ProtuneUInt80: Off +1: On +requires compatible video settings
Video White BalanceUInt8Hero3 only. +Requires protune on.
Video ColorUInt8Hero3 only. +Requires protune on.
Video GainUInt8Hero3 only. +Requires protune on.
Video SharpnessUInt8Hero3 Only +requires protune on
Video ExposureUInt8Requires protune on. + +Hero4 range only from -2.0 to 2.0. Hero3+ from -5.0 to 5.0. We only care about -2.0 to 2.0
Gimbal EnabledUInt80: Off +1: On
Extra1UInt8Reserved for future use.
Extra2UInt8Reserved for future use.
Extra3UInt8Reserved for future use.
Extra4UInt8Reserved for future use.
Extra5UInt8Reserved for future use.
Extra6UInt8Reserved for future use.
Extra7UInt8Reserved for future use.
Extra1UInt16Reserved for future use.
Extra2UInt16Reserved for future use.
Extra3UInt16Reserved for future use.
Extra4UInt16Reserved for future use.
Extra5UInt16Reserved for future use.
+ + +#### GOPRO_REQUEST_STATE + +* **Sent by:** App -> *ShotManager* +* **Valid:** ??? + +Requests that *ShotManager* send the app a [GOPRO_STATE](#gopro-state) packet + +**SL version:** 1.2.0 + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt325007
messageLengthUInt320
+ + +#### GOPRO_SET_EXTENDED_REQUEST + +* **Sent by:** App -> *ShotManager* +* **Valid:** ??? + +Wrapper for the MAVLink version. See [here](https://docs.google.com/document/d/1CcYOCZRw9C4sIQu4xDXjPMkxZYROmTLB0EtpZamnq74/edit#) for reference on this message. The app will send this to *ShotManager* via TCP, so that *ShotManager* can handle this without using the lossy MAVLink connection. This differs from the [GOPRO_SET_REQUEST](#gopro_set_request) in that all 4 payload bytes can be set. + +**SL version:** 1.3 + + + + + + + + + + + + + + + + + + + + + + + + + + + +
FieldTypeValue/Description
messageTypeUInt325009
messageLengthUInt326
commandUInt16
valueUInt8[4]
+ +### GeoFence messages + +#### GEOFENCE_SET_DATA + +* **Sent by:** App -> *ShotManager*. +* **Valid:** Always + +Sent from app to *ShotManager* to set geofence data. The data is encapsulated in a JSON blob. There are three keys in the JSON Blob, each of them is an array of polygon's information. They must be of equal sizes. *ShotManager* will validate this packet and respond with *GEOFENCE_SET_ACK* with either a positive ack (good data) or negative ack (bad data). + +**version:** v2.4.0+ + + +| Field | Type | Value/Description +---------------- | ---- | ------------ +messageType | UInt32| 3000 +messageLength | UInt32| length of the following JSON blob +JSON blob | N/A | JSON blob payload + +JSON Blob: + +| Field | Type | Value/Description +---------------- | ---- | ------------ +coord | JSON Array | ordered list of polygon, which is in turn an ordered list of (lat, lon), counter-clockwise vertices of genfence polygon +subCoord | JSON Array | ordered list of polygon, which is in turn an ordered list of (lat, lon), counter-clockwise vertices of geofence sub polygon +type | JSON Array | ordered list of polygon types, 1 for exclusive geofence and 0 for inclusive geofence + + +Example: +```python +data = { + 'coord': [ + [ + [37.873391778802457, -122.30124440324217], + [37.873425469262187, -122.30088420379124], + [37.87289578293214, -122.30071240112851], + [37.872942621197538, -122.30124440324217] + ] + ], + 'subCoord': [ + [ + [37.873405777181389, -122.30090131123818], + [37.872916521332435, -122.30074261865762], + [37.872958695063687, -122.3012216429134], + [37.873375815652267, -122.3012216429134] + ] + ], + 'type': [ + 0 + ] + } +``` + + +#### GEOFENCE_SET_ACK + +* **Sent by:** App <- *ShotManager*. +* **Valid:** After *GEOFENCE_SET_DATA*, *GEOFENCE_UPDATE_POLY* or *GEOFENCE_CLEAR* is sent from App to *ShotManager* + +Acknowledge from *ShotManager* to the App after message from the App. Can either be a positive ack (*ShotManager* took the change) or negative ack (*ShotManger* rejected the change). + +**version:** v2.4.0+ + + +| Field | Type | Value/Description +---------------- | ---- | ------------ +messageType | UInt32| 3001 +messageLength | UInt32| 3 +count | UInt16| The total number of polygons existing on *ShotManger* right now) +valid | Bool | True: Positive ack / False: Negative ack + + +#### GEOFENCE_UPDATE_POLY + +* **Sent by:** App -> *ShotManager*. +* **Valid:** When *ShotManager* already has polygon data + +Update a polygon, either: 1. Update a vertex; 2. add a new vertex; 3. delete a vertex. The course of action is determined by the first byte after messaegLength. +__This message is currently not used and is subject to change.__ + +**version:** v2.4.0+ + + +| Field | Type | Value/Description +---------------- | ---- | ------------ +messageType | UInt32| 3002 +messageLength | UInt32| 37 +action | UInt8 | action to be performed, see below +polygonIndex | UInt16| Index of target polygon +vertexIndex | UInt16| Index of target vertex +lat | Double| latitude of new coordinate on polygon +lon | Double| longitude of new coordinate on polygon +subLat | Double| latitude of new coordinate on subpolygon +subLon | Double| longitude of new coordinate on subpolygon + +Action: + +| Value | Description +------- | ----------- +0 | Update Vertex +1 | Add Vertex +2 | Remove Vertex, the following four Double will be ignored in this case + + +#### GEOFENCE_CLEAR + +* **Sent by:** App <-> *ShotManager*. +* **Valid:** Always + +Either the App or *ShotManager* can send this message to indicate GeoFence has been cleared on their sides. When this message is sent from App to *ShotManager*, *ShotManager* will also send a *GEOFENCE_SET_ACK*. + +**version:** v2.4.0+ + + +| Field | Type | Value/Description +---------------- | ---- | ------------ +messageType | UInt32| 3003 +messageLength | UInt32| 0 + + +#### GEOFENCE_ACTIVATED + +* **Sent by:** App <- *ShotManager*. +* **Valid:** Always + +*ShotManager* will send this message to the App every time GeoFence is activating. + +**version:** v2.4.0+ + + +| Field | Type | Value/Description +---------------- | ---- | ------------ +messageType | UInt32| 3004 +messageLength | UInt32| 0 + + +### Site Scan Inspect (SOLO_INSPECT_) messages + +#### SOLO_INSPECT_START + +* **Sent by:** App -> *ShotManager*. +* **Valid:** Only once during an Inspect shot. + +Sent by the app to *ShotManager* to notify that the inspect shot is ready to commence and the vehicle should climb to the provided **takeoffAlt**. Shotmanager will either takeoff or place the vehicle into *GUIDED* depending on whether the vehicle is already in the air or not. + +**version:** v2.2.0+ + + +| Field | Type | Value/Description +---------------- | ---- | ------------ +messageType | UInt32| 10001 +messageLength | UInt32| 4 +takeoffAlt | Float | Relative altitude from takeoff (in meters) that the vehicle should navigate to. + + +#### SOLO_INSPECT_SET_WAYPOINT + +* **Sent by:** App -> *ShotManager*. +* **Valid:** Anytime during the active Inspect shot + +Sent by the app to *ShotManager* to instruct Solo to navigate towards the provided waypoint. + +**version:** v2.2.0+ + + +| Field | Type | Value/Description +---------------- | ---- | ------------ +messageType | UInt32| 10002 +messageLength | UInt32| 12 +latitude | Float | Latitude in decimal degrees +longitude | Float | Longitude in decimal degrees +latitude | Float | Relative altitude from takeoff (in meters) + + +#### SOLO_INSPECT_MOVE_GIMBAL + +* **Sent by:** App -> *ShotManager*. +* **Valid:** Anytime during the active Inspect shot + +Sent by the app to *ShotManager* to instruct Solo to actuate gimbal to desired orientation. + +**version:** v2.2.0+ + + +| Field | Type | Value/Description +---------------- | ---- | ------------ +messageType | UInt32| 10003 +messageLength | UInt32| 12 +pitch | Float | Body-relative pitch in degrees (0 to -90) +roll | Float | Body-relative roll in degrees +yaw | Float | Earth frame Yaw (heading) in degrees (0 to 360) + + +#### SOLO_INSPECT_MOVE_VEHICLE + +* **Sent by:** App -> *ShotManager*. +* **Valid:** Anytime during the active Inspect shot + +Sent by the app to *ShotManager* to instruct Solo to move with a certain velocity (body-relative NED frame). + +**version:** v2.2.0+ + + +| Field | Type | Value/Description +---------------- | ---- | ------------ +messageType | UInt32| 10004 +messageLength | UInt32| 12 +vx | Float | Desired velocity in body-x (NED) +vy | Float | Desired velocity in body-y (NED) +vz | Float | Desired velocity in body-z (NED) + +### Site Scan Scan (SOLO_SCAN_) messages + +#### SOLO_SCAN_START + +* **Sent by:** App -> *ShotManager*. +* **Valid:** Only once during a Scan shot. + +Sent by the app to *ShotManager* to notify that the scan shot is ready to commence. + +**version:** v2.2.0+ + + +| Field | Type | Value/Description +---------------- | ---- | ------------ +messageType | UInt32| 10101 +messageLength | UInt32| 0 + + +### Site Scan Survey (SOLO_SURVEY_) messages + + +#### SOLO_SURVEY_START + +* **Sent by:** App -> *ShotManager*. +* **Valid:** Only once during a Survey shot. + +Sent by the app to *ShotManager* to notify that the survey shot is ready to commence. + +**version:** v2.2.0+ + + +| Field | Type | Value/Description +---------------- | ---- | ------------ +messageType | UInt32| 10201 +messageLength | UInt32| 0 + diff --git a/shotmanager/extFunctions.py b/shotmanager/extFunctions.py new file mode 100644 index 0000000..7715d84 --- /dev/null +++ b/shotmanager/extFunctions.py @@ -0,0 +1,251 @@ +''' +extFunctions.py + +This is the extended button function handler for Open Solo. This allows +controller button events above and beyond the original design to be configured +and utilized. The enumerations and available use here in extFunctions is +detailed below: + +BUTTON ENUMERATIONS AND USAGE: + ButtonPower = 0 Not used here. On Artoo only. + ButtonFly = 1 Not used here. Artoo and buttonManager.py only. + ButtonRTL = 2 Not used here. Artoo and buttonManager.py only. + ButtonLoiter = 3 Not used here. Artoo and buttonManager.py only. + ButtonA = 4 HoldRelease & LongHold available here + ButtonB = 5 HoldRelease & LongHold available here + ButtonPreset1 = 6 LongHold available here + ButtonPreset2 = 7 LongHold available here + ButtonCameraClick = 8 HoldRelease & LongHold available here + + +EVENT ENUMERATIONS AND USAGE: + Press = 0 Not used in Open Solo. + Release = 1 Not used in Open Solo. + ClickRelease = 2 Normal single button click (< 0.5 seconds) + Hold = 3 Not used + ShortHold = 4 Not used + LongHold = 5 3 second button hold + DoubleClick = 6 Not used + HoldRelease = 7 2 second hold and let go + LongHoldRelease =8 Not used. Just use LongHold since it's more intuitive. +''' +import errno +import os +import platform +import select +import socket +import string +import sys +import threading +import monotonic +import time +import modes +import settings +import shots +import shotLogger +import shots +from pymavlink import mavutil +from dronekit import VehicleMode +from sololink import btn_msg +from GoProConstants import * +sys.path.append(os.path.realpath('')) + +logger = shotLogger.logger + +# PWM values used in DO_SET_SERVO commands. +# Change these as desired. Or you can change the individial values +# in the function calls below +SERVO_PWM_LOW = 1000 +SERVO_PWM_MID = 1500 +SERVO_PWM_HIGH = 2000 + + +class extFunctions(): + + def __init__(self, vehicle, shotMgr): + self.vehicle = vehicle + self.shotMgr = shotMgr + self.gimablRetracted = 0 + self.shotSetting = -1 + self.modeSetting = -1 + self.FuncSetting = -1 + + def handleExtButtons(self, btnevent): + + if btnevent is None: + return + + button, event = btnevent + + # form a string in the format of {button},{event} + lookup = "%r,%r" % (button, event) + + # log the string + logger.log("[Ext Func]: Reading Button Event %s" % lookup) + + # lookup the setting for {button},{event} in extFunctions.conf + # returns the enumerations as shot,mode,function as a string + lookupResult = settings.readSettingExt(lookup) + + # dump it if no result + if lookupResult == 0: + return + + # log the found settings string + logger.log("[Ext Func]: Read %s" % lookupResult) + + # split the result string + buttonMapping = lookupResult.split(',') + + # set the shot, mode, and function setting vars using the split string + self.shotSetting = int(buttonMapping[0]) + self.modeSetting = int(buttonMapping[1]) + self.funcSetting = int(buttonMapping[2]) + + # if setting is a smart shot, call the specified shot + # if setting is a flight mode, call mode change function + if self.shotSetting >= 0 and self.modeSetting == -1: + self.enterSmartShot(self.shotSetting) + elif self.shotSetting == -1 and self.modeSetting >= 0: # Button is assigned a flight mode + self.changeFlightMode(self.modeSetting) + + # if setting has a function, do that too. + if self.funcSetting >= 0: # Button is assigned a function + # FUNCTIONS: + # -1 = none + # 1 = Landing gear up + # 2 = Landing gear down + # 3 = Gimbal lock toggle + # 4 = Servo 6 low + # 5 = Servo 6 med + # 6 = Servo 6 high + # 7 = Servo 7 low + # 8 = Servo 7 med + # 9 = Servo 7 high + # 10 = Servo 8 low + # 11 = Servo 8 med + # 12 = Servo 8 high + # 13 = Gripper Close + # 14 = Gripper Open + + if self.funcSetting == 1: # landing gear up + self.landingGear(1) + elif self.funcSetting == 2: # landing gear down + self.landingGear(0) + elif self.funcSetting == 3: # gimbal lock toggle + self.gimbalRetractToggle() + elif self.funcSetting == 4: # servo 6 low + self.setServo(6, SERVO_PWM_LOW) + elif self.funcSetting == 5: # servo 6 mid + self.setServo(6, SERVO_PWM_MID) + elif self.funcSetting == 6: # servo 6 high + self.setServo(6, SERVO_PWM_HIGH) + elif self.funcSetting == 7: # servo 7 low + self.setServo(7, SERVO_PWM_LOW) + elif self.funcSetting == 8: # servo 7 mid + self.setServo(7, SERVO_PWM_MID) + elif self.funcSetting == 9: # servo 7 high + self.setServo(7, SERVO_PWM_HIGH) + elif self.funcSetting == 10: # servo 8 low + self.setServo(8, SERVO_PWM_LOW) + elif self.funcSetting == 11: # servo 8 mid + self.setServo(8, SERVO_PWM_MID) + elif self.funcSetting == 12: # servo 8 high + self.setServo(8, SERVO_PWM_HIGH) + elif self.funcSetting == 13: # gripper close + self.setGripper(0) + elif self.funcSetting == 13: # gripper open + self.setGripper(1) + else: + pass + + def enterSmartShot(self, shot): + if self.shotMgr.currentShot != shots.APP_SHOT_NONE: + # exit whatever shot we're already in if we're in one. + self.shotMgr.enterShot(shots.APP_SHOT_NONE) + if shot in shots.CAN_START_FROM_ARTOO: + # enter the requested shot + logger.log("[Ext Func]: Trying shot %s via Artoo extended button" % shots.SHOT_NAMES[shot]) + self.shotMgr.enterShot(shot) + + def changeFlightMode(self, mode): + # Change flight modes. Exit a smart shot if we're in one. + if mode >= 0: + self.shotMgr.vehicle.mode = VehicleMode(mavutil.mode_mapping_acm.get(mode)) + logger.log("[Ext Func]: Requested a mode change via Artoo button to %s." % (modes.MODE_NAMES[mode])) + else: + logger.log("[Ext Func]: Invalid mode request. Quit pushing my buttons.") + + def landingGear(self, up_down): + # Raise or lower landing gear using mavlink command + # Argument up_down: 0 = down, 1 = up + if up_down == 0: + logger.log("[Ext Func] Landing Gear Down") + elif up_down == 1: + logger.log("[Ext Func] Landing Gear Up") + else: + logger.log("[Ext Func] Landing Gear Command Error. Putting gear down.") + up_down = 0 + msg = self.vehicle.message_factory.command_long_encode( + 0, 1, # target system, target component + mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION, # frame + 0, # confirmation + -1, # param 1:-1 = all landing gear + up_down, # param 2: 0 = down, 1 = up + 0, 0, 0, 0, 0) # params 3-7 (not used) + self.vehicle.send_mavlink(msg) + + def gimbalRetractToggle(self): + if self.gimablRetracted == 0: + self.gimablRetracted = 1 + logger.log("[Ext Func] Toggling gimbal to locked") + # set gimbal mode to locked''' + msg = self.vehicle.message_factory.command_long_encode( + 0, 1, # target system, target component + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, # frame + 0, # confirmation + 0, 0, 0, 0, 0, 0, # params 1-6 not used + mavutil.mavlink.MAV_MOUNT_MODE_RETRACT) # param 7 MAV_MOUNT_MODE + self.vehicle.send_mavlink(msg) + else: + self.gimablRetracted = 0 + logger.log("[Ext Func] Toggling gimbal to unlocked") + # set gimbal mode to normal + msg = self.vehicle.message_factory.command_long_encode( + 0, 1, # target system, target component + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, # frame + 0, # confirmation + 1, 1, 1, # parms 1-4 stabilize yes + 0, 0, 0, # params 4-6 not used + mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) # param 7 MAV_MOUNT_MODE + self.vehicle.send_mavlink(msg) + + + def setServo(self, ServoNum, PWMval): + # Set an ArduPilot servo output + # Argument ServoNum is the ArduPilot servo number + # Argument PWMval is the PWM value the servo will be set for + + msg = self.vehicle.message_factory.command_long_encode( + 0, 1, # target system, target component + mavutil.mavlink.MAV_CMD_DO_SET_SERVO, # frame + 0, # confirmation + ServoNum, # param 1: Desired servo number + PWMval, # param 2: Desired PWM value + 0, 0, 0, 0, 0) # params 3-7 (not used) + self.vehicle.send_mavlink(msg) + + + def setGripper(self, CloseOpen): + # Set the ArduPilot gripper + # Argument CloseOpen is 0 for close and 1 for open + + msg = self.vehicle.message_factory.command_long_encode( + 0, 1, # target system, target component + mavutil.mavlink.MAV_CMD_DO_GRIPPER, # frame + 0, # confirmation + 1, # param 1: Gripper # (ArduPilot only supports 1) + CloseOpen, # param 2: Open or Close + 0, 0, 0, 0, 0) # params 3-7 (not used) + self.vehicle.send_mavlink(msg) + diff --git a/shotmanager/extSettings.conf b/shotmanager/extSettings.conf new file mode 100644 index 0000000..e6e19bb --- /dev/null +++ b/shotmanager/extSettings.conf @@ -0,0 +1,89 @@ +[extFunctions] +4,7=-1, 0 ,-1 +4,5=-1, 3, -1 +5,7=-1, 11, -1 +5,5=-1, 13, -1 +6,5=-1, -1, -1 +7,5=-1, -1, -1 +8,7=-1, -1, -1 +8,5=-1, -1, 3 + + +[USAGE] +USAGE: {button},{event} = {shot}, {mode}, {func} + + +[USAGE-BUTTONS] +ButtonPower = 0 +ButtonFly = 1 +ButtonRTL = 2 +ButtonLoiter = 3 +ButtonA = 4 +ButtonB = 5 +ButtonPreset1 = 6 +ButtonPreset2 = 7 +ButtonCameraClick = 8 + + +[USAGE-EVENTS] +Short hold: 7 +Long hold: 5 + + +[USAGE-SHOTS] +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 + + +[USAGE-MODES] +-1 = None +0 = Stabilize +1 = Acro +2 = FLY: Manual (Alt Hold) +3 = Autonomous +4 = Takeoff +5 = FLY (Loiter) +6 = RTL +7 = Circle +8 = Position (Do not use!!) +9 = Land +10 = OF_LOITER (Do not use!!) +11 = Drift +13 = Sport +14 = Flip +15 = Auto-tune +16 = Position Hold +17 = Brake +18 = Throw (ArduCopter 3.5+) +19 = ADS-B Avoid (ArduCopter 3.5+ and not a user mode) +20 = Guided No GPS (ArduCopter 3.5+) +21 = SmartRTL (ArduCopter 3.6+ Only) + + +[USAGE-FUNCTIONS] +-1 = none +1 = Landing gear up (Not available in ArduCopter yet) +2 = Landing gear down (Not available in ArduCopter yet) +3 = Gimbal lock toggle +4 = Servo 6 low +5 = Servo 6 med +6 = Servo 6 high +7 = Servo 7 low +8 = Servo 7 med +9 = Servo 7 high +10 = Servo 8 low +11 = Servo 8 med +12 = Servo 8 high +13 = Gripper Close (ArduCopter 3.5+ Only) +14 = Gripper Open (ArduCopter 3.5+ Only) + diff --git a/shotmanager/flyController.py b/shotmanager/flyController.py new file mode 100644 index 0000000..859c21b --- /dev/null +++ b/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 + diff --git a/shotmanager/follow.py b/shotmanager/follow.py new file mode 100644 index 0000000..a3b0103 --- /dev/null +++ b/shotmanager/follow.py @@ -0,0 +1,685 @@ +''' +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. +''' + +import os +import math +import struct +import monotonic +import collections +import app_packet +import camera +import location_helpers +import pathHandler +import shotLogger +import shots +import socket +from dronekit import Vehicle, LocationGlobalRelative, VehicleMode +from pymavlink import mavutil +from shotManagerConstants import * +from orbitController import OrbitController +from lookAtController import LookAtController +from flyController import FlyController +from leashController import LeashController +from os import sys, path +from vector3 import Vector3 +from sololink import btn_msg +sys.path.append(os.path.realpath('')) + +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.. + 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.ClickRelease: + + # 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.ClickRelease: + 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('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(' 0: + (id, length, lat, lon, alt) = struct.unpack(' 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 + self.camPitch * 100, # Pitch in centidegrees + 0.0, # Roll not used + self.camYaw * 100, # Yaw in centidegrees + 0 # save position + ) + self.vehicle.send_mavlink(msg) + + else: + # if we don't have a gimbal, just set CONDITION_YAW + msg = self.vehicle.message_factory.command_long_encode( + 0, 1, # 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 + diff --git a/shotmanager/leashController.py b/shotmanager/leashController.py new file mode 100644 index 0000000..5316fe6 --- /dev/null +++ b/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.0 +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 \ No newline at end of file diff --git a/shotmanager/location_helpers.py b/shotmanager/location_helpers.py new file mode 100644 index 0000000..854ebb4 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/shotmanager/lookAtController.py b/shotmanager/lookAtController.py new file mode 100644 index 0000000..c43575a --- /dev/null +++ b/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) + diff --git a/shotmanager/main.py b/shotmanager/main.py new file mode 100755 index 0000000..575fbe0 --- /dev/null +++ b/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() diff --git a/shotmanager/modes.py b/shotmanager/modes.py new file mode 100755 index 0000000..6c239c1 --- /dev/null +++ b/shotmanager/modes.py @@ -0,0 +1,46 @@ +#!/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', + 18 : 'Throw\0', + 19 : 'ADS-B AVOID\0', + 20 : 'GUIDED NO GPS\0', + 21 : 'SMART RTL\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 diff --git a/shotmanager/multipoint.py b/shotmanager/multipoint.py new file mode 100644 index 0000000..938d544 --- /dev/null +++ b/shotmanager/multipoint.py @@ -0,0 +1,942 @@ +# 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.0 #m/s^2 +NORM_ACCEL_LIMIT = 1.75 #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. Use mount_control if using a gimbal. Use just condition_yaw if no 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 in centidegrees + 0.0, # roll not used + newYaw * 100, # yaw in centidegrees + 0 # save position + ) + self.vehicle.send_mavlink(pointingMsg) + + # if no gimbal, assume fixed mount and use condition_yaw only + else: + # if we don't have a gimbal, just set CONDITION_YAW + pointingMsg = self.vehicle.message_factory.command_long_encode( + 0, 1, # target system, target component + mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command + 0, # confirmation + newYaw, # param 1 - target angle (degrees) + YAW_SPEED, # param 2 - yaw speed (deg/s) + 0, # param 3 - direction, always shortest route for now... + 0.0, # relative offset + 0, 0, 0 # params 5-7 (unused) + ) + self.vehicle.send_mavlink(pointingMsg) + + + def 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(' 1: + # initialize multiPoint cable + self.enterPlayMode() + + # handle pause button + if button == btn_msg.ButtonLoiter and event == btn_msg.ClickRelease: + 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(' 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 in centidegrees + 0.0, # roll not used + startYaw * 100, # yaw in centidegrees + 0 # save position + ) + self.vehicle.send_mavlink(pointingMsg) + + # 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, 1, # 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) + ) + 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('= 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(' 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('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.ClickRelease: + if self.roi is None: + self.spotLock() + if button == btn_msg.ButtonLoiter and event == btn_msg.ClickRelease: + 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(' 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(' 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 diff --git a/shotmanager/pano.py b/shotmanager/pano.py new file mode 100644 index 0000000..8973c9e --- /dev/null +++ b/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) 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 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(' 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 + self.camPitch * 100, # Pitch in centidegrees + 0.0, # Roll not used + self.camYaw * 100, # Yaw in centidegrees + 0 # save position + ) + self.vehicle.send_mavlink(msg) + + else: + # if we don't have a gimbal, just set CONDITION_YAW + msg = self.vehicle.message_factory.command_long_encode( + 0, 1, # 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)) + diff --git a/shotmanager/pathHandler.py b/shotmanager/pathHandler.py new file mode 100644 index 0000000..771a701 --- /dev/null +++ b/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 = 2 +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 diff --git a/shotmanager/prereqs/.mavinit.scr b/shotmanager/prereqs/.mavinit.scr new file mode 100644 index 0000000..5545e3a --- /dev/null +++ b/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 diff --git a/shotmanager/prereqs/inittab b/shotmanager/prereqs/inittab new file mode 100644 index 0000000..f3e8e0e --- /dev/null +++ b/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 diff --git a/shotmanager/rcManager.py b/shotmanager/rcManager.py new file mode 100644 index 0000000..1b232f0 --- /dev/null +++ b/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 = 900 +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, THROTTLE_FAILSAFE, 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 in smart shots where the pitch paddle is used for altitude up/down (such as in zipline free look) + # >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) + diff --git a/shotmanager/requirements.txt b/shotmanager/requirements.txt new file mode 100644 index 0000000..577f400 --- /dev/null +++ b/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/OpenSolo/sololink-python.git#egg=sololink diff --git a/shotmanager/returnHome.py b/shotmanager/returnHome.py new file mode 100644 index 0000000..bfa730a --- /dev/null +++ b/shotmanager/returnHome.py @@ -0,0 +1,356 @@ +# +# 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 + + # Exit the shot and use RTL Mode + self.vehicle.mode = VehicleMode("RTL") + self.shotmgr.rcMgr.enableRemapping( false ) + return + + ########################################################## + + # 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.ClickRelease: + #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) + diff --git a/shotmanager/rewind.py b/shotmanager/rewind.py new file mode 100644 index 0000000..c1fd77a --- /dev/null +++ b/shotmanager/rewind.py @@ -0,0 +1,348 @@ +# +# 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 + + # Exit the shot and use RTL Mode + self.vehicle.mode = VehicleMode("RTL") + self.shotmgr.rcMgr.enableRemapping( false ) + return + + ############################################################ + + # 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.vehicle.mode = VehicleMode("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.ClickRelease): + #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) diff --git a/shotmanager/rewindManager.py b/shotmanager/rewindManager.py new file mode 100644 index 0000000..dd40637 --- /dev/null +++ b/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(' 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(' /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 diff --git a/shotmanager/selfie.py b/shotmanager/selfie.py new file mode 100644 index 0000000..c42f7e3 --- /dev/null +++ b/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.ClickRelease: + self.pointAtROI() + + + if button == btn_msg.ButtonLoiter and event == btn_msg.ClickRelease: + 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(' 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 + self.camPitch * 100, # Pitch in centidegrees + 0.0, # Roll not used + self.camYaw * 100, # Yaw in centidegrees + 0 # save position + ) + self.vehicle.send_mavlink(msg) + + else: + # if we don't have a gimbal, just set CONDITION_YAW + msg = self.vehicle.message_factory.command_long_encode( + 0, 1, # 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) diff --git a/shotmanager/settings.py b/shotmanager/settings.py new file mode 100644 index 0000000..62cbb5d --- /dev/null +++ b/shotmanager/settings.py @@ -0,0 +1,93 @@ +# +# 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" + CONFIG_FILE_EXT = "/usr/bin/extSettings.conf" + +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 + +def readSettingExt(name): + # get our saved button mappings for extended button functions + 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_EXT) + settingsLock.release() + + try: + return config.get("extFunctions", name) + except: + logger.log("[Ext Func Settings]: Unable to read %s from %s"%(name, CONFIG_FILE_EXT,)) + 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() + diff --git a/shotmanager/setupShots.sh b/shotmanager/setupShots.sh new file mode 100755 index 0000000..1d1e696 --- /dev/null +++ b/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" \ No newline at end of file diff --git a/shotmanager/shotFactory.py b/shotmanager/shotFactory.py new file mode 100644 index 0000000..9523642 --- /dev/null +++ b/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.") \ No newline at end of file diff --git a/shotmanager/shotLogger.py b/shotmanager/shotLogger.py new file mode 100644 index 0000000..b5b3fc0 --- /dev/null +++ b/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() diff --git a/shotmanager/shotManager.py b/shotmanager/shotManager.py new file mode 100644 index 0000000..ef36bb1 --- /dev/null +++ b/shotmanager/shotManager.py @@ -0,0 +1,504 @@ +# +# 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 +import extFunctions + +# 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 + +WPNAV_ACCEL_DEFAULT = 200 +ATC_ACCEL_DEFAULT = 36000 + + +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 extended functions ### + self.extFunctions = extFunctions.extFunctions(self.vehicle,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(' %s"%(self.lastMode, mode.name)) + + self.lastMode = mode.name + + modeIndex = modes.getAPMModeIndexFromName( mode.name, self.vehicle) + + if modeIndex < 0: + logger.log("couldn't find this mode index: %s" % self.mode.name) + return + else: + self.currentModeIndex = modeIndex + + # If changing to Guided, we're probably in a smart shot so no + # need to do anything here. If not, we need to run all this + # cleanup stuff in case the stick and button remapping did not + # properly clear from the last smart shot. + + if mode.name == 'GUIDED': + return + + self.setNormalControl(modeIndex, mode) + + except Exception as e: + logger.log('[shot]: mode callback error, %s' % e) + + def setNormalControl(self, modeIndex, mode): + # If we were in a smart shot, log that we bailed out out due to a mode change + if self.currentShot != shots.APP_SHOT_NONE: + logger.log("[callback]: Detected that we are not in the correct apm mode for this shot. Exiting shot!") + self.currentShot = shots.APP_SHOT_NONE + + # mark curController for garbage collection + if self.curController != None: + del self.curController + self.curController = None + + # re-enable manual gimbal controls (RC Targeting mode) + self.vehicle.gimbal.release() + + # disable the stick re-mapper + self.rcMgr.enableRemapping( False ) + + # let the world know + if self.appMgr.isAppConnected(): + self.appMgr.broadcastShotToApp(shots.APP_SHOT_NONE) + + # let Artoo know too + self.buttonManager.setArtooShot(shots.APP_SHOT_NONE, modeIndex) + + # set new button mappings appropriately + self.buttonManager.setButtonMappings() + + 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): + '''No longer used. Pause comes from the controller''' + return + + + # 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): + self.paramValue = self.vehicle.parameters.get(name, wait_ready=False) or default + logger.log("[SHOT]: Parameter %s is %d" %(name, self.paramValue)) + return self.paramValue + + # This sets the parameter matching the given name + def setParam(self, name, value): + logger.log("[SHOT]: Setting parameter %s to %d" %(name, value)) + return self.vehicle.parameters.set(name, value) + + # Set the ATC_ACCEL_P and ATC_ACCEL_R parameters + def setATCaccel(self,val): + self.setParam("ATC_ACCEL_P_MAX",val) + self.setParam("ATC_ACCEL_R_MAX",val) + + # 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 ''' + + # Nothing to do here now. ArduCopter handles it. + + + 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) \ No newline at end of file diff --git a/shotmanager/shotManagerConstants.py b/shotmanager/shotManagerConstants.py new file mode 100644 index 0000000..b6f31b7 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/shotmanager/shots.py b/shotmanager/shots.py new file mode 100755 index 0000000..be62733 --- /dev/null +++ b/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 diff --git a/shotmanager/sim/StressPerformance.py b/shotmanager/sim/StressPerformance.py new file mode 100644 index 0000000..36bda29 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/shotmanager/sim/TestRC.py b/shotmanager/sim/TestRC.py new file mode 100755 index 0000000..043c441 --- /dev/null +++ b/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("") +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('") + if button == "A": + packet = struct.pack('', 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 diff --git a/shotmanager/sim/shotmanager.conf b/shotmanager/sim/shotmanager.conf new file mode 100644 index 0000000..4a19703 --- /dev/null +++ b/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 diff --git a/shotmanager/sim/shotmanager.sandbox.conf b/shotmanager/sim/shotmanager.sandbox.conf new file mode 100644 index 0000000..a0d8be7 --- /dev/null +++ b/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 diff --git a/shotmanager/transect.py b/shotmanager/transect.py new file mode 100644 index 0000000..9370a72 --- /dev/null +++ b/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 diff --git a/shotmanager/vector2.py b/shotmanager/vector2.py new file mode 100644 index 0000000..5c9bcc5 --- /dev/null +++ b/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]) diff --git a/shotmanager/vector3.py b/shotmanager/vector3.py new file mode 100755 index 0000000..4a6c302 --- /dev/null +++ b/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]) \ No newline at end of file diff --git a/shotmanager/vectorPathHandler.py b/shotmanager/vectorPathHandler.py new file mode 100644 index 0000000..2e4e5a5 --- /dev/null +++ b/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 = 200 +WPNAV_ACCEL_Z = 160 + +# 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) + diff --git a/shotmanager/yawPitchOffsetter.py b/shotmanager/yawPitchOffsetter.py new file mode 100644 index 0000000..f27f431 --- /dev/null +++ b/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 diff --git a/shotmanager/zipline.py b/shotmanager/zipline.py new file mode 100644 index 0000000..4099fa0 --- /dev/null +++ b/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.ClickRelease: + self.setupZipline() + + if button == btn_msg.ButtonB and event == btn_msg.ClickRelease: + # 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.ClickRelease: + 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(' 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 + self.camPitch * 100, # Pitch in centidegrees + 0.0, # Roll not used + self.camYaw * 100, # Yaw in centidegrees + 0 # save position + ) + self.vehicle.send_mavlink(msg) + + else: + # if we don't have a gimbal, just set CONDITION_YAW + msg = self.vehicle.message_factory.command_long_encode( + 0, 1, # 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 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) +