-
Notifications
You must be signed in to change notification settings - Fork 1
/
vehicle_state.py
260 lines (196 loc) · 10.2 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
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
© Copyright 2015-2016, 3D Robotics.
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 __future__ import print_function
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
# 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)
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")