Merge pull request 'heading-bug-development' (#1) from heading-bug-development into master

Reviewed-on: #1

calibrate_ads.py

@@ -0,0 +1,59 @@
+#! /usr/bin/python3
+
+import struct
+import sys
+import numpy as np
+from scipy.optimize import least_squares
+
+
+def magnitutde(vector):
+    sum = 0
+    for entry in vector:
+        sum += entry**2
+    return np.sqrt(sum)
+
+def residuals(coeff, vec, type="mag"):
+    """
+    Function returns the difference between circle and ellipsis
+    """
+    if type == "mag":
+        r = (0.538+0.503)/2 # mean between Kiel and Kiruna
+    else:
+        r = 1
+    x, y, z = vec
+    a, b, c, x0, y0, z0 = coeff
+    return a*(x-x0)*(x-x0) + b*(y-y0)*(y-y0) + c*(z-z0)*(z-z0) - (r*r)
+
+
+def main():
+    mag_sens = 6842 # LSB/gauss
+    acc_sens = 16000 # 1000 LSB/g
+    mags = []
+    accs = []
+    for l in sys.stdin:
+        if l[:3] == "I2C":
+            if l[4:7] == "MAG":
+                mags.append([struct.unpack('<h', struct.pack('<H', int(num)))[0] for num in l.split()[-3:]])
+            if l[4:7] == "ACC":
+                accs.append([struct.unpack('<h', struct.pack('<H', int(num)))[0] for num in l.split()[-3:]])
+    for mag in mags:
+        mag[0] = -mag[0]/mag_sens
+        mag[1] = -mag[1]/mag_sens
+        mag[2] =  mag[2]/mag_sens
+    for acc in accs:
+        acc[0] =  acc[0]/acc_sens
+        acc[1] =  acc[1]/acc_sens
+        acc[2] = -acc[2]/acc_sens
+
+    acc_initial = [1.0, 1.0, 1.0, 0.1, 0.1, 0.1]
+    mag_initial = [1.0, 1.0, 1.0, 0.1, 0.1, 0.1]
+
+    acc_parms = least_squares(residuals, acc_initial, args=(acc, "acc"))
+    mag_parms = least_squares(residuals, mag_initial, args=(mag, "mag"))
+
+    print("Mag [a,b,c,x0,y0,z0]:", [float(a) for a in mag_parms.x])
+    print("Acc [a,b,c,x0,y0,z0]:", [float(a) for a in acc_parms.x])
+
+
+if __name__ == "__main__":
+    main()