Compute current calibration for RPi HAL based sensing.

The script loads output of "currentcal" command saved
in "curcal-out.dat" file and computes offset
and cross-talk compensation matrix.

Signed-off-by: Pavel Pisa <pisa@cmp.felk.cvut.cz>

diff --git a/tools/datavis/pxmc-spimc-curcal.py b/tools/datavis/pxmc-spimc-curcal.py
new file mode 100755 (executable)
index 0000000..d91107d
--- /dev/null
+++ b/tools/datavis/pxmc-spimc-curcal.py
@@ -0,0 +1,57 @@
+#! /usr/bin/python
+
+import numpy as np
+from numpy import linalg
+import math
+from pylab import plot, subplot, figure, show, ylim, yticks
+from pprint import pprint
+import argparse
+
+if __name__ == '__main__':
+  data_fname = 'curcal-out.dat'
+  data_buff = np.loadtxt(data_fname)
+  zero_rows = 1
+
+  phase_ord = [0, 1, 2]
+
+  pwm_val = data_buff[:, np.multiply(phase_ord, 2)]
+  pwm_en  = data_buff[:, np.multiply(phase_ord, 2) + 1]
+  cur_raw = data_buff[:, [6, 7, 8]]
+
+  cur_offs  = cur_raw[0, :]
+  cur_zerob = cur_raw[zero_rows:, :] - cur_offs
+
+  rows = cur_zerob.shape[0]
+  cur_resp = np.ndarray(shape=(rows / 2, 3))
+  cur_resid = np.ndarray(shape=(rows, 3))
+  cur_refall = np.ndarray(shape=(rows, 3))
+  cur_ref = np.ndarray(shape=(rows / 2, 3))
+
+  for i in range(0, rows):
+    pwmen = pwm_en[zero_rows + i, :]
+    pwmv = np.multiply(pwm_val[zero_rows + i, :], pwmen)
+    pwmcent = np.sum(pwmv) / np.sum(pwmen)
+    pwmv = np.multiply(pwmv - pwmcent, pwmen)
+    cur_refall[i, :] = pwmv
+
+  for i in range(0, rows / 2):
+    cur_resp[i, :] = (cur_zerob[2 * i, :] - cur_zerob[2 * i + 1, :]) / 2.0
+    cur_resid[2 * i, :] = cur_zerob[2 * i, :] - cur_resp[i, :]
+    cur_resid[2 * i + 1, :] = cur_zerob[2 * i + 1, :] + cur_resp[i, :]
+    cur_ref[i, :] = (cur_refall[2 * i, :] - cur_refall[2 * i + 1, :]) / 2.0
+
+  print cur_zerob
+  print cur_resid
+  #print cur_resp
+  #print cur_ref
+
+  Y = cur_ref.T
+  U = cur_resp.T
+  C = np.dot(Y, linalg.inv(U))
+
+  print C
+  print Y
+  print np.dot(C, U)
+  for i in range(0, rows):
+    #print cur_zerob[i, :]
+    print np.dot(C, cur_zerob[i, :].T)