From 7c3e614c19a76913c2be57bd26d4b007ddf6c0b1 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Tue, 23 Sep 2014 21:56:48 -0700
Subject: [PATCH] LogAnalyzer: Add IMU Mismatch check

---
 Tools/LogAnalyzer/tests/TestIMUMatch.py | 105 ++++++++++++++++++++++++
 1 file changed, 105 insertions(+)
 create mode 100644 Tools/LogAnalyzer/tests/TestIMUMatch.py

diff --git a/Tools/LogAnalyzer/tests/TestIMUMatch.py b/Tools/LogAnalyzer/tests/TestIMUMatch.py
new file mode 100644
index 0000000000..40dada9844
--- /dev/null
+++ b/Tools/LogAnalyzer/tests/TestIMUMatch.py
@@ -0,0 +1,105 @@
+from LogAnalyzer import Test,TestResult
+import DataflashLog
+from math import sqrt
+
+
+class TestIMUMatch(Test):
+    '''test for empty or near-empty logs'''
+
+    def __init__(self):
+        Test.__init__(self)
+        self.name = "IMU Mismatch"
+
+    def run(self, logdata, verbose):
+
+        #tuning parameters:
+        warn_threshold = .75
+        fail_threshold = 1.5
+        filter_tc = 5.0
+
+        self.result = TestResult()
+        self.result.status = TestResult.StatusType.GOOD
+
+        if ("IMU" in logdata.channels) and (not "IMU2" in logdata.channels):
+            self.result.status = TestResult.StatusType.NA
+            self.result.statusMessage = "No IMU2"
+            return
+
+        if (not "IMU" in logdata.channels) or (not "IMU2" in logdata.channels):
+            self.result.status = TestResult.StatusType.UNKNOWN
+            self.result.statusMessage = "No IMU log data"
+            return
+
+        imu1 = logdata.channels["IMU"]
+        imu2 = logdata.channels["IMU2"]
+
+        imu1_timems = imu1["TimeMS"].listData
+        imu1_accx = imu1["AccX"].listData
+        imu1_accy = imu1["AccY"].listData
+        imu1_accz = imu1["AccZ"].listData
+
+        imu2_timems = imu2["TimeMS"].listData
+        imu2_accx = imu2["AccX"].listData
+        imu2_accy = imu2["AccY"].listData
+        imu2_accz = imu2["AccZ"].listData
+
+        imu1 = []
+        imu2 = []
+
+        for i in range(len(imu1_timems)):
+            imu1.append({ 't': imu1_timems[i][1]*1.0E-3, 'x': imu1_accx[i][1], 'y': imu1_accy[i][1], 'z': imu1_accz[i][1]})
+
+        for i in range(len(imu2_timems)):
+            imu2.append({ 't': imu2_timems[i][1]*1.0E-3, 'x': imu2_accx[i][1], 'y': imu2_accy[i][1], 'z': imu2_accz[i][1]})
+
+        imu1.sort(key=lambda x: x['t'])
+        imu2.sort(key=lambda x: x['t'])
+
+        imu2_index = 0
+
+        last_t = None
+
+        xdiff_filtered = 0
+        ydiff_filtered = 0
+        zdiff_filtered = 0
+        max_diff_filtered = 0
+
+        for i in range(len(imu1)):
+            #find closest imu2 value
+            t = imu1[i]['t']
+            dt = 0 if last_t is None else t-last_t
+            dt=min(dt,.1)
+
+            next_imu2 = None
+            for i in range(imu2_index,len(imu2)):
+                next_imu2 = imu2[i]
+                imu2_index=i
+                if next_imu2['t'] >= t:
+                    break
+            prev_imu2 = imu2[imu2_index-1]
+            closest_imu2 = next_imu2 if abs(next_imu2['t']-t)<abs(prev_imu2['t']-t) else prev_imu2
+
+            xdiff = imu1[i]['x']-closest_imu2['x']
+            ydiff = imu1[i]['y']-closest_imu2['y']
+            zdiff = imu1[i]['z']-closest_imu2['z']
+
+            xdiff_filtered += (xdiff-xdiff_filtered)*dt/filter_tc
+            ydiff_filtered += (ydiff-ydiff_filtered)*dt/filter_tc
+            zdiff_filtered += (zdiff-zdiff_filtered)*dt/filter_tc
+
+            diff_filtered = sqrt(xdiff_filtered**2+ydiff_filtered**2+zdiff_filtered**2)
+            max_diff_filtered = max(max_diff_filtered,diff_filtered)
+            #print max_diff_filtered
+            last_t = t
+
+
+        if max_diff_filtered > fail_threshold:
+            self.result.statusMessage = "Check vibration or accelerometer calibration. (Mismatch: %.2f, WARN: %.2f, FAIL: %.2f)" % (max_diff_filtered,warn_threshold,fail_threshold)
+            self.result.status = TestResult.StatusType.FAIL
+        elif max_diff_filtered > warn_threshold:
+            self.result.statusMessage = "Check vibration or accelerometer calibration. (Mismatch: %.2f, WARN: %.2f, FAIL: %.2f)" % (max_diff_filtered,warn_threshold,fail_threshold)
+            self.result.status = TestResult.StatusType.WARN
+        else:
+            self.result.statusMessage = "(Mismatch: %.2f, WARN: %.2f, FAIL: %.2f)" % (max_diff_filtered,warn_threshold, fail_threshold)
+
+