/
status.py
127 lines (107 loc) · 5.51 KB
/
status.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
#!/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 dronekit import connect, VehicleMode, LocationGlobal, LocationGlobalRelative
from pymavlink import mavutil # Needed for command message definitions
import time
import math
#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()
vehicle = connect("udpin:0.0.0.0:14550", 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
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
target_alt = vehicle.location.global_relative_frame.alt+5;
print "Taking off!"
vehicle.simple_takeoff(target_alt) # Take off to target altitude
print "Set default/target airspeed to 3"
vehicle.airspeed = 3
# Wait until the vehicle reaches a safe height before processing the goto (otherwise the command
# after Vehicle.simple_takeoff will execute immediately).
while True:
print " Altitude: ", vehicle.location.global_relative_frame.alt
#Break and return from function just below target altitude.
if vehicle.location.global_relative_frame.alt>=99:
print "Reached target altitude"
break
time.sleep(1)
vehicle.mode = VehicleMode("RTL")
print("Completed")