Compare commits
2 commits
bd5129b1fe
...
5993c1e16a
| Author | SHA1 | Date | |
|---|---|---|---|
|
5993c1e16a | ||
|
|
3d699175d5 |
2 changed files with 278 additions and 1 deletions
1
.gitignore
vendored
1
.gitignore
vendored
|
|
@ -1,4 +1,3 @@
|
||||||
data/
|
data/
|
||||||
sandbox.ipynb
|
sandbox.ipynb
|
||||||
thebox.ipynb
|
|
||||||
__pycache__/
|
__pycache__/
|
||||||
278
thebox.ipynb
Normal file
278
thebox.ipynb
Normal file
|
|
@ -0,0 +1,278 @@
|
||||||
|
{
|
||||||
|
"cells": [
|
||||||
|
{
|
||||||
|
"cell_type": "markdown",
|
||||||
|
"id": "0e2149ba",
|
||||||
|
"metadata": {},
|
||||||
|
"source": [
|
||||||
|
"### Failure mode:\n",
|
||||||
|
"The measurements when the device is turned 360° or 180° are not shown on the plot\n",
|
||||||
|
"\n",
|
||||||
|
"### Possible points of failure:\n",
|
||||||
|
"- [X] Parsing the line\n",
|
||||||
|
"- [X] Translating from Stephan's uint to int\n",
|
||||||
|
"- [ ] Sensitivity conversion (Bits to gauss/g)\n",
|
||||||
|
"- [ ] Calibration (Offsets and scale factors)\n",
|
||||||
|
"- [ ] Rotating the vectors from body to world frame\n",
|
||||||
|
"- [ ] Translating Psi to heading (compass 360 vs atan2 -pi/pi)\n",
|
||||||
|
"\n",
|
||||||
|
"### Parsing the line:\n",
|
||||||
|
"Visual inspection: it parses the correct numbers\n",
|
||||||
|
"\n",
|
||||||
|
"### Translating from Stephan's uint to int:\n",
|
||||||
|
"Using online Hex calculator: Numbers are translated correctly\n",
|
||||||
|
"\n",
|
||||||
|
"### Sensitivity Conversion:\n",
|
||||||
|
"It worked in my BA?\n",
|
||||||
|
"\n",
|
||||||
|
"### Calibration:\n",
|
||||||
|
"Don't do the calibration: get only slightly different results (not correct)\n",
|
||||||
|
"\n",
|
||||||
|
"### Rotating vectors from body to world frame:\n",
|
||||||
|
"Don't do the rotation: get only slightly different results (not correct)\n",
|
||||||
|
"\n",
|
||||||
|
"### Translating Psi to heading:\n",
|
||||||
|
"Show heading before and after translation?"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": null,
|
||||||
|
"id": "02d11b53",
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [
|
||||||
|
{
|
||||||
|
"name": "stdout",
|
||||||
|
"output_type": "stream",
|
||||||
|
"text": [
|
||||||
|
"[64, 64, 16640] [2013, 62408, 60868]\n"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"source": [
|
||||||
|
"# The four I2C lines are from thermal vacuum week before and after turning SETH 180°\n",
|
||||||
|
"\n",
|
||||||
|
"#\"I2C-MAG 6 2013 62408 60868\" tvw vor drehen: 283, 76\n",
|
||||||
|
"#\"I2C-MAG 13 64494 62825 60944\" tvw nach drehen: 273, 86\n",
|
||||||
|
"\n",
|
||||||
|
"#\"I2C-ACC 5 64 64 16640\"\n",
|
||||||
|
"#\"I2C-ACC 13 65280 384 16384\"\n",
|
||||||
|
"\n",
|
||||||
|
"acc_line = \"I2C-ACC 5 64 64 16640\"\n",
|
||||||
|
"mag_line = \"I2C-MAG 6 2013 62408 60868\"\n",
|
||||||
|
"\n",
|
||||||
|
"acc_raw = [int(num) for num in acc_line.split()[2:]]\n",
|
||||||
|
"mag_raw = [int(num) for num in mag_line.split()[2:]]\n",
|
||||||
|
"\n",
|
||||||
|
"print(acc_raw, mag_raw)"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 87,
|
||||||
|
"id": "f5e95171",
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [
|
||||||
|
{
|
||||||
|
"name": "stdout",
|
||||||
|
"output_type": "stream",
|
||||||
|
"text": [
|
||||||
|
"[ 64 64 16640] [ 2013 -3128 -4668]\n"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"source": [
|
||||||
|
"import numpy as np\n",
|
||||||
|
"acc = np.array(acc_raw, dtype=np.uint16).astype(np.int16)\n",
|
||||||
|
"mag = np.array(mag_raw, dtype=np.uint16).astype(np.int16)\n",
|
||||||
|
"\n",
|
||||||
|
"print(acc, mag)"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 65,
|
||||||
|
"id": "c740479f",
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"def magnitutde(vector):\n",
|
||||||
|
" sum = 0\n",
|
||||||
|
" for entry in vector:\n",
|
||||||
|
" sum += entry**2\n",
|
||||||
|
" return np.sqrt(sum)"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": null,
|
||||||
|
"id": "8b474a3b",
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [
|
||||||
|
{
|
||||||
|
"name": "stdout",
|
||||||
|
"output_type": "stream",
|
||||||
|
"text": [
|
||||||
|
"[ 0.004 0.004 -1.04 ] [-0.29421222 0.45717626 -0.68225665]\n",
|
||||||
|
"[ 0.0050263 0.01918991 -1.03981812] [-0.32445959 0.68920168 -0.54429259]\n",
|
||||||
|
"1.0400073240162937 0.9362304201894025\n"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"source": [
|
||||||
|
"mag_sens = 6842 # LSB/gauss\n",
|
||||||
|
"acc_sens = 16000 # 1000 LSB/g and device gives only a 12 bit number so divide by 16 (1 hex bit) equivalent to shifting 4 bits\n",
|
||||||
|
"maga, magb, magc, magx0, magy0, magz0 = [1.02305, 1.13811, 1.16184, 0.0265714, -0.188856, -0.177294] # fit coefficients from BA thesis\n",
|
||||||
|
"acca, accb, accc, accx0, accy0, accz0= [0.972148, 0.968608, 0.974061, -0.00109779, -0.0154984, 0.0135722]\n",
|
||||||
|
"\n",
|
||||||
|
"acc = acc/acc_sens\n",
|
||||||
|
"mag = mag/mag_sens\n",
|
||||||
|
"mag[0] = -mag[0]\n",
|
||||||
|
"mag[1] = -mag[1]\n",
|
||||||
|
"acc[2] = -acc[2]\n",
|
||||||
|
"print(acc, mag)\n",
|
||||||
|
"\n",
|
||||||
|
"acc0 = np.array([accx0, accy0, accz0])\n",
|
||||||
|
"mag0 = np.array([magx0, magy0, magz0])\n",
|
||||||
|
"accsc = np.array([np.sqrt(acca), np.sqrt(accb), np.sqrt(accc)])\n",
|
||||||
|
"magsc = np.array([np.sqrt(maga), np.sqrt(magb), np.sqrt(magc)])\n",
|
||||||
|
"acc = (acc-acc0)*accsc\n",
|
||||||
|
"mag = (mag-mag0)*magsc\n",
|
||||||
|
"\n",
|
||||||
|
"print(acc, mag)\n",
|
||||||
|
"print(magnitutde(acc), magnitutde(mag))"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 90,
|
||||||
|
"id": "dba8c2a4",
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"def Rz(phi):\n",
|
||||||
|
" \"\"\"\n",
|
||||||
|
" Rotation matrix of angle phi around z axis\n",
|
||||||
|
" \"\"\"\n",
|
||||||
|
" return np.array([[np.cos(phi), -np.sin(phi), 0],\n",
|
||||||
|
" [np.sin(phi), np.cos(phi), 0],\n",
|
||||||
|
" [0 , 0, 1]])\n",
|
||||||
|
"\n",
|
||||||
|
"def Ry(theta):\n",
|
||||||
|
" \"\"\"\n",
|
||||||
|
" Rotation matrix of angle theta around y axis\n",
|
||||||
|
" \"\"\"\n",
|
||||||
|
" return np.array([[np.cos(theta), 0, np.sin(theta)],\n",
|
||||||
|
" [0, 1, 0],\n",
|
||||||
|
" [-np.sin(theta), 0, np.cos(theta)]])\n"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 91,
|
||||||
|
"id": "745eb63b",
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [
|
||||||
|
{
|
||||||
|
"name": "stdout",
|
||||||
|
"output_type": "stream",
|
||||||
|
"text": [
|
||||||
|
"0.01983724813093511\n",
|
||||||
|
"1.3146270321449216 3.122517354075979 3.122517354075979 0.01983724813093511\n"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"source": [
|
||||||
|
"import math\n",
|
||||||
|
"phi = np.arctan2(acc[1], acc[0])\n",
|
||||||
|
"g_xy = magnitutde([acc[0], acc[1]])\n",
|
||||||
|
"print(g_xy)\n",
|
||||||
|
"thetanp = np.arctan2(g_xy, acc[2])\n",
|
||||||
|
"thetamath = math.atan2(g_xy, acc[2])\n",
|
||||||
|
"pitch = np.arctan2(acc[0], acc[2])\n",
|
||||||
|
"roll = np.arctan2(acc[1], acc[2])\n",
|
||||||
|
"print(phi, thetanp, thetamath, g_xy)"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 89,
|
||||||
|
"id": "f9a28ed4",
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [
|
||||||
|
{
|
||||||
|
"name": "stdout",
|
||||||
|
"output_type": "stream",
|
||||||
|
"text": [
|
||||||
|
"115.209929112543\n"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"source": [
|
||||||
|
"heading = np.arctan2(mag[1], mag[0])\n",
|
||||||
|
"\n",
|
||||||
|
"if heading <= 0:\n",
|
||||||
|
" heading = 2*np.pi + heading\n",
|
||||||
|
" #mag_world_psi = abs(mag_world_psi) # because compass counts clock-wise and atan2 counter CW\n",
|
||||||
|
"else:\n",
|
||||||
|
" pass\n",
|
||||||
|
" #mag_world_psi = 2*np.pi - mag_world_psi\n",
|
||||||
|
"\n",
|
||||||
|
"heading = heading * 180/np.pi\n",
|
||||||
|
"print(heading)"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 92,
|
||||||
|
"id": "0d66b509",
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [
|
||||||
|
{
|
||||||
|
"name": "stdout",
|
||||||
|
"output_type": "stream",
|
||||||
|
"text": [
|
||||||
|
"115.0709015160913\n"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"source": [
|
||||||
|
"mag_world = np.matmul(Rz(-phi), np.matmul(Ry(np.pi-thetanp), np.matmul(Rz(phi), mag)))\n",
|
||||||
|
"mag_world_psi = np.arctan2(mag_world[1], mag_world[0])\n",
|
||||||
|
"\n",
|
||||||
|
"\n",
|
||||||
|
"if mag_world_psi <= 0:\n",
|
||||||
|
" mag_world_psi = 2*np.pi + mag_world_psi\n",
|
||||||
|
" #mag_world_psi = abs(mag_world_psi) # because compass counts clock-wise and atan2 counter CW\n",
|
||||||
|
"else:\n",
|
||||||
|
" pass\n",
|
||||||
|
" #mag_world_psi = 2*np.pi - mag_world_psi\n",
|
||||||
|
"\n",
|
||||||
|
"mag_world_psi = mag_world_psi * 180/np.pi\n",
|
||||||
|
"\n",
|
||||||
|
"print(mag_world_psi)"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"metadata": {
|
||||||
|
"kernelspec": {
|
||||||
|
"display_name": "Python 3",
|
||||||
|
"language": "python",
|
||||||
|
"name": "python3"
|
||||||
|
},
|
||||||
|
"language_info": {
|
||||||
|
"codemirror_mode": {
|
||||||
|
"name": "ipython",
|
||||||
|
"version": 3
|
||||||
|
},
|
||||||
|
"file_extension": ".py",
|
||||||
|
"mimetype": "text/x-python",
|
||||||
|
"name": "python",
|
||||||
|
"nbconvert_exporter": "python",
|
||||||
|
"pygments_lexer": "ipython3",
|
||||||
|
"version": "3.10.12"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"nbformat": 4,
|
||||||
|
"nbformat_minor": 5
|
||||||
|
}
|
||||||
Loading…
Add table
Add a link
Reference in a new issue