Merge branch 'heading-bug-development' of ssh://forge.bexus.org/Seth/seth_tools into heading-bug-development

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()

seth_hk.py

@@ -129,6 +129,11 @@ def hdorn(l):
     return HK_T
 
 def calibrate_vectors(acc_raw, mag_raw):
+    """
+    The uints are turned to ints, then you divide by the sensitivity to get physical units.
+    Alignment with new coordinate system is done where gravity is (0,0,-1).
+    The fit coefficients determined in BA are applied.
+    """
     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_raw[0] = mag_raw[0] + 700 #Values for 360-29e (guessed)
@@ -167,15 +172,22 @@ def Ry(theta):
 
 
 def calculate_heading(mag, phi, theta):
+    """
+    Uses the gravity vector's phi and theta to rotate mag vector in coord system with xy parallel to ground
+    and z pointing to zenith. x axis is direction in which the telescope is looking, mag vector is magnetic north
+    thus, the heading is simply the angle between x and north. Translate this to compass.
+    """
     declination = 0 # Kiruna
     #phi = 0
     theta = 0 # Set zero because it works...
     mag_world = np.matmul(Rz(-phi), np.matmul(Ry(theta-np.pi), np.matmul(Rz(phi), mag)))
     mag_world_psi = np.arctan2(mag_world[1], mag_world[0]) + declination # angle mag vector in wf, x axis in wf + delta because TN is delta left of the MN (=mag vector)
     if mag_world_psi <= 0:
-        mag_world_psi = abs(mag_world_psi) # because compass counts clock-wise and atan2 counter CW
+        mag_world_psi = 2*np.pi + mag_world_psi
+        #mag_world_psi = abs(mag_world_psi) # because compass counts clock-wise and atan2 counter CW
     else:
-        mag_world_psi = 2*np.pi - mag_world_psi
+        pass
+        #mag_world_psi = 2*np.pi - mag_world_psi
     heading = mag_world_psi * 180/np.pi # heading in degrees (0deg = True North)
     return heading
 
@@ -184,8 +196,8 @@ def calculate_ads(acc_raw, mag_raw):
     acc, mag = calibrate_vectors(acc_raw, mag_raw)
     pitch = math.atan(acc[0]/acc[2]) # math package returns in radians
     roll = math.atan(acc[1]/acc[2])
-    phi = math.atan2(acc[1], acc[2])
+    phi = math.atan2(acc[1], acc[0])
-    g_length_xy = math.sqrt(acc[0]**2+acc[1]**2+acc[2]**2)
+    g_length_xy = math.sqrt(acc[0]**2+acc[1]**2)
     theta = math.atan2(g_length_xy, acc[2])
     heading = calculate_heading(mag, phi, theta)
     return heading, roll, pitch