From 15c21b139561a5d2fa41af95417c6bc64d0134eb Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 15 Apr 2022 13:41:37 +1000
Subject: [PATCH] Tools: added test script for checking for Fix2 missed frames

used to track down the F405 and L431 lost frames
---
 Tools/scripts/CAN/fix2_gap.py | 44 +++++++++++++++++++++++++++++++++++
 1 file changed, 44 insertions(+)
 create mode 100755 Tools/scripts/CAN/fix2_gap.py

diff --git a/Tools/scripts/CAN/fix2_gap.py b/Tools/scripts/CAN/fix2_gap.py
new file mode 100755
index 0000000000..2fa6011d5e
--- /dev/null
+++ b/Tools/scripts/CAN/fix2_gap.py
@@ -0,0 +1,44 @@
+#!/usr/bin/env python3
+'''
+test script to check if all CAN GPS nodes are producing Fix2 frames at the expected rate
+'''
+
+import dronecan, time
+from dronecan import uavcan
+
+# get command line arguments
+from argparse import ArgumentParser
+parser = ArgumentParser(description='Fix2 gap example')
+parser.add_argument("--bitrate", default=1000000, type=int, help="CAN bit rate")
+parser.add_argument("--node-id", default=100, type=int, help="CAN node ID")
+parser.add_argument("--max-gap", default=0.25, type=int, help="max gap in seconds")
+parser.add_argument("port", default=None, type=str, help="serial port or mavcan URI")
+args = parser.parse_args()
+
+# Initializing a DroneCAN node instance.
+node = dronecan.make_node(args.port, node_id=args.node_id, bitrate=args.bitrate)
+
+# Initializing a node monitor
+node_monitor = dronecan.app.node_monitor.NodeMonitor(node)
+
+last_fix2 = {}
+
+def handle_fix2(msg):
+    nodeid = msg.transfer.source_node_id
+    tstamp = msg.transfer.ts_real
+    if not nodeid in last_fix2:
+        last_fix2[nodeid] = tstamp
+        return
+    dt = tstamp - last_fix2[nodeid]
+    last_fix2[nodeid] = tstamp
+    if dt > args.max_gap:
+        print("Node %u gap=%.3f" % (nodeid, dt))
+
+# callback for printing ESC status message to stdout in human-readable YAML format.
+node.add_handler(dronecan.uavcan.equipment.gnss.Fix2, handle_fix2)
+
+while True:
+    try:
+        node.spin()
+    except Exception as ex:
+        print(ex)