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 0 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files
Showing only changes of commit 0da36eb56e - Show all commits

remove quick and dirty

Prev Next
Nicolas Rohrbeck 2025-09-29 10:50:19 +02:00
commit 0da36eb56e

10
seth_hk.py
View file

@ -136,14 +136,6 @@ 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
@ -178,8 +170,6 @@ def calculate_heading(mag, phi, theta):
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: