This repository has been archived by the owner on Apr 10, 2022. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 10
/
Copy pathleashController.py
157 lines (122 loc) · 6.05 KB
/
leashController.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
# 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