Seth/seth_tools
9
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

heading-bug-development #1

Merged
nicolas merged 4 commits from heading-bug-development into master 2025-09-29 10:51:41 +02:00
Conversation 0 Commits 4 Files changed 1 +10
1 changed files with 10 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files
Showing only changes of commit 35dafabebf - Show all commits

Possible solution for the heading bug

Prev Next
Constantin 2025-09-26 13:09:56 +02:00
commit 35dafabebf

10
seth_hk.py
View file

@ -131,6 +131,14 @@ def hdorn(l):
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_raw[0] = mag_raw[0] + 700 #Values for 360-29e (guessed)
mag_raw[1] = mag_raw[1] +4000 # For tvw-36e use -500 and +2500
if mag_raw[0] < 0:
mag_raw[0] = mag_raw[0]+65535
if mag_raw[0] > 65535:
mag_raw[0] = mag_raw[0]-65535
if mag_raw[1] > 65535:
mag_raw[1] = mag_raw[1]-65535
mag_calib = [1.02305, 1.13811, 1.16184, 0.0265714, -0.188856, -0.177294] # 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
@ -160,6 +168,8 @@ def Ry(theta):
def calculate_heading(mag, phi, theta):
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: