mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 22:24:32 +02:00
39743a9be0
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
157 lines
6.0 KiB
Python
157 lines
6.0 KiB
Python
# 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 |