Permalink
Browse files

DroneKit IT: use new QGC json format

  • Loading branch information...
1 parent 53be474 commit 2c78e9de5daecf1b85d19d30ef36c67c279c965f @sanderux sanderux committed with LorenzMeier Dec 28, 2016
@@ -28,7 +28,7 @@
# 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 @@
# 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)
- missionlist.append(cmd)
+ 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"
+}

0 comments on commit 2c78e9d

Please sign in to comment.