Tools: add sensor_status_change.py

Tools/scripts/sensor_status_change.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python
+
+"""
+Parses a log file and shows how the SENSOR_STATUS flags changed over time
+
+AP_FLAKE8_CLEAN
+
+"""
+
+from __future__ import print_function
+
+import optparse
+import sys
+import time
+
+from pymavlink import mavutil
+
+
+class SYS_STATUS_Change(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 bit_description(self, bit_number):
+        if 1 << bit_number not in mavutil.mavlink.enums["MAV_SYS_STATUS_SENSOR"]:
+            return "UNKNOWN_BIT[%u]" % bit_number
+
+        name = mavutil.mavlink.enums["MAV_SYS_STATUS_SENSOR"][1 << bit_number].name
+        # return name with common prefix removed:
+        return name[len("MAV_SYS_STATUS_"):]
+
+    def run(self):
+
+        self.progress("Creating connection")
+        self.conn = mavutil.mavlink_connection(master)
+
+        fields = ['present', 'enabled', 'health']
+
+        current = dict()
+        for f in fields:
+            current[f] = 0
+        while True:
+            m = self.conn.recv_match(type="SYS_STATUS")
+            if m is None:
+                break
+
+            line = ""
+            for f in fields:
+                current_values = current[f]
+                new_values = getattr(m, "onboard_control_sensors_" + f)
+                for bit in range(0, 32):
+                    mask = 1 << bit
+                    old_bit_set = current_values & mask
+                    new_bit_set = new_values & mask
+                    if new_bit_set and not old_bit_set:
+                        line += " %s+%s" % (f, self.bit_description(bit))
+                    elif not new_bit_set and old_bit_set:
+                        line += " %s-%s" % (f, self.bit_description(bit))
+                current[f] = new_values
+
+            if len(line) == 0:
+                continue
+
+            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: %s" % (formatted_timestamp, line))
+
+
+if __name__ == '__main__':
+    parser = optparse.OptionParser("sys_status_change.py [options]")
+
+    (opts, args) = parser.parse_args()
+
+    if len(args) < 1:
+        parser.print_help()
+        sys.exit(1)
+
+    master = args[0]
+
+    tester = SYS_STATUS_Change(master)
+    tester.run()