-
Notifications
You must be signed in to change notification settings - Fork 572
/
mavmission.py
executable file
·79 lines (65 loc) · 3.01 KB
/
mavmission.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
#!/usr/bin/env python
'''
extract mavlink mission from log
'''
from __future__ import print_function
from argparse import ArgumentParser
parser = ArgumentParser(description=__doc__)
parser.add_argument("--output", default='mission.txt', help="output file")
parser.add_argument("logs", metavar="LOG", nargs="+")
args = parser.parse_args()
from pymavlink import mavutil, mavwp
parms = {}
def mavmission(logfile):
'''extract mavlink mission'''
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
num_wps = None
while True:
m = mlog.recv_match(type=['MISSION_ITEM','CMD','WAYPOINT','MISSION_ITEM_INT'])
if m is None:
break
if m.get_type() == 'CMD':
try:
frame = m.Frame
except AttributeError:
print("Warning: assuming frame is GLOBAL_RELATIVE_ALT")
frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT
num_wps = m.CTot
m = mavutil.mavlink.MAVLink_mission_item_message(0,
0,
m.CNum,
frame,
m.CId,
0, 1,
m.Prm1, m.Prm2, m.Prm3, m.Prm4,
m.Lat, m.Lng, m.Alt)
if m.get_type() == 'MISSION_ITEM_INT':
m = mavutil.mavlink.MAVLink_mission_item_message(m.target_system,
m.target_component,
m.seq,
m.frame,
m.command,
m.current,
m.autocontinue,
m.param1,
m.param2,
m.param3,
m.param4,
m.x*1.0e-7,
m.y*1.0e-7,
m.z)
if m.current >= 2:
continue
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
if num_wps is not None:
while wp.count() > num_wps:
wp.remove(wp.wp(wp.count() - 1))
wp.save(args.output)
print("Saved %u waypoints to %s" % (wp.count(), args.output))
total = 0.0
for filename in args.logs:
mavmission(filename)