new calibration parms for magnetometer

calibrate_ads.py

@@ -12,7 +12,7 @@ def magnitutde(vector):
         sum += entry**2
     return np.sqrt(sum)
 
-def residuals(coeff, vec, type="mag"):
+def residuals(coeff, vecs, type="mag"):
     """
     Function returns the difference between circle and ellipsis
     """
@@ -20,9 +20,15 @@ def residuals(coeff, vec, type="mag"):
         r = (0.538+0.503)/2 # mean between Kiel and Kiruna
     else:
         r = 1
-    x, y, z = vec
+    x, y, z = [], [], []
+    for vec in vecs:
+        x.append(vec[0])
+        y.append(vec[1])
+        z.append(vec[2])
+    x, y, z = np.array(x), np.array(y), np.array(z)
     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)
+    c, z0 = 1, 0
+    return (r*r) - a*(x-x0)*(x-x0) - b*(y-y0)*(y-y0) - c*(z-z0)*(z-z0)
 
 
 def main():
@@ -46,10 +52,11 @@ def main():
         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]
+    mag_initial = [7., 7., 1., 0.05, 0.42, 0.0]
 
-    acc_parms = least_squares(residuals, acc_initial, args=(acc, "acc"))
+
-    mag_parms = least_squares(residuals, mag_initial, args=(mag, "mag"))
+    acc_parms = least_squares(residuals, acc_initial, args=(np.array(accs), "type=acc"), method='lm')
+    mag_parms = least_squares(residuals, mag_initial, args=(np.array(mags), "type=mag"), method='lm')
 
     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])

raw_circle.py

@@ -0,0 +1,56 @@
+#! /usr/bin/python3
+
+import argparse
+import numpy as np
+import struct
+import matplotlib.pyplot as plt
+import math
+
+parser = argparse.ArgumentParser()
+parser.add_argument('-f', '--file')
+args = parser.parse_args()
+fp = args.file
+
+mag_lines = []
+acc_lines = []
+mag_calib = [7., 7., 1., 0.05, 0.42, 0.0]
+mag_sens = 6842 # LSB/gauss
+acc_sens = 16000 # 1000 LSB/g
+
+with open(fp) as f:
+    for l in f:
+        if l[:7] == "I2C-MAG":
+            mag_line = [struct.unpack('<h', struct.pack('<H', int(num)))[0] for num in l.split()[-3:]]
+            mag_line[0] = -mag_line[0]/mag_sens
+            mag_line[1] = -mag_line[1]/mag_sens
+            mag_line[2] =  mag_line[2]/mag_sens
+            mag = [(mag_line[0]-mag_calib[3])*math.sqrt(mag_calib[0]), (mag_line[1]-mag_calib[4])*math.sqrt(mag_calib[1]), (mag_line[2]-mag_calib[5])*math.sqrt(mag_calib[2])]
+            mag_lines.append(mag)
+
+        if l[:7] == "I2C-ACC":
+            acc_line = [struct.unpack('<h', struct.pack('<H', int(num)))[0] for num in l.split()[-3:]]
+            acc_line[0] =  acc_line[0]/acc_sens
+            acc_line[1] =  acc_line[1]/acc_sens
+            acc_line[2] = -acc_line[2]/acc_sens
+            acc_lines.append(acc_line)
+
+fig = plt.figure()
+ax = fig.add_subplot(projection='3d')
+
+xs = []
+ys = []
+zs = []
+
+for vec in mag_lines:
+    xs.append(vec[0])
+    ys.append(vec[1])
+    zs.append(vec[2])
+
+ax.scatter(xs, ys, zs) # from the docs: The z-positions. Either an array of the same length as xs and ys or a single value
+ax.set_xlabel('X in Gs')
+ax.set_ylabel('Y in Gs')
+ax.set_zlabel('Z in Gs')
+
+plt.show()
+
+

seth_hk.py

@@ -136,7 +136,7 @@ def calibrate_vectors(acc_raw, mag_raw):
     """
     mag_sens = 6842 # LSB/gauss
     acc_sens = 16000 # 1000 LSB/g and it is only a 12 bit number so divide by 16 (1hex bit) equivalent to shifting 4 bits
-    mag_calib = [1.02305, 1.13811, 1.16184, 0.0265714, -0.188856, -0.177294] # fit coefficients from BA thesis
+    mag_calib = [37.23933025452744, 41.29799068099641, 1.0, 0.07392279199604498, 0.4336054231239714, 0.0] # fit coefficients from BA thesis
     acc_calib = [0.972148, 0.968608, 0.974061, -0.00109779, -0.0154984, 0.0135722]
     acc_raw = [struct.unpack('<h', struct.pack('<H', int(num)))[0] for num in acc_raw] # turns int into hex, turns hex into signed int
     mag_raw = [struct.unpack('<h', struct.pack('<H', int(num)))[0] for num in mag_raw]