From be5eb100fcf01374208efbec7d1a1d85460a6b2f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 18 Sep 2023 15:58:26 +1000
Subject: [PATCH] tools: added extract_batch_imu.py

---
 tools/extract_batch_imu.py | 65 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 65 insertions(+)
 create mode 100755 tools/extract_batch_imu.py

diff --git a/tools/extract_batch_imu.py b/tools/extract_batch_imu.py
new file mode 100755
index 000000000..979f6f793
--- /dev/null
+++ b/tools/extract_batch_imu.py
@@ -0,0 +1,65 @@
+#!/usr/bin/env python3
+
+'''
+extract ISBH and ISBD messages from AP_Logging files into a bin GYR and ACC log
+'''
+from pymavlink import DFReader
+from argparse import ArgumentParser
+
+parser = ArgumentParser('Parse bin log into GYR+ACC bin log')
+parser.add_argument("inlog")
+parser.add_argument("outlog")
+
+args = parser.parse_args()
+
+from pymavlink import mavutil
+
+class DF_logger:
+    '''write to a DF format log'''
+    def __init__(self, filename):
+        self.outf = open(filename,'wb')
+        self.outf.write(bytes([0]))
+        self.outf.flush()
+        self.mlog = DFReader.DFReader_binary(filename)
+        self.outf.seek(0)
+        self.formats = {}
+
+    def write(self, name, fmt, fields, *args):
+        if not name in self.formats:
+            self.formats[name] = self.mlog.add_format(DFReader.DFFormat(0, name, 0, fmt, fields))
+            self.outf.write(self.mlog.make_format_msgbuf(self.formats[name]))
+        self.outf.write(self.mlog.make_msgbuf(self.formats[name], args))
+        self.outf.flush()
+
+def extract_batch(inlog, outlog):
+    print("Processing log %s" % inlog)
+    mlog = mavutil.mavlink_connection(inlog)
+    dflog = DF_logger(outlog)
+
+    ISBH = None
+    count = 0
+
+    while True:
+        m = mlog.recv_match(type=['ISBH','ISBD'])
+        if m is None:
+            break
+        msg_type = m.get_type()
+        if msg_type == "ISBH":
+            ISBH = m
+            count += 1
+        elif msg_type == "ISBD" and ISBH is not None:
+            if m.N != ISBH.N:
+                continue
+            stype = 'ACC' if ISBH.type == 0 else 'GYR'
+            stype += '%u' % ISBH.instance
+            sample_us = int(1.0e6 / ISBH.smp_rate)
+            tbase = ISBH.SampleUS + (m.seqno * 32) * sample_us
+            mul = 1.0/ISBH.mul
+            for i in range(32):
+                dflog.write(stype, 'Qfff', 'TimeUS,X,Y,Z',
+                            tbase+i*sample_us,
+                            m.x[i]*mul, m.y[i]*mul, m.z[i]*mul)
+
+    print("Extracted %u frames" % count)
+extract_batch(args.inlog, args.outlog)
+