mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 22:24:32 +02:00
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
This commit is contained in:
Matt
parent
296a8af950
commit
39743a9be0
@ -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.
|
||||
|
||||
# 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.
|
||||
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
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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)
|
||||
)
|
||||
self.vehicle.send_mavlink(pointingMsg)
|
||||
|
||||
# send pointing command to vehicle
|
||||
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'''
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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):
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user