You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
97 lines
2.6 KiB
97 lines
2.6 KiB
#!/usr/bin/env python |
|
|
|
""" |
|
Parses a log file and shows how the solution status changed over time |
|
|
|
AP_FLAKE8_CLEAN |
|
|
|
""" |
|
|
|
from __future__ import print_function |
|
|
|
import optparse |
|
import sys |
|
import time |
|
|
|
from pymavlink import mavutil |
|
|
|
|
|
class SolutionStatusChange(object): |
|
def __init__(self, master): |
|
self.master = master |
|
|
|
def progress(self, text): |
|
'''emit text with possible timestamps etc''' |
|
print("%u: %s" % (time.time(), text)) |
|
|
|
def run(self): |
|
|
|
self.progress("Creating connection") |
|
self.conn = mavutil.mavlink_connection(master) |
|
|
|
bit_descriptions = { |
|
"attitude": 0, |
|
"horiz_vel": 1, |
|
"vert_vel": 2, |
|
"horiz_pos_rel": 3, |
|
"horiz_pos_abs": 4, |
|
"vert_pos": 5, |
|
"terrain_alt": 6, |
|
"const_pos_mode": 7, |
|
"pred_horiz_pos_rel": 8, |
|
"pred_horiz_pos_abs": 9, |
|
"takeoff_detected": 10, |
|
"takeoff": 11, |
|
"touchdown": 12, |
|
"using_gps": 13, |
|
"gps_glitching": 14, |
|
"gps_quality_good": 15, |
|
"initalized": 16, |
|
} |
|
|
|
desired_type = "XKF4" |
|
old_message_per_core = {} |
|
while True: |
|
m = self.conn.recv_match(type=desired_type) |
|
if m is None: |
|
break |
|
if m.C not in old_message_per_core: |
|
old_message_per_core[m.C] = m |
|
continue |
|
ss = m.SS |
|
current = old_message_per_core[m.C] |
|
if ss == current.SS: |
|
continue |
|
line = "" |
|
for bit in bit_descriptions.keys(): |
|
old_bit_set = current.SS & (1 << bit_descriptions[bit]) |
|
new_bit_set = ss & (1 << bit_descriptions[bit]) |
|
if new_bit_set and not old_bit_set: |
|
line += " +%s" % bit |
|
elif not new_bit_set and old_bit_set: |
|
line += " -%s" % bit |
|
|
|
old_message_per_core[m.C] = m |
|
|
|
timestamp = getattr(m, '_timestamp', 0.0) |
|
formatted_timestamp = "%s.%02u" % ( |
|
time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(timestamp)), |
|
int(timestamp * 100.0) % 100) |
|
|
|
print("%s: C=%u %s" % (formatted_timestamp, m.C, line)) |
|
current = m |
|
|
|
|
|
if __name__ == '__main__': |
|
parser = optparse.OptionParser("solution-status-change.py [options]") |
|
|
|
(opts, args) = parser.parse_args() |
|
|
|
if len(args) < 1: |
|
parser.print_help() |
|
sys.exit(1) |
|
|
|
master = args[0] |
|
|
|
tester = SolutionStatusChange(master) |
|
tester.run()
|
|
|