Tools: add quick-and-dirty EKF-status-change dump tool

Tools/scripts/solution_status_change.py

@@ -0,0 +1,97 @@
+#!/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()