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

Compare commits

base: Seth:1e1653bb059ed6dd02864fe0720c26d2e1f57877
Branches Tags
Seth:master
Seth:kiruna
Seth:kiruna-consti
..
compare: Seth:819cc0f7c95040a671a9e00481111ccad4fe29c2
Branches Tags
Seth:master
Seth:kiruna
Seth:kiruna-consti

2 commits
1e1653bb05 ... 819cc0f7c9

Author SHA1 Message Date
Nicolas Rohrbeck
819cc0f7c9 Merge branch 'master' of ssh://forge.bexus.org/Seth/seth_tools 2025-09-02 16:11:39 +02:00
Nicolas Rohrbeck
73df43494b use struct to correctly interpret ADS 2025-09-02 16:11:33 +02:00
1 changed files with 4 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

10
seth_hk.py
View file

@ -8,6 +8,7 @@ import math
import numpy as np import numpy as np
import argparse import argparse
import sys import sys
import struct
parser = argparse.ArgumentParser(prog=sys.argv[0], description="Searches stdin or FILE line by line for P, H and HDORN lines") parser = argparse.ArgumentParser(prog=sys.argv[0], description="Searches stdin or FILE line by line for P, H and HDORN lines")
@ -125,16 +126,13 @@ def hdorn(l):
HK_T.append(degC(d[6])) HK_T.append(degC(d[6]))
return HK_T return HK_T
def plausibility():
return
def calibrate_vectors(acc_raw, mag_raw): def calibrate_vectors(acc_raw, mag_raw):
mag_sens = 6842 # LSB/gauss 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 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 = [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_calib = [0.972148, 0.968608, 0.974061, -0.00109779, -0.0154984, 0.0135722]
acc_raw = [np.int16(np.uint16(num)) for num in acc_raw] 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 = [np.int16(np.uint16(num)) for num in mag_raw] mag_raw = [struct.unpack('<h', struct.pack('<H', int(num)))[0] for num in mag_raw]
acc_raw = [acc_raw[0]/acc_sens, acc_raw[1]/acc_sens, -acc_raw[2]/acc_sens] acc_raw = [acc_raw[0]/acc_sens, acc_raw[1]/acc_sens, -acc_raw[2]/acc_sens]
mag_raw = [-mag_raw[0]/mag_sens, -mag_raw[1]/mag_sens, mag_raw[2]/mag_sens] mag_raw = [-mag_raw[0]/mag_sens, -mag_raw[1]/mag_sens, mag_raw[2]/mag_sens]
acc = [(acc_raw[0]-acc_calib[3])*math.sqrt(acc_calib[0]), (acc_raw[1]-acc_calib[4])*math.sqrt(acc_calib[1]), (acc_raw[2]-acc_calib[5])*math.sqrt(acc_calib[2])] acc = [(acc_raw[0]-acc_calib[3])*math.sqrt(acc_calib[0]), (acc_raw[1]-acc_calib[4])*math.sqrt(acc_calib[1]), (acc_raw[2]-acc_calib[5])*math.sqrt(acc_calib[2])]
@ -161,7 +159,7 @@ def Ry(theta):
def calculate_heading(mag, phi, theta): def calculate_heading(mag, phi, theta):
declination = 0 # Kiruna declination = 0 # Kiruna
mag_world = np.matmul(Rz(-phi), np.matmul(Ry(theta-np.pi), np.matmul(Rz(phi), mag))) mag_world = np.matmul(Rz(-phi), np.matmul(Ry(theta-np.pi), np.matmul(Rz(phi), mag)))
mag_world_psi = float(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) 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: if mag_world_psi <= 0:
mag_world_psi = 2*np.pi + mag_world_psi # because compass counts clock-wise and atan2 counter CW mag_world_psi = 2*np.pi + mag_world_psi # because compass counts clock-wise and atan2 counter CW
else: else: