From 39743a9be013c25584b7770fa6e37ed797cb09a8 Mon Sep 17 00:00:00 2001 From: Matt Date: Sat, 16 Dec 2017 11:36:51 -0500 Subject: [PATCH] SHOTS: Update for buttons, AC Master, & speeds Update to all smart shots for some or all of the following: - Button handling for the new Open Solo button handling rework - Copter yaw compatibility with ArduCopter master - Accelleration fixes and standardization --- shotmanager/follow.py | 189 +++++++++++++++++-------------- shotmanager/leashController.py | 2 +- shotmanager/multipoint.py | 72 +++++++++--- shotmanager/orbit.py | 5 +- shotmanager/orbitController.py | 2 +- shotmanager/pano.py | 62 ++++++---- shotmanager/pathHandler.py | 2 +- shotmanager/selfie.py | 4 +- shotmanager/vectorPathHandler.py | 4 +- shotmanager/zipline.py | 30 +++-- 10 files changed, 230 insertions(+), 142 deletions(-) diff --git a/shotmanager/follow.py b/shotmanager/follow.py index 1661503..5aba962 100644 --- a/shotmanager/follow.py +++ b/shotmanager/follow.py @@ -1,52 +1,48 @@ -# 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. +''' +follow.py +shotmanager -from dronekit import Vehicle, LocationGlobalRelative, VehicleMode +The Follow Me smart shot controller. +Runs as a DroneKit-Python script. +Created by Will Silva on 12/14/2015. +Altitude and Leash follow created by Jason Short 2/25/2016 + +Copyright (c) 2016 3D Robotics. +Licensed under the Apache License, Version 2.0 (the "License"); +You may not use this file except in compliance with the License. +You may obtain a copy of the License at +http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +''' -from pymavlink import mavutil import os -from os import sys, path import math import struct import monotonic import collections - -sys.path.append(os.path.realpath('')) import app_packet import camera import location_helpers import pathHandler import shotLogger -from shotManagerConstants import * import shots import socket +from 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 -# on host systems these files are located here from sololink import btn_msg +sys.path.append(os.path.realpath('')) FOLLOW_PORT = 14558 SOCKET_TIMEOUT = 0.01 @@ -75,27 +71,38 @@ ROI_ALT_FILTER_GAIN = 0.65 # Relative contribution of the previous and new data logger = shotLogger.logger +''' +Define the different followStates: -# 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 + 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 @@ -196,9 +203,11 @@ class FollowShot(): ''' Position Control - Note: All follow controllers return position and velocity of the drone in "absolute" space (as opposed to relative to the ROI) - Pos: Lat, lon, alt. Alt is relative to home location. NEU frame, of course. - Vel: Speed in the x,y, and z directions. NEU frame. vel.z needs to be inverted before passing to the autopilot. + 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) @@ -273,7 +282,7 @@ class FollowShot(): # if we can handle the button we do def handleButton(self, button, event): - if button == btn_msg.ButtonA and event == btn_msg.Press: + 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: @@ -284,7 +293,7 @@ class FollowShot(): self.initState(FOLLOW_ORBIT) # Change Follow Mode to Look At Me on Pause button press - if button == btn_msg.ButtonLoiter and event == btn_msg.Press: + if button == btn_msg.ButtonLoiter and event == btn_msg.ClickRelease: self.initState(FOLLOW_LOOKAT) self.shotmgr.notifyPause(True) self.updateAppOptions() @@ -616,42 +625,57 @@ class FollowShot(): # 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 - ) + 0, 1, # target system, target component + # pitch is in centidegrees + self.camPitch * 100, + 0.0, # roll + # yaw is in centidegrees + 0, #self.camYaw * 100, (Disabled by Matt due to ArduCopter master mount_control problem) + 0 # save position + ) + self.vehicle.send_mavlink(msg) + + # Use MAV_CMD_CONDITION_YAW since the yaw in MSG_MOUNT_CONTROL is not working in AC35 + msg = self.vehicle.message_factory.command_long_encode( + 0, 0, # target system, target component + mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command + 0, #confirmation + self.camYaw, # param 1 - target angle + YAW_SPEED, # param 2 - yaw speed + self.camDir, # param 3 - direction + 0.0, # relative offset + 0, 0, 0 # params 5-7 (unused) + ) + self.vehicle.send_mavlink(msg) + else: # if we don't have a gimbal, just set CONDITION_YAW msg = self.vehicle.message_factory.command_long_encode( - 0, 0, # target system, target component - mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command - 0, #confirmation - self.camYaw, # param 1 - target angle - YAW_SPEED, # param 2 - yaw speed - self.camDir, # param 3 - direction - 0.0, # relative offset - 0, 0, 0 # params 5-7 (unused) - ) - self.vehicle.send_mavlink(msg) + 0, 0, # target system, target component + mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command + 0, #confirmation + self.camYaw, # param 1 - target angle + YAW_SPEED, # param 2 - yaw speed + self.camDir, # param 3 - direction + 0.0, # relative offset + 0, 0, 0 # params 5-7 (unused) + ) + self.vehicle.send_mavlink(msg) def handleLookAtPointing(self, tempROI): # set ROI msg = self.vehicle.message_factory.command_int_encode( - 0, 1, # target system, target component - mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, #frame - mavutil.mavlink.MAV_CMD_DO_SET_ROI, #command - 0, #current - 0, #autocontinue - 0, 0, 0, 0, #params 1-4 - tempROI.lat*1.E7, - tempROI.lon*1.E7, - tempROI.alt + self.ROIAltitudeOffset #offset for ROI - ) + 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) @@ -673,3 +697,4 @@ class FollowShot(): return False else: return True + diff --git a/shotmanager/leashController.py b/shotmanager/leashController.py index 7ae8e12..5316fe6 100644 --- a/shotmanager/leashController.py +++ b/shotmanager/leashController.py @@ -33,7 +33,7 @@ from orbitController import OrbitController logger = shotLogger.logger #Path accel/decel constants -PATH_ACCEL = 2.5 +PATH_ACCEL = 2.0 ACCEL_PER_TICK = PATH_ACCEL * UPDATE_TIME ORBIT_RAD_FOR_MIN_SPEED = 2.0 diff --git a/shotmanager/multipoint.py b/shotmanager/multipoint.py index fe364ab..8003347 100644 --- a/shotmanager/multipoint.py +++ b/shotmanager/multipoint.py @@ -9,9 +9,8 @@ # 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. @@ -42,8 +41,8 @@ logger = shotLogger.logger # CONSTANTS -ACCEL_LIMIT = 2.5 #m/s^2 -NORM_ACCEL_LIMIT = 2.25 #m/s^2 +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) @@ -290,8 +289,11 @@ class MultipointShot(): newPitch += self.yawPitchOffsetter.pitchOffset newYaw += self.yawPitchOffsetter.yawOffset - # formulate mavlink message for mount controller - # do we have a gimbal? + # Formulate mavlink message for mount controller. Use mount_control if using a gimbal. Use just condition_yaw if no gimbal. + # Modified by Matt 2017-05-18 for ArducCopter master compatibility + + # mav_cmd_do_mount_control should handle gimbal pitch and copter yaw, but yaw is not working in master. + # So this mount_control command is only going to do the gimbal pitch for now. if self.vehicle.mount_status[0] is not None: pointingMsg = self.vehicle.message_factory.mount_control_encode( 0, 1, # target system, target component @@ -300,7 +302,23 @@ class MultipointShot(): newYaw * 100, # yaw (centidegrees) 0 # save position ) - # if not, assume fixed mount + self.vehicle.send_mavlink(pointingMsg) + + # Temporary fix while the yaw control is not working in the above mount_control command. + pointingMsg = self.vehicle.message_factory.command_long_encode( + 0, 0, # target system, target component + mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command + 0, # confirmation + newYaw, # param 1 - target angle (degrees) + YAW_SPEED, # param 2 - yaw speed (deg/s) + 0, # param 3 - direction, always shortest route for now... + 0.0, # relative offset + 0, 0, 0 # params 5-7 (unused) + ) + self.vehicle.send_mavlink(pointingMsg) + + + # if no gimbal, assume fixed mount and use condition_yaw only else: # if we don't have a gimbal, just set CONDITION_YAW pointingMsg = self.vehicle.message_factory.command_long_encode( @@ -313,9 +331,8 @@ class MultipointShot(): 0.0, # relative offset 0, 0, 0 # params 5-7 (unused) ) - - # send pointing command to vehicle - self.vehicle.send_mavlink(pointingMsg) + self.vehicle.send_mavlink(pointingMsg) + def interpolateCamera(self): '''Interpolate (linear) pitch and yaw between cable control points''' @@ -474,9 +491,9 @@ class MultipointShot(): def handleButton(self, button, event): '''handle actions for button presses''' if not self.cableCamPlaying: - if button == btn_msg.ButtonA and event == btn_msg.Press: + if button == btn_msg.ButtonA and event == btn_msg.ClickRelease: self.recordLocation() - if button == btn_msg.ButtonB and event == btn_msg.Press: + if button == btn_msg.ButtonB and event == btn_msg.ClickRelease: self.recordLocation() # only try to start a cable if we have more than 2 points if len(self.waypoints) > 1: @@ -484,7 +501,7 @@ class MultipointShot(): self.enterPlayMode() # handle pause button - if button == btn_msg.ButtonLoiter and event == btn_msg.Press: + if button == btn_msg.ButtonLoiter and event == btn_msg.ClickRelease: self.setCruiseSpeed(state = PAUSED) logger.log("[multipoint]: Pause button pressed on Artoo.") @@ -831,11 +848,32 @@ class MultipointShot(): if self.vehicle.mount_status[0] is not None: pointingMsg = self.vehicle.message_factory.mount_control_encode( 0, 1, # target system, target component - startPitch * 100, # pitch (centidegrees) + newPitch * 100, # pitch (centidegrees) 0.0, # roll (centidegrees) - startYaw * 100, # yaw (centidegrees) + newYaw * 100, # yaw (centidegrees) 0 # save position ) + self.vehicle.send_mavlink(pointingMsg) + + pointingMsg = self.vehicle.message_factory.command_long_encode( + 0, 0, # target system, target component + mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command + 0, # confirmation + startYaw, # param 1 - target angle (degrees) + ATTACH_YAW_SPEED, # param 2 - yaw speed (deg/s) + 0, # param 3 - direction, always shortest route for now... + 0.0, # relative offset + 0, 0, 0 # params 5-7 (unused) + ) + self.vehicle.send_mavlink(pointingMsg) + + # pointingMsg = self.vehicle.message_factory.mount_control_encode( + # 0, 1, # target system, target component + # startPitch * 100, # pitch (centidegrees) + # 0.0, # roll (centidegrees) + # startYaw * 100, # yaw (centidegrees) + # 0 # save position + # ) # if not, assume fixed mount else: # if we don't have a gimbal, just set CONDITION_YAW @@ -849,9 +887,7 @@ class MultipointShot(): 0.0, # relative offset 0, 0, 0 # params 5-7 (unused) ) - - # send pointing command to vehicle - self.vehicle.send_mavlink(pointingMsg) + self.vehicle.send_mavlink(pointingMsg) def listenForAttach(self): '''Checks if vehicle attaches to cable''' diff --git a/shotmanager/orbit.py b/shotmanager/orbit.py index 3ebb203..7221bbb 100644 --- a/shotmanager/orbit.py +++ b/shotmanager/orbit.py @@ -24,7 +24,6 @@ import os from os import sys, path import math import struct - sys.path.append(os.path.realpath('')) import app_packet import camera @@ -259,10 +258,10 @@ class OrbitShot(): def handleButton(self, button, event): '''Handle a controller button press''' - if button == btn_msg.ButtonA and event == btn_msg.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.Press: + if button == btn_msg.ButtonLoiter and event == btn_msg.ClickRelease: if self.pathHandler: self.pathHandler.togglePause() self.updateAppOptions() diff --git a/shotmanager/orbitController.py b/shotmanager/orbitController.py index 1689bad..926e005 100644 --- a/shotmanager/orbitController.py +++ b/shotmanager/orbitController.py @@ -33,7 +33,7 @@ from vector3 import Vector3 logger = shotLogger.logger #Path accel/decel constants -PATH_ACCEL = 2.5 +PATH_ACCEL = 2.0 ACCEL_PER_TICK = PATH_ACCEL * UPDATE_TIME ORBIT_MIN_SPEED = 3.0 diff --git a/shotmanager/pano.py b/shotmanager/pano.py index adb624e..6c12c6c 100644 --- a/shotmanager/pano.py +++ b/shotmanager/pano.py @@ -6,19 +6,19 @@ # Runs as a DroneKit-Python script under MAVProxy. # # Created by Jason Short and Will Silva on 11/30/2015. -# Copyright (c) 2015 3D Robotics. +# 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 @@ -391,7 +391,7 @@ class PanoShot(): # if we can handle the button we do def handleButton(self, button, event): - if button == btn_msg.ButtonA and event == btn_msg.Press: + if button == btn_msg.ButtonA and event == btn_msg.ClickRelease: # don't use A button for video ever if self.panoType != PANO_VIDEO: if self.state == PANO_SETUP: @@ -407,12 +407,12 @@ class PanoShot(): self.setButtonMappings() - if button == btn_msg.ButtonLoiter and event == btn_msg.Press: + if button == btn_msg.ButtonLoiter and event == btn_msg.ClickRelease: if self.panoType == PANO_VIDEO: self.degSecondYaw = 0 # cycle through options - if self.state == PANO_SETUP and button == btn_msg.ButtonB and event == btn_msg.Press: + if self.state == PANO_SETUP and button == btn_msg.ButtonB and event == btn_msg.ClickRelease: #clear Pano Video yaw speed self.degSecondYaw = 0 if self.panoType == PANO_VIDEO: @@ -469,27 +469,41 @@ class PanoShot(): # 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 - ) + 0, 1, # target system, target component + # pitch is in centidegrees + self.camPitch * 100, + 0.0, # roll + # yaw is in centidegrees + 0, # self.camYaw * 100, (Disabled by Matt for now due to ArduCopter master mount_control bug. Using condition_yaw instead) + 0 # save position + ) + self.vehicle.send_mavlink(msg) + + msg = self.vehicle.message_factory.command_long_encode( # Using condition_yaw temporarily until mount_control yaw issue is fixed + 0, 0, # target system, target component + mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command + 0, #confirmation + self.camYaw, # param 1 - target angle + YAW_SPEED, # param 2 - yaw speed + self.camDir, # param 3 - direction + 0.0, # relative offset + 0, 0, 0 # params 5-7 (unused) + ) + self.vehicle.send_mavlink(msg) + else: # if we don't have a gimbal, just set CONDITION_YAW msg = self.vehicle.message_factory.command_long_encode( - 0, 0, # target system, target component - mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command - 0, #confirmation - self.camYaw, # param 1 - target angle - YAW_SPEED, # param 2 - yaw speed - self.camDir, # param 3 - direction - 0.0, # relative offset - 0, 0, 0 # params 5-7 (unused) - ) - self.vehicle.send_mavlink(msg) + 0, 0, # target system, target component + mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command + 0, #confirmation + self.camYaw, # param 1 - target angle + YAW_SPEED, # param 2 - yaw speed + self.camDir, # param 3 - direction + 0.0, # relative offset + 0, 0, 0 # params 5-7 (unused) + ) + self.vehicle.send_mavlink(msg) def enterPhotoMode(self): # switch into photo mode if we aren't already in it diff --git a/shotmanager/pathHandler.py b/shotmanager/pathHandler.py index fd7071a..771a701 100644 --- a/shotmanager/pathHandler.py +++ b/shotmanager/pathHandler.py @@ -11,7 +11,7 @@ import shots logger = shotLogger.logger #Path accel/decel constants -PATH_ACCEL = 6 +PATH_ACCEL = 2 ACCEL_PER_TICK = PATH_ACCEL * UPDATE_TIME diff --git a/shotmanager/selfie.py b/shotmanager/selfie.py index d163a1c..179a97b 100644 --- a/shotmanager/selfie.py +++ b/shotmanager/selfie.py @@ -103,11 +103,11 @@ class SelfieShot(): # if we can handle the button we do def handleButton(self, button, event): - if button == btn_msg.ButtonA and event == btn_msg.Press: + if button == btn_msg.ButtonA and event == btn_msg.ClickRelease: self.pointAtROI() - if button == btn_msg.ButtonLoiter and event == btn_msg.Press: + if button == btn_msg.ButtonLoiter and event == btn_msg.ClickRelease: if self.pathHandler: self.shotmgr.notifyPause(True) self.pathHandler.togglePause() diff --git a/shotmanager/vectorPathHandler.py b/shotmanager/vectorPathHandler.py index 1550a9a..2e4e5a5 100644 --- a/shotmanager/vectorPathHandler.py +++ b/shotmanager/vectorPathHandler.py @@ -12,8 +12,8 @@ from vector3 import Vector3 logger = shotLogger.logger #Path accel/decel constants -WPNAV_ACCEL = 3.4 -WPNAV_ACCEL_Z = 1.6 +WPNAV_ACCEL = 200 +WPNAV_ACCEL_Z = 160 # for 3D max speed HIGH_PATH_SPEED = 5.0 diff --git a/shotmanager/zipline.py b/shotmanager/zipline.py index 4cc8a8d..1dc5b91 100644 --- a/shotmanager/zipline.py +++ b/shotmanager/zipline.py @@ -10,15 +10,15 @@ # 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 @@ -160,17 +160,17 @@ class ZiplineShot(): def handleButton(self, button, event): - if button == btn_msg.ButtonA and event == btn_msg.Press: + if button == btn_msg.ButtonA and event == btn_msg.ClickRelease: self.setupZipline() - if button == btn_msg.ButtonB and event == btn_msg.Press: + 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.Press: + if button == btn_msg.ButtonLoiter and event == btn_msg.ClickRelease: if self.pathHandler: self.pathHandler.togglePause() self.cruiseSpeed = self.pathHandler.cruiseSpeed @@ -321,10 +321,24 @@ class ZiplineShot(): # pitch is in centidegrees self.camPitch * 100.0, 0.0, # roll - # yaw is in centidegrees - self.camYaw * 100.0, + 0, # self.camYaw * 100.0, # yaw is in centidegrees (Disabled by Matt for now due to ArduCopter master mount_control bug. Using condition_yaw instead) 0 # save position ) + self.vehicle.send_mavlink(msg) + + msg = self.vehicle.message_factory.command_long_encode( # Using condition_yaw temporarily until mount_control yaw issue is fixed + 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) + + else: # if we don't have a gimbal, just set CONDITION_YAW msg = self.vehicle.message_factory.command_long_encode( @@ -337,7 +351,7 @@ class ZiplineShot(): 0.0, # relative offset 0, 0, 0 # params 5-7 (unused) ) - self.vehicle.send_mavlink(msg) + self.vehicle.send_mavlink(msg) def handleSpotLock(self, channels):