-
Notifications
You must be signed in to change notification settings - Fork 1.4k
/
vehicle_state.py
270 lines (204 loc) · 10.5 KB
/
vehicle_state.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
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
"""
vehicle_state.py:
Demonstrates how to get and set vehicle state and parameter information,
and how to observe vehicle attribute (state) changes.
Full documentation is provided at http://python.dronekit.io/examples/vehicle_state.html
"""
from dronekit import connect, VehicleMode
import time
#Set up option parsing to get connection string
import argparse
parser = argparse.ArgumentParser(description='Print out vehicle state information. Connects to SITL on local PC by default.')
parser.add_argument('--connect',
help="vehicle connection target string. If not specified, SITL automatically started and used.")
args = parser.parse_args()
connection_string = args.connect
sitl = None
#Start SITL if no connection string specified
if not args.connect:
print "Starting copter simulator (SITL)"
from dronekit_sitl import SITL
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,353']
sitl.launch(sitl_args, await_ready=True, restart=True)
connection_string = 'tcp:127.0.0.1:5760'
# Connect to the Vehicle.
# Set `wait_ready=True` to ensure default attributes are populated before `connect()` returns.
print "\nConnecting to vehicle on: %s" % connection_string
vehicle = connect(connection_string, wait_ready=True)
vehicle.wait_ready('autopilot_version')
# Get all vehicle attributes (state)
print "\nGet all vehicle attribute values:"
print " Autopilot Firmware version: %s" % vehicle.version
print " Major version number: %s" % vehicle.version.major
print " Minor version number: %s" % vehicle.version.minor
print " Patch version number: %s" % vehicle.version.patch
print " Release type: %s" % vehicle.version.release_type()
print " Release version: %s" % vehicle.version.release_version()
print " Stable release?: %s" % vehicle.version.is_stable()
print " Autopilot capabilities"
print " Supports MISSION_FLOAT message type: %s" % vehicle.capabilities.mission_float
print " Supports PARAM_FLOAT message type: %s" % vehicle.capabilities.param_float
print " Supports MISSION_INT message type: %s" % vehicle.capabilities.mission_int
print " Supports COMMAND_INT message type: %s" % vehicle.capabilities.command_int
print " Supports PARAM_UNION message type: %s" % vehicle.capabilities.param_union
print " Supports ftp for file transfers: %s" % vehicle.capabilities.ftp
print " Supports commanding attitude offboard: %s" % vehicle.capabilities.set_attitude_target
print " Supports commanding position and velocity targets in local NED frame: %s" % vehicle.capabilities.set_attitude_target_local_ned
print " Supports set position + velocity targets in global scaled integers: %s" % vehicle.capabilities.set_altitude_target_global_int
print " Supports terrain protocol / data handling: %s" % vehicle.capabilities.terrain
print " Supports direct actuator control: %s" % vehicle.capabilities.set_actuator_target
print " Supports the flight termination command: %s" % vehicle.capabilities.flight_termination
print " Supports mission_float message type: %s" % vehicle.capabilities.mission_float
print " Supports onboard compass calibration: %s" % vehicle.capabilities.compass_calibration
print " Global Location: %s" % vehicle.location.global_frame
print " Global Location (relative altitude): %s" % vehicle.location.global_relative_frame
print " Local Location: %s" % vehicle.location.local_frame
print " Attitude: %s" % vehicle.attitude
print " Velocity: %s" % vehicle.velocity
print " GPS: %s" % vehicle.gps_0
print " Gimbal status: %s" % vehicle.gimbal
print " Battery: %s" % vehicle.battery
print " EKF OK?: %s" % vehicle.ekf_ok
print " Last Heartbeat: %s" % vehicle.last_heartbeat
print " Rangefinder: %s" % vehicle.rangefinder
print " Rangefinder distance: %s" % vehicle.rangefinder.distance
print " Rangefinder voltage: %s" % vehicle.rangefinder.voltage
print " Heading: %s" % vehicle.heading
print " Is Armable?: %s" % vehicle.is_armable
print " System status: %s" % vehicle.system_status.state
print " Groundspeed: %s" % vehicle.groundspeed # settable
print " Airspeed: %s" % vehicle.airspeed # settable
print " Mode: %s" % vehicle.mode.name # settable
print " Armed: %s" % vehicle.armed # settable
# Get Vehicle Home location - will be `None` until first set by autopilot
while not vehicle.home_location:
cmds = vehicle.commands
cmds.download()
cmds.wait_ready()
if not vehicle.home_location:
print " Waiting for home location ..."
# We have a home location, so print it!
print "\n Home location: %s" % vehicle.home_location
# Set vehicle home_location, mode, and armed attributes (the only settable attributes)
print "\nSet new home location"
# Home location must be within 50km of EKF home location (or setting will fail silently)
# In this case, just set value to current location with an easily recognisable altitude (222)
my_location_alt = vehicle.location.global_frame
my_location_alt.alt = 222.0
vehicle.home_location = my_location_alt
print " New Home Location (from attribute - altitude should be 222): %s" % vehicle.home_location
#Confirm current value on vehicle by re-downloading commands
cmds = vehicle.commands
cmds.download()
cmds.wait_ready()
print " New Home Location (from vehicle - altitude should be 222): %s" % vehicle.home_location
print "\nSet Vehicle.mode = GUIDED (currently: %s)" % vehicle.mode.name
vehicle.mode = VehicleMode("GUIDED")
while not vehicle.mode.name=='GUIDED': #Wait until mode has changed
print " Waiting for mode change ..."
time.sleep(1)
# Check that vehicle is armable
while not vehicle.is_armable:
print " Waiting for vehicle to initialise..."
time.sleep(1)
# If required, you can provide additional information about initialisation
# using `vehicle.gps_0.fix_type` and `vehicle.mode.name`.
print "\nSet Vehicle.armed=True (currently: %s)" % vehicle.armed
vehicle.armed = True
while not vehicle.armed:
print " Waiting for arming..."
time.sleep(1)
print " Vehicle is armed: %s" % vehicle.armed
# Add and remove and attribute callbacks
#Define callback for `vehicle.attitude` observer
last_attitude_cache = None
def attitude_callback(self, attr_name, value):
# `attr_name` - the observed attribute (used if callback is used for multiple attributes)
# `self` - the associated vehicle object (used if a callback is different for multiple vehicles)
# `value` is the updated attribute value.
global last_attitude_cache
# Only publish when value changes
if value!=last_attitude_cache:
print " CALLBACK: Attitude changed to", value
last_attitude_cache=value
print "\nAdd `attitude` attribute callback/observer on `vehicle`"
vehicle.add_attribute_listener('attitude', attitude_callback)
print " Wait 2s so callback invoked before observer removed"
time.sleep(2)
print " Remove Vehicle.attitude observer"
# Remove observer added with `add_attribute_listener()` specifying the attribute and callback function
vehicle.remove_attribute_listener('attitude', attitude_callback)
# Add mode attribute callback using decorator (callbacks added this way cannot be removed).
print "\nAdd `mode` attribute callback/observer using decorator"
@vehicle.on_attribute('mode')
def decorated_mode_callback(self, attr_name, value):
# `attr_name` is the observed attribute (used if callback is used for multiple attributes)
# `attr_name` - the observed attribute (used if callback is used for multiple attributes)
# `value` is the updated attribute value.
print " CALLBACK: Mode changed to", value
print " Set mode=STABILIZE (currently: %s) and wait for callback" % vehicle.mode.name
vehicle.mode = VehicleMode("STABILIZE")
print " Wait 2s so callback invoked before moving to next example"
time.sleep(2)
print "\n Attempt to remove observer added with `on_attribute` decorator (should fail)"
try:
vehicle.remove_attribute_listener('mode', decorated_mode_callback)
except:
print " Exception: Cannot remove observer added using decorator"
# Demonstrate getting callback on any attribute change
def wildcard_callback(self, attr_name, value):
print " CALLBACK: (%s): %s" % (attr_name,value)
print "\nAdd attribute callback detecting ANY attribute change"
vehicle.add_attribute_listener('*', wildcard_callback)
print " Wait 1s so callback invoked before observer removed"
time.sleep(1)
print " Remove Vehicle attribute observer"
# Remove observer added with `add_attribute_listener()`
vehicle.remove_attribute_listener('*', wildcard_callback)
# Get/Set Vehicle Parameters
print "\nRead and write parameters"
print " Read vehicle param 'THR_MIN': %s" % vehicle.parameters['THR_MIN']
print " Write vehicle param 'THR_MIN' : 10"
vehicle.parameters['THR_MIN']=10
print " Read new value of param 'THR_MIN': %s" % vehicle.parameters['THR_MIN']
print "\nPrint all parameters (iterate `vehicle.parameters`):"
for key, value in vehicle.parameters.iteritems():
print " Key:%s Value:%s" % (key,value)
print "\nCreate parameter observer using decorator"
# Parameter string is case-insensitive
# Value is cached (listeners are only updated on change)
# Observer added using decorator can't be removed.
@vehicle.parameters.on_attribute('THR_MIN')
def decorated_thr_min_callback(self, attr_name, value):
print " PARAMETER CALLBACK: %s changed to: %s" % (attr_name, value)
print "Write vehicle param 'THR_MIN' : 20 (and wait for callback)"
vehicle.parameters['THR_MIN']=20
for x in range(1,5):
#Callbacks may not be updated for a few seconds
if vehicle.parameters['THR_MIN']==20:
break
time.sleep(1)
#Callback function for "any" parameter
print "\nCreate (removable) observer for any parameter using wildcard string"
def any_parameter_callback(self, attr_name, value):
print " ANY PARAMETER CALLBACK: %s changed to: %s" % (attr_name, value)
#Add observer for the vehicle's any/all parameters parameter (defined using wildcard string ``'*'``)
vehicle.parameters.add_attribute_listener('*', any_parameter_callback)
print " Change THR_MID and THR_MIN parameters (and wait for callback)"
vehicle.parameters['THR_MID']=400
vehicle.parameters['THR_MIN']=30
## Reset variables to sensible values.
print "\nReset vehicle attributes/parameters and exit"
vehicle.mode = VehicleMode("STABILIZE")
vehicle.armed = False
vehicle.parameters['THR_MIN']=130
vehicle.parameters['THR_MID']=500
#Close vehicle object before exiting script
print "\nClose vehicle object"
vehicle.close()
# Shut down simulator if it was started.
if sitl is not None:
sitl.stop()
print("Completed")