calibrate_ads.py

@@ -1,59 +0,0 @@
-#! /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()