This repository was 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
Expand file tree
/
Copy pathorbit.py
More file actions
352 lines (279 loc) · 13.7 KB
/
orbit.py
File metadata and controls
352 lines (279 loc) · 13.7 KB
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
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
# orbit.py
# shotmanager
#
# The orbit smart shot.
# Runs as a DroneKit-Python script.
#
# Created by Will Silva and Eric Liao on 1/19/2015.
# 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
import os
from os import sys, path
import math
import struct
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
import orbitController
from orbitController import OrbitController
from vector3 import Vector3
from sololink import btn_msg
SHALLOW_ANGLE_THRESHOLD = -5
# if we didn't aim at the ground or we don't have a gimbal
MAX_ALT_DIFF = 9999.0
TICKS_TO_IGNORE_PADDLE = 2.0 * UPDATE_RATE
# in degrees per second
YAW_SPEED = 25.0
APP_UPDATE = 15
DEFAULT_PILOT_VELZ_MAX_VALUE = 133.0
ZVEL_FACTOR = 0.95
logger = shotLogger.logger
class OrbitShot():
def __init__(self, vehicle, shotmgr):
# copy vehicle object
self.vehicle = vehicle
# reference shotManager object
self.shotmgr = shotmgr
# initialize roi to None
self.roi = None
# Altitude difference between where the ROI is and where the camera should look
self.ROIAltitudeOffset = 0
# should we inform the app the ROI has moved
self.roi_needsUpdate = False
# counter to send perioidic app updates when needed
self.appNeedsUpdate = 0
# initialize path controller to None
self.pathController = None
# keep track of how long we've kept the paddle centered
self.ticksPaddleCentered = float('inf')
# initialize pathHandler to None
self.pathHandler = None
# get max climb rate from APM parameters
self.maxClimbRate = self.shotmgr.getParam( "PILOT_VELZ_MAX", DEFAULT_PILOT_VELZ_MAX_VALUE ) / 100.0 * ZVEL_FACTOR
# log the max altitude downloaded from APM
logger.log("[orbit]: Maximum climb rate stored: %f" % self.maxClimbRate)
# get max altitude from APM parameters
self.maxAlt = self.shotmgr.getParam( "FENCE_ALT_MAX", DEFAULT_FENCE_ALT_MAX )
# log the max altitude downloaded from APM
logger.log("[orbit]: Maximum altitude stored: %f" % self.maxAlt)
# check APM params to see if Altitude Limit should apply
if self.shotmgr.getParam( "FENCE_ENABLE", DEFAULT_FENCE_ENABLE ) == 0:
self.maxAlt = None
logger.log("[Orbit]: Altitude Limit is disabled.")
# Now change the vehicle into guided mode
self.vehicle.mode = VehicleMode("GUIDED")
# set gimbal targeting mode
msg = self.vehicle.message_factory.mount_configure_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, #mount_mode
1, # stabilize roll
1, # stabilize pitch
1, # stabilize yaw
)
self.vehicle.send_mavlink(msg)
# enable stick remappings
self.shotmgr.rcMgr.enableRemapping( True )
# channels are expected to be floating point values in the (-1.0, 1.0) range
def handleRCs( self, channels ):
# don't continue until an roi is set
if not self.pathHandler or not self.roi:
return
# allow user to rotate the ROI about the vehicle
if abs(channels[YAW]) > 0:
# adding 180 flips the az (ROI to vehicle) to (vehicle to ROI)
tmp = self.roi.alt
self.pathController.azimuth += channels[YAW] * YAW_SPEED * UPDATE_TIME
self.roi = location_helpers.newLocationFromAzimuthAndDistance(
self.vehicle.location.global_relative_frame,
180 + self.pathController.azimuth,
self.pathController.radius)
self.roi.alt = tmp
self.roi_needsUpdate = True
# call path controller
pos, vel = self.pathController.move(channels, self.pathHandler.cruiseSpeed, self.roi)
# mavlink expects 10^7 integer for accuracy
latInt = int(pos.lat * 10000000)
lonInt = int(pos.lon * 10000000)
# create the SET_POSITION_TARGET_GLOBAL_INT command
msg = 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
latInt, lonInt, pos.alt, # x, y, z positions
vel.x, vel.y, vel.z, # x, y, z velocity in m/s
0, 0, 0, # x, y, z acceleration (not used)
0, 0) # yaw, yaw_rate (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
# pointing logic
# Adjust the height of the ROI using the paddle
# we pass in both the filtered gimbal paddle value as well as the raw one
self.setROIAltitude( channels[5], channels[7] )
# set ROI
msg = self.vehicle.message_factory.command_long_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_DO_SET_ROI, #command
0, #confirmation
0, 0, 0, 0, #params 1-4
self.roi.lat,
self.roi.lon,
self.roi.alt + self.ROIAltitudeOffset
)
# send pointing message for either cam mode
self.vehicle.send_mavlink(msg)
self.updateApp()
def updateApp(self):
# used to limit the bandwidth needed to transmit many ROI updates
self.appNeedsUpdate += 1
if self.appNeedsUpdate > APP_UPDATE:
self.appNeedsUpdate = 0
if self.roi_needsUpdate:
self.roi_needsUpdate = False
# send this ROI to the app
#logger.log("ROI %f %f" % (self.roi.lat, self.roi.lon))
packet = struct.pack('<IIddf', app_packet.SOLO_MESSAGE_LOCATION, 20, self.roi.lat, self.roi.lon, self.roi.alt)
self.shotmgr.appMgr.sendPacket(packet)
def spotLock(self):
'''take the angle of the copter and lock onto a ground level target'''
logger.log("[Orbit] spotLock")
# don't use a shallow angle resulting in massively distant ROIs
pitch = min(camera.getPitch(self.vehicle), SHALLOW_ANGLE_THRESHOLD)
# Get ROI for the vehicle to look at
spotLock = location_helpers.getSpotLock(self.vehicle.location.global_relative_frame, pitch, camera.getYaw(self.vehicle))
self.addLocation(spotLock)
def addLocation(self, loc):
'''Adds a new or overwrites the current orbit ROI'''
self.roi = loc
# tell the app about the new ROI
packet = struct.pack('<IIddf', app_packet.SOLO_MESSAGE_LOCATION, 20, self.roi.lat, self.roi.lon, self.roi.alt)
self.shotmgr.appMgr.sendPacket(packet)
# should we init the Orbit state machine
if self.pathHandler is None:
self.initOrbitShot()
# Initialize the location of the vehicle
startRadius = location_helpers.getDistanceFromPoints(self.roi, self.vehicle.location.global_relative_frame)
startAz = location_helpers.calcAzimuthFromPoints(self.roi, self.vehicle.location.global_relative_frame)
startAlt = self.vehicle.location.global_relative_frame.alt
logger.log("[Orbit]: Add Location for Orbit controller.")
logger.log("[Orbit]: -->radius: %f, azimuth: %f" % (startRadius, startAz))
logger.log("[Orbit]: -->lat: %f, lon: %f, alt: %f" % (self.roi.lat, self.roi.lon, self.roi.alt))
# Initialize the open-loop orbit controller
self.pathController = orbitController.OrbitController(self.roi, startRadius, startAz, startAlt)
self.pathController.setOptions(maxClimbRate = self.maxClimbRate, maxAlt = self.maxAlt)
def initOrbitShot(self):
'''Initialize the orbit autonomous controller - called once'''
# create pathHandler object
self.pathHandler = pathHandler.PathHandler( self.vehicle, self.shotmgr )
# set button mappings
self.setButtonMappings()
def setButtonMappings(self):
'''Map the controller buttons'''
buttonMgr = self.shotmgr.buttonManager
if self.roi:
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_ORBIT, 0, "\0")
else:
buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_ORBIT, btn_msg.ARTOO_BITMASK_ENABLED, "Begin\0")
buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_ORBIT, 0, "\0")
def handleButton(self, button, event):
'''Handle a controller button 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.ClickRelease:
if self.pathHandler:
self.pathHandler.togglePause()
self.updateAppOptions()
self.shotmgr.notifyPause(True)
else:
self.shotmgr.notifyPause(False)
def updateAppOptions(self):
'''send our current set of options to the app'''
packet = struct.pack('<IIf', app_packet.SOLO_SHOT_OPTIONS, 4, self.pathHandler.cruiseSpeed)
self.shotmgr.appMgr.sendPacket(packet)
# Sets our ROI according to the filtered gimbal paddle.
# We know the desired gimbal angle (linear conversion from filtered gimbal paddle value)
# So we just need to convert that to an ROI altitude
# We only use this to set our ROI if the user is actually adjusting it.
# In order to tell, we compare rawPaddleValue to 0.0, but we need to wait a bit
# so that filteredPaddleInput settles
# NOTE: This will not handle preset gimbal animations from Artoo. But
# I don't think there's a way to do that without adding more messaging between Artoo/shot manager.
# Using a preset during an Orbit isn't something anyone's asked for.
#
# Note that we no longer update the actual ROI and instead just maintain an offset that is adjusted for pointing (but the vehicle is still controlled relative to the original ROI)
def setROIAltitude(self, filteredPaddleInput, rawPaddleValue):
# no gimbal, no reason to change ROI
if self.vehicle.mount_status[0] == None:
return
# if the paddle is centered, we tick up our count
# If it's been centered for long enough don't allow the filtered paddle value to
# overwrite our ROI Alt
# We want to wait a little bit after the gimbal paddle returns to center so that
# the filtered value stabilizes
if rawPaddleValue == 0.0:
self.ticksPaddleCentered += 1
if self.ticksPaddleCentered > TICKS_TO_IGNORE_PADDLE:
return
else:
self.ticksPaddleCentered = 0
# our input is in the range (-1.0, 1.0)
# let's convert it to a desired angle, where -1.0 -> -90.0 and 1.0 -> 0.0
input = ( filteredPaddleInput + 1.0 ) / 2.0
# inverse it
input = 1.0 - input
# convert to gimbal range in radians
theta = input * math.radians(90.0)
# now we want to calculate our altitude difference of our ROI
# we want our ROI to be at angle theta from our copter
# tan theta = altDiff / radius
# altDiff = radius * tan theta
tanTheta = math.tan(theta)
altDiff = self.pathController.radius * tanTheta
# In the case that we're pointing straight down,
# cap the number we're sending, because otherwise it'll cause issues.
if altDiff > MAX_ALT_DIFF:
altDiff = MAX_ALT_DIFF
# XXX suspect - why bring in vehicle alt to open loop controller?
self.ROIAltitudeOffset = self.vehicle.location.global_relative_frame.alt - altDiff
def handlePacket(self, packetType, packetLength, packetValue):
try:
if packetType == app_packet.SOLO_RECORD_POSITION:
logger.log("[orbit]: record spotlock")
self.spotLock()
elif packetType == app_packet.SOLO_MESSAGE_LOCATION:
(lat, lon, alt) = struct.unpack('<ddf', packetValue)
logger.log("[orbit]: Location received from app: %f, %f, %f." %( lat, lon, alt ) )
# forces the controller to reset the pathhandler
self.roi = None
self.addLocation(LocationGlobalRelative(lat, lon, alt))
elif packetType == app_packet.SOLO_SHOT_OPTIONS:
if self.pathHandler:
(cruiseSpeed,) = struct.unpack('<f', packetValue)
self.pathHandler.setCruiseSpeed(cruiseSpeed)
logger.log("[orbit]: Cruise speed set to %.2f." % (cruiseSpeed,))
else:
return False
except Exception as e:
logger.log('[orbit]: Error handling packet. (%s)' % e)
return False
else:
return True