-
Notifications
You must be signed in to change notification settings - Fork 0
/
mission.py
40 lines (33 loc) · 863 Bytes
/
mission.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
def save_mission(vehicle):
"""
Save a mission in the Waypoint file format
(http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format).
"""
missionlist=[]
cmds = vehicle.commands
cmds.download()
cmds.wait_ready()
#output='QGC WPL 110\n'
for cmd in cmds:
missionlist.append(cmd)
#Add file-format information
#Add home location as 0th waypoint
waypoint={}
home=vehicle.home_location
#print(home.lat,home.lon,home.alt)
waypoint[0]={
'lat':home.lat,
'lng':home.lon,
}
#Add commands
inc=1
for cmd in missionlist:
if cmd.command!=22:
waypoint[inc]= {
'lat': cmd.x,
'lng': cmd.y,
'alt': cmd.z,
'command': cmd.command
}
inc=inc+1
return waypoint