mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 22:24:32 +02:00
341 lines
11 KiB
Python
341 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
|
|
|
|
# data manager for breadcrumbs
|
|
self.rewindManager = shotmgr.rewindManager
|
|
|
|
# defines how we exit
|
|
self.exitToRTL = True
|
|
|
|
# enable stick remappings
|
|
self.shotmgr.rcMgr.enableRemapping( True )
|
|
|
|
# enter GUIDED mode
|
|
logger.log("[Rewind] Try Guided")
|
|
self.setButtonMappings()
|
|
self.vehicle.mode = VehicleMode("GUIDED")
|
|
|
|
# grab a copy of home
|
|
self.homeLocation = self.shotmgr.getHomeLocation()
|
|
|
|
''' spline '''
|
|
# default targetP
|
|
self.targetP = 0.0
|
|
|
|
# initialize cable to None
|
|
self.cable = None
|
|
|
|
# last time that the controller was advanced
|
|
self.lastTime = None
|
|
|
|
self.splineOrigin = None
|
|
|
|
if not self.generateSplines():
|
|
logger.log("[Rewind]: Spline generation failed.")
|
|
|
|
if self.cable is not None:
|
|
# go to 1.0
|
|
self.cable.setTargetP(1.0)
|
|
|
|
# give cable controller our desired speed
|
|
self.cable.trackSpeed(REWIND_SPEED)
|
|
|
|
# Camera control
|
|
self.camYaw = camera.getYaw(self.vehicle)
|
|
self.camPitch = camera.getPitch(self.vehicle)
|
|
self.camDir = 1
|
|
self.manualGimbalTargeting()
|
|
|
|
|
|
def handleRCs(self, channels):
|
|
if self.cable is None:
|
|
if self.exitToRTL:
|
|
self.exitRewind()
|
|
else:
|
|
self.shotmgr.enterShot(shots.APP_SHOT_NONE)
|
|
return
|
|
|
|
self.travel()
|
|
|
|
# Freelook
|
|
self.manualPitch(channels)
|
|
self.manualYaw(channels)
|
|
self.handleFreeLookPointing()
|
|
|
|
if self.cable.reachedTarget():
|
|
self.cable.trackSpeed(0)
|
|
if self.exitToRTL:
|
|
logger.log("[Rewind] exiting at end of Spline")
|
|
self.exitRewind()
|
|
|
|
if self.isNearHome():
|
|
if self.exitToRTL:
|
|
logger.log("[Rewind] Exiting Near Home")
|
|
self.exitRewind()
|
|
|
|
|
|
def exitRewind(self):
|
|
self.rewindManager.resetSpline()
|
|
self.shotmgr.enterShot(shots.APP_SHOT_RTL)
|
|
|
|
|
|
def travel(self):
|
|
# advance cable controller by dt (time elapsed)
|
|
now = monotonic.monotonic()
|
|
if self.lastTime is None:
|
|
dt = UPDATE_TIME
|
|
else:
|
|
dt = now - self.lastTime
|
|
self.lastTime = now
|
|
|
|
self.cable.update(dt)
|
|
|
|
# add NED position vector to spline origin (addVectorToLocation needs NEU)
|
|
self.commandPos = location_helpers.addVectorToLocation(self.splineOrigin, Vector3(self.cable.position.x, self.cable.position.y, -self.cable.position.z))
|
|
|
|
# assign velocity from controller
|
|
self.commandVel = self.cable.velocity
|
|
|
|
# formulate mavlink message for pos-vel controller
|
|
posVelMsg = self.vehicle.message_factory.set_position_target_global_int_encode(
|
|
0, # time_boot_ms (not used)
|
|
0, 1, # target system, target component
|
|
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame
|
|
0b0000110111000000, # type_mask - enable pos/vel
|
|
int(self.commandPos.lat * 10000000), # latitude (degrees*1.0e7)
|
|
int(self.commandPos.lon * 10000000), # longitude (degrees*1.0e7)
|
|
self.commandPos.alt, # altitude (meters)
|
|
self.commandVel.x, self.commandVel.y, self.commandVel.z, # North, East, Down velocity (m/s)
|
|
0, 0, 0, # x, y, z acceleration (not used)
|
|
0, 0) # yaw, yaw_rate (not used)
|
|
|
|
# send pos-vel command to vehicle
|
|
self.vehicle.send_mavlink(posVelMsg)
|
|
|
|
|
|
def isNearHome(self):
|
|
if self.homeLocation is None:
|
|
return True
|
|
|
|
mydist = location_helpers.getDistanceFromPoints(self.vehicle.location.global_relative_frame, self.homeLocation)
|
|
return (mydist < REWIND_MIN_HOME_DISTANCE)
|
|
|
|
|
|
def handleButton(self, button, event):
|
|
# any Pause button press or release should get out of Rewind
|
|
if button == btn_msg.ButtonLoiter and (event == btn_msg.Release or event == btn_msg.Press):
|
|
#exit to fly
|
|
self.shotmgr.enterShot(shots.APP_SHOT_NONE)
|
|
return
|
|
|
|
|
|
def handlePacket(self, packetType, packetLength, packetValue):
|
|
return False
|
|
|
|
|
|
def setButtonMappings(self):
|
|
buttonMgr = self.shotmgr.buttonManager
|
|
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_REWIND, 0, "\0")
|
|
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_REWIND, 0, "\0")
|
|
|
|
|
|
def updateAppOptions(self):
|
|
return
|
|
|
|
def addLocation(self, loc):
|
|
return
|
|
|
|
def generateSplines(self):
|
|
'''Generate the multi-point spline'''
|
|
|
|
logger.log("[Rewind] generateSplines")
|
|
# store the Lat,lon,alt locations
|
|
ctrlPtsLLA = []
|
|
# store vectors for relative offsets
|
|
ctrlPtsCart = []
|
|
# set initial control point as origin
|
|
ctrlPtsCart.append(Vector3(0, 0, 0))
|
|
|
|
# try and load a point
|
|
loc = self.rewindManager.queueNextloc()
|
|
if loc is None:
|
|
return False
|
|
|
|
logger.log("[Rewind] read loc: %f %f %f" % (loc.lat, loc.lon, loc.alt))
|
|
|
|
ctrlPtsLLA.append(loc)
|
|
# store as spline origin
|
|
self.splineOrigin = ctrlPtsLLA[0]
|
|
|
|
# read all available locations
|
|
while (loc is not None):
|
|
loc = self.rewindManager.queueNextloc()
|
|
if loc is not None:
|
|
logger.log("[Rewind] read loc: %f %f %f" % (loc.lat, loc.lon, loc.alt))
|
|
ctrlPtsLLA.append(loc)
|
|
else:
|
|
print "loc: None"
|
|
|
|
# try and have a 3 point spline or longer:
|
|
if len(ctrlPtsLLA) < 2:
|
|
return False
|
|
|
|
# Save offsets from home for spline
|
|
for n in range(1, len(ctrlPtsLLA)):
|
|
ctrlPtsCart.append(location_helpers.getVectorFromPoints(self.splineOrigin, ctrlPtsLLA[n]))
|
|
ctrlPtsCart[-1].z *= -1. #NED
|
|
|
|
# Construct spline object
|
|
try:
|
|
self.cable = cableController.CableController(points = ctrlPtsCart, maxSpeed = REWIND_SPEED, minSpeed = REWIND_MIN_SPEED, tanAccelLim = TANGENT_ACCEL_LIMIT, normAccelLim = NORM_ACCEL_LIMIT, smoothStopP = 0.7, maxAlt = 400)
|
|
except ValueError, e:
|
|
logger.log("%s", e)
|
|
return False
|
|
|
|
#set the location to the start point
|
|
self.cable.setCurrentP(0)
|
|
return True
|
|
|
|
|
|
def manualGimbalTargeting(self):
|
|
'''set gimbal targeting mode to manual'''
|
|
msg = self.vehicle.message_factory.mount_configure_encode(
|
|
0, 1, # target system, target component
|
|
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, # mount_mode
|
|
1, # stabilize roll
|
|
1, # stabilize pitch
|
|
1, # stabilize yaw
|
|
)
|
|
self.vehicle.send_mavlink(msg)
|
|
|
|
|
|
def manualPitch(self, channels):
|
|
if abs(channels[RAW_PADDLE]) > abs(channels[THROTTLE]):
|
|
value = channels[RAW_PADDLE]
|
|
else:
|
|
value = channels[THROTTLE]
|
|
|
|
self.camPitch += PITCH_SPEED * UPDATE_TIME * value
|
|
|
|
if self.camPitch > 0.0:
|
|
self.camPitch = 0.0
|
|
elif self.camPitch < -90:
|
|
self.camPitch = -90
|
|
|
|
|
|
def manualYaw(self, channels):
|
|
if channels[YAW] == 0:
|
|
return
|
|
|
|
self.camYaw += channels[YAW] * YAW_SPEED * UPDATE_TIME
|
|
if self.camYaw > 360:
|
|
self.camYaw -= 360
|
|
if self.camYaw < 0:
|
|
self.camYaw += 360
|
|
|
|
# required for gimbals w/o Yaw
|
|
if channels[YAW] > 0:
|
|
self.camDir = 1
|
|
else:
|
|
self.camDir = -1
|
|
|
|
def handleFreeLookPointing(self):
|
|
'''Handle free look'''
|
|
# if we do have a gimbal, use mount_control to set pitch and yaw
|
|
if self.vehicle.mount_status[0] is not None:
|
|
msg = self.vehicle.message_factory.mount_control_encode(
|
|
0, 1, # target system, target component
|
|
# pitch is in centidegrees
|
|
self.camPitch * 100.0,
|
|
0.0, # roll
|
|
# yaw is in centidegrees
|
|
self.camYaw * 100.0,
|
|
0 # save position
|
|
)
|
|
else:
|
|
# if we don't have a gimbal, just set CONDITION_YAW
|
|
msg = self.vehicle.message_factory.command_long_encode(
|
|
0, 0, # target system, target component
|
|
mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command
|
|
0, # confirmation
|
|
self.camYaw, # param 1 - target angle
|
|
YAW_SPEED, # param 2 - yaw speed
|
|
self.camDir, # param 3 - direction XXX
|
|
0.0, # relative offset
|
|
0, 0, 0 # params 5-7 (unused)
|
|
)
|
|
self.vehicle.send_mavlink(msg)
|