mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 22:24:32 +02:00
244546d390
The rewind smart shot has a number of reliability and safety issues. As such, it is being discontinued in Open Solo. If anything still happens to call the rewind smart shot, it will immediately exit to ArduCopter RTL mode.
349 lines
11 KiB
Python
349 lines
11 KiB
Python
#
|
|
# 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)
|