forked from Archive/PX4-Autopilot
DroneKit IT: use new QGC json format
This commit is contained in:
Sander Smeets
Lorenz Meier
parent
53be474191
commit
2c78e9de5d
|
|
@ -28,7 +28,7 @@ alt_acceptance_radius = 5
|
|||
# Import DroneKit-Python
|
||||
from dronekit import connect, Command, VehicleMode
|
||||
from pymavlink import mavutil
|
||||
import time, sys, argparse
|
||||
import time, sys, argparse, json
|
||||
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("-c", "--connect", help="connection string")
|
||||
|
|
@ -80,36 +80,15 @@ print " Alt: %s" % vehicle.location.global_relative_frame.alt
|
|||
# Functions
|
||||
################################################################################################
|
||||
|
||||
|
||||
def readmission(aFileName):
|
||||
"""
|
||||
Load a mission from a file into a list. The mission definition is in the Waypoint file
|
||||
format (http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format).
|
||||
This function is used by upload_mission().
|
||||
"""
|
||||
cmds = vehicle.commands
|
||||
def read_mission_json(f):
|
||||
d = json.load(f)
|
||||
current = True
|
||||
missionlist=[]
|
||||
with open(aFileName) as f:
|
||||
for i, line in enumerate(f):
|
||||
if i==0:
|
||||
if not line.startswith('QGC WPL 110'):
|
||||
raise Exception('File is not supported WP version')
|
||||
else:
|
||||
linearray=line.split('\t')
|
||||
ln_index=int(linearray[0])
|
||||
ln_currentwp=int(linearray[1])
|
||||
ln_frame=int(linearray[2])
|
||||
ln_command=int(linearray[3])
|
||||
ln_param1=float(linearray[4])
|
||||
ln_param2=float(linearray[5])
|
||||
ln_param3=float(linearray[6])
|
||||
ln_param4=float(linearray[7])
|
||||
ln_param5=float(linearray[8])
|
||||
ln_param6=float(linearray[9])
|
||||
ln_param7=float(linearray[10])
|
||||
ln_autocontinue=int(linearray[11].strip())
|
||||
cmd = Command( 0, 0, 0, ln_frame, ln_command, ln_currentwp, ln_autocontinue, ln_param1, ln_param2, ln_param3, ln_param4, ln_param5, ln_param6, ln_param7)
|
||||
for wp in d['items']:
|
||||
cmd = Command( 0, 0, 0, int(wp['frame']), int(wp['command']), current, int(wp['autoContinue']), float(wp['param1']), float(wp['param2']), float(wp['param3']), float(wp['param4']), float(wp['coordinate'][0]), float(wp['coordinate'][1]), float(wp['coordinate'][2]))
|
||||
missionlist.append(cmd)
|
||||
if current:
|
||||
current = False
|
||||
return missionlist
|
||||
|
||||
|
||||
|
|
@ -118,7 +97,8 @@ def upload_mission(aFileName):
|
|||
Upload a mission from a file.
|
||||
"""
|
||||
#Read mission from file
|
||||
missionlist = readmission(aFileName)
|
||||
with open(aFileName) as f:
|
||||
missionlist = read_mission_json(f)
|
||||
|
||||
#Clear existing mission from vehicle
|
||||
cmds = vehicle.commands
|
||||
|
|
@ -131,51 +111,6 @@ def upload_mission(aFileName):
|
|||
return missionlist
|
||||
|
||||
|
||||
def download_mission():
|
||||
"""
|
||||
Downloads the current mission and returns it in a list.
|
||||
It is used in save_mission() to get the file information to save.
|
||||
"""
|
||||
print " Download mission from vehicle"
|
||||
missionlist=[]
|
||||
cmds = vehicle.commands
|
||||
cmds.download()
|
||||
cmds.wait_ready()
|
||||
for cmd in cmds:
|
||||
missionlist.append(cmd)
|
||||
return missionlist
|
||||
|
||||
def save_mission(aFileName):
|
||||
"""
|
||||
Save a mission in the Waypoint file format
|
||||
(http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format).
|
||||
"""
|
||||
print "\nSave mission from Vehicle to file: %s" % aFileName
|
||||
#Download mission from vehicle
|
||||
missionlist = download_mission()
|
||||
#Add file-format information
|
||||
output='QGC WPL 110\n'
|
||||
#Add home location as 0th waypoint
|
||||
home = vehicle.home_location
|
||||
output+="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n\n\n" % (0,1,0,16,0,0,0,0,home.lat,home.lon,home.alt,1)
|
||||
#Add commands
|
||||
for cmd in missionlist:
|
||||
commandline="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n" % (cmd.seq,cmd.current,cmd.frame,cmd.command,cmd.param1,cmd.param2,cmd.param3,cmd.param4,cmd.x,cmd.y,cmd.z,cmd.autocontinue)
|
||||
output+=commandline
|
||||
with open(aFileName, 'w') as file_:
|
||||
print " Write mission to file"
|
||||
file_.write(output)
|
||||
|
||||
|
||||
def printfile(aFileName):
|
||||
"""
|
||||
Print a mission file to demonstrate "round trip"
|
||||
"""
|
||||
print "\nMission file: %s" % aFileName
|
||||
with open(aFileName) as f:
|
||||
for line in f:
|
||||
print ' %s' % line.strip()
|
||||
|
||||
|
||||
################################################################################################
|
||||
# Listeners
|
||||
|
|
|
|||
|
|
@ -1,5 +1,89 @@
|
|||
QGC WPL 110
|
||||
0 0 3 84 0.0 0.0 -0.0 0.0 47.3975105286 8.55026626587 10.0 1
|
||||
1 0 3 16 0.0 0.0 -0.0 0.0 47.395450592 8.55018424988 10.0 1
|
||||
2 0 3 16 0.0 0.0 -0.0 0.0 47.3954582214 8.54566764832 10.0 1
|
||||
3 0 3 85 0.0 0.0 -0.0 0.0 47.3977355957 8.54561138153 10.0 1
|
||||
{
|
||||
"MAV_AUTOPILOT": 12,
|
||||
"complexItems": [
|
||||
],
|
||||
"groundStation": "QGroundControl",
|
||||
"items": [
|
||||
{
|
||||
"autoContinue": true,
|
||||
"command": 84,
|
||||
"coordinate": [
|
||||
47.397510528564453,
|
||||
8.5502662658691406,
|
||||
10
|
||||
],
|
||||
"frame": 3,
|
||||
"id": 1,
|
||||
"param1": 0,
|
||||
"param2": 0,
|
||||
"param3": 0,
|
||||
"param4": 0,
|
||||
"type": "missionItem"
|
||||
},
|
||||
{
|
||||
"autoContinue": true,
|
||||
"command": 16,
|
||||
"coordinate": [
|
||||
47.395450592041016,
|
||||
8.5501842498779297,
|
||||
10
|
||||
],
|
||||
"frame": 3,
|
||||
"id": 2,
|
||||
"param1": 0,
|
||||
"param2": 0,
|
||||
"param3": 0,
|
||||
"param4": 0,
|
||||
"type": "missionItem"
|
||||
},
|
||||
{
|
||||
"autoContinue": true,
|
||||
"command": 16,
|
||||
"coordinate": [
|
||||
47.395420809467687,
|
||||
8.5456138849258423,
|
||||
10
|
||||
],
|
||||
"frame": 3,
|
||||
"id": 3,
|
||||
"param1": 0,
|
||||
"param2": 0,
|
||||
"param3": 0,
|
||||
"param4": 0,
|
||||
"type": "missionItem"
|
||||
},
|
||||
{
|
||||
"autoContinue": true,
|
||||
"command": 85,
|
||||
"coordinate": [
|
||||
47.397735595703125,
|
||||
8.5456113815307617,
|
||||
10
|
||||
],
|
||||
"frame": 3,
|
||||
"id": 4,
|
||||
"param1": 0,
|
||||
"param2": 0,
|
||||
"param3": 0,
|
||||
"param4": 0,
|
||||
"type": "missionItem"
|
||||
}
|
||||
],
|
||||
"plannedHomePosition": {
|
||||
"autoContinue": true,
|
||||
"command": 16,
|
||||
"coordinate": [
|
||||
47.39659309387207,
|
||||
8.5479388236999512,
|
||||
0
|
||||
],
|
||||
"frame": 0,
|
||||
"id": 0,
|
||||
"param1": 0,
|
||||
"param2": 0,
|
||||
"param3": 0,
|
||||
"param4": 0,
|
||||
"type": "missionItem"
|
||||
},
|
||||
"version": "1.0"
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue