├── tests
├── __init__.py
├── test_imu_stream.py
├── README.md
└── test_build_cmd.py
├── iotools
├── __init__.py
├── config
│ ├── response_commands.json
│ └── data_commands.json
├── build_cmd.py
└── imu_stream.py
├── Rotating IMU.png
├── environment.yml
├── .vscode
└── settings.json
├── setup.py
├── README.md
├── .github
└── workflows
│ └── ci.yml
├── .gitignore
├── executables
└── data_collection.py
└── trajectory
└── rmx.py
/tests/__init__.py:
--------------------------------------------------------------------------------
1 |
--------------------------------------------------------------------------------
/iotools/__init__.py:
--------------------------------------------------------------------------------
1 | from .imu_stream import IMU
2 |
3 |
4 |
--------------------------------------------------------------------------------
/tests/test_imu_stream.py:
--------------------------------------------------------------------------------
1 | #TODO: Add tests
2 | # use mock up function for serial object
--------------------------------------------------------------------------------
/Rotating IMU.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/marcus-24/IMU-Tracking/HEAD/Rotating IMU.png
--------------------------------------------------------------------------------
/environment.yml:
--------------------------------------------------------------------------------
1 | name: imu_env
2 | channels:
3 | - conda-forge
4 | - defaults
5 | dependencies:
6 | - jupyter
7 | - matplotlib
8 | - numpy
9 | - pandas
10 | - pip
11 | - pycodestyle
12 | - pylint
13 | - pyserial=3.4
14 | - python=3.10
15 | - pip:
16 | - -e .
--------------------------------------------------------------------------------
/tests/README.md:
--------------------------------------------------------------------------------
1 | # Unit tests
2 | These unit tests are executed in the GitHub Actions workflow (.github/workflows/ci.yml) for each push and pull request. If you would like to execute them manually, you can run the following command in the tests folder:
3 |
4 | `python -m unittest discover`
5 |
--------------------------------------------------------------------------------
/.vscode/settings.json:
--------------------------------------------------------------------------------
1 | {
2 | "python.linting.pylintEnabled": true,
3 | "python.linting.enabled": true,
4 | "python.linting.pycodestyleEnabled": true,
5 | "python.linting.pylintArgs": [
6 | "--max-line-length=120",
7 | "--generate-members"
8 | ],
9 | "python.linting.pycodestyleArgs":[
10 | "--max-line-length=120",
11 | ],
12 | "python.testing.unittestArgs": [
13 | "-v",
14 | "-s",
15 | "./tests",
16 | "-p",
17 | "*test*.py"
18 | ],
19 | "python.testing.pytestEnabled": false,
20 | "python.testing.unittestEnabled": true
21 | }
--------------------------------------------------------------------------------
/setup.py:
--------------------------------------------------------------------------------
1 | import os
2 | from setuptools import setup
3 |
4 |
5 | def read(fname: str) -> str:
6 | """Reads README file
7 | Args:
8 | fname (str): path to readme file
9 | Returns:
10 | str: contents in readme
11 | """
12 | full_fname = os.path.join(os.path.dirname(__file__), fname)
13 | with open(full_fname, encoding="utf-8") as file:
14 | return file.read()
15 |
16 |
17 | setup(
18 | name="IMU-Tracking",
19 | version="0.0.1",
20 | author="Marcus Allen",
21 | author_email="marcusCallen24@gmail.com",
22 | py_modules=[],
23 | long_description=read('README.md')
24 | )
25 |
--------------------------------------------------------------------------------
/iotools/config/response_commands.json:
--------------------------------------------------------------------------------
1 | {
2 | "success bit":{
3 | "command": 1,
4 | "raw length": 1
5 | },
6 |
7 | "timestamp":{
8 | "command": 2,
9 | "raw length": 4,
10 | "unpack": ">I"
11 | },
12 |
13 | "command echo":{
14 | "command": 4,
15 | "raw length": 1
16 | },
17 |
18 | "additive checksum":{
19 | "command": 8,
20 | "raw length": 1
21 | },
22 |
23 | "logical id":{
24 | "command": 16,
25 | "raw length": 1
26 | },
27 |
28 | "serial number":{
29 | "command": 32,
30 | "raw length": 4
31 | },
32 |
33 | "data length":{
34 | "command": 64,
35 | "raw length": 1
36 | }
37 | }
--------------------------------------------------------------------------------
/iotools/config/data_commands.json:
--------------------------------------------------------------------------------
1 | {
2 | "button state":{
3 | "command": 250,
4 | "return length": 1,
5 | "raw length": 1
6 | },
7 |
8 | "gyroscope":{
9 | "command": 38,
10 | "unpack": ">3f",
11 | "return length": 3,
12 | "raw length": 12
13 | },
14 |
15 | "accelerometer":{
16 | "command": 39,
17 | "unpack": ">3f",
18 | "return length": 3,
19 | "raw length": 12
20 | },
21 |
22 | "magnetometer":{
23 | "command": 67,
24 | "unpack": "f",
25 | "return length": 1,
26 | "raw length": 4
27 | },
28 |
29 | "magnetometer norm":{
30 | "command": 35,
31 | "unpack": ">3f",
32 | "return length": 3,
33 | "raw length": 12
34 | },
35 |
36 | "temperature C":{
37 | "command": 43,
38 | "unpack": "f",
39 | "return length": 1,
40 | "raw length": 4
41 | },
42 |
43 | "quaternions":{
44 | "command": 0,
45 | "unpack": ">4f",
46 | "return length": 4,
47 | "raw length": 16
48 | },
49 |
50 | "euler angles":{
51 | "command": 1,
52 | "unpack": ">3f",
53 | "return length": 3,
54 | "raw length": 12
55 | },
56 |
57 | "battery voltage":{
58 | "command": 201,
59 | "unpack": "f",
60 | "return length": 1,
61 | "raw length": 4
62 | }
63 | }
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # IMU-Tracking
2 | 
3 | ## Objective
4 | This repository focuses on demonstrating techniques to track kinematics from inertial measurement units (IMUs).
5 |
6 | ## Setup
7 | To keep the python modules consistent between users, the `imu_env` python environment can be downloaded via conda using the following command:
8 |
9 | `conda env create -f environment.yml`
10 |
11 | This installs the python modules from the conda channels and the local modules (i.e.: iotools) from the setup.py file.
12 |
13 | ## IMU Data Collection
14 |
15 | The plot shown below was generated using the "executables/data_collection.py" script while an IMU was placed on a rotating stool for 20 seconds.
16 |
17 | 
18 |
19 |
20 | The end goal will be to generate the cartesian trajectory of the IMU relative to the global earth frame and minimize the sensor drift and noise as much as possible. State estimation algorithms such as the Extended Kalman Filter can be used to reduce the trajectory error.
21 |
22 | More to come soon!
23 |
24 | ## References
25 |
26 | - Yost Labs Bluetooth Manual: https://yostlabs.com/wp/wp-content/uploads/pdf/3-Space-Sensor-Users-Manual-Bluetooth-1.pdf
27 |
--------------------------------------------------------------------------------
/.github/workflows/ci.yml:
--------------------------------------------------------------------------------
1 | name: ci
2 | # reference: https://dev.to/epassaro/caching-anaconda-environments-in-github-actions-5hde
3 | on:
4 | push:
5 | branches:
6 | - '*'
7 |
8 | pull_request:
9 | branches:
10 | - '*'
11 |
12 | schedule:
13 | - cron: '0 0 * * *'
14 |
15 | env:
16 | CACHE_NUMBER: 0 # increase to reset cache manually
17 |
18 | jobs:
19 | build:
20 |
21 | strategy:
22 | matrix:
23 | include:
24 | - os: ubuntu-latest
25 | label: linux-64
26 | prefix: /usr/share/miniconda3/envs/my-env
27 |
28 | - os: macos-latest
29 | label: osx-64
30 | prefix: /Users/runner/miniconda3/envs/my-env
31 |
32 | - os: windows-latest
33 | label: win-64
34 | prefix: C:\Miniconda3\envs\my-env
35 |
36 | name: ${{ matrix.label }}
37 | runs-on: ${{ matrix.os }}
38 | steps:
39 | - uses: actions/checkout@v2
40 |
41 | - name: Setup Mambaforge
42 | uses: conda-incubator/setup-miniconda@v2
43 | with:
44 | miniforge-variant: Mambaforge
45 | miniforge-version: latest
46 | activate-environment: my-env
47 | use-mamba: true
48 |
49 | - name: Set cache date
50 | run: echo "DATE=$(date +'%Y%m%d')" >> $GITHUB_ENV
51 |
52 | - uses: actions/cache@v2
53 | with:
54 | path: ${{ matrix.prefix }}
55 | key: ${{ matrix.label }}-conda-${{ hashFiles('environment.yml') }}-${{ env.DATE }}-${{ env.CACHE_NUMBER }}
56 | id: cache
57 |
58 | - name: Update environment
59 | run: mamba env update -n my-env -f environment.yml
60 | if: steps.cache.outputs.cache-hit != 'true'
61 |
62 | - name: Run tests
63 | shell: bash -l {0} # adds name to bash command
64 | run: python -m unittest discover ./tests
--------------------------------------------------------------------------------
/tests/test_build_cmd.py:
--------------------------------------------------------------------------------
1 | # standard imports
2 | import unittest
3 | from struct import pack
4 |
5 | # local imports
6 | from iotools.build_cmd import BuildCommands, DATA_CMDS, RESP_CMDS, SET_RESP_HEAD, SET_SLOT
7 |
8 |
9 | class BuildCmdTests(unittest.TestCase):
10 |
11 | def setUp(self) -> None:
12 | self.bldg_cmd = BuildCommands()
13 |
14 | def test_all_responses(self):
15 | self.bldg_cmd.resp_cmds = RESP_CMDS
16 | response_cmd_total = 127 # sum of all the response commands
17 | self.assertEqual(pack('B', SET_RESP_HEAD) + pack('>I', response_cmd_total),
18 | self.bldg_cmd.pack_response_header())
19 |
20 | def test_invalid_response(self):
21 | resp_cmd = {'fake response': {"command": 900,
22 | "unpack": ">3f",
23 | "raw length": 12}
24 | }
25 |
26 | with self.assertRaises(ValueError):
27 | self.bldg_cmd.resp_cmds = resp_cmd
28 |
29 | def test_fill_all_streaming_slots_with_commands(self):
30 | '''Set up data commands'''
31 | n_cmds = 8 # number of commands to extract
32 | eight_cmds = {key: value for idx, (key, value) in enumerate(DATA_CMDS.items()) if idx < n_cmds}
33 |
34 | '''Set hex commands'''
35 | hex_cmds = [SET_SLOT] + [cmd["command"] for _, cmd in eight_cmds.items()]
36 | self.bldg_cmd.data_cmds = eight_cmds
37 |
38 | self.assertEqual(self.bldg_cmd.pack_data_cmds(),
39 | pack('BBBBBBBBB', *hex_cmds))
40 |
41 | def test_invalid_data_command(self):
42 | data_cmd = {'fake sensor': {"command": 1020,
43 | "unpack": ">3h",
44 | "return length": 8,
45 | "raw length": 15}
46 | }
47 | with self.assertRaises(ValueError):
48 | self.bldg_cmd.data_cmds = data_cmd
49 |
50 | def test_too_many_streaming_slots(self):
51 |
52 | with self.assertRaises(ValueError):
53 | self.bldg_cmd.data_cmds = DATA_CMDS
54 |
55 |
56 | if __name__ == '__main__':
57 | unittest.main()
--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | # Byte-compiled / optimized / DLL files
2 | __pycache__/
3 | *.py[cod]
4 | *$py.class
5 |
6 | # C extensions
7 | *.so
8 |
9 | # Distribution / packaging
10 | .Python
11 | build/
12 | develop-eggs/
13 | dist/
14 | downloads/
15 | eggs/
16 | .eggs/
17 | lib/
18 | lib64/
19 | parts/
20 | sdist/
21 | var/
22 | wheels/
23 | pip-wheel-metadata/
24 | share/python-wheels/
25 | *.egg-info/
26 | .installed.cfg
27 | *.egg
28 | MANIFEST
29 |
30 | # PyInstaller
31 | # Usually these files are written by a python script from a template
32 | # before PyInstaller builds the exe, so as to inject date/other infos into it.
33 | *.manifest
34 | *.spec
35 |
36 | # Installer logs
37 | pip-log.txt
38 | pip-delete-this-directory.txt
39 |
40 | # Unit test / coverage reports
41 | htmlcov/
42 | .tox/
43 | .nox/
44 | .coverage
45 | .coverage.*
46 | .cache
47 | nosetests.xml
48 | coverage.xml
49 | *.cover
50 | *.py,cover
51 | .hypothesis/
52 | .pytest_cache/
53 |
54 | # Translations
55 | *.mo
56 | *.pot
57 |
58 | # Django stuff:
59 | *.log
60 | local_settings.py
61 | db.sqlite3
62 | db.sqlite3-journal
63 |
64 | # Flask stuff:
65 | instance/
66 | .webassets-cache
67 |
68 | # Scrapy stuff:
69 | .scrapy
70 |
71 | # Sphinx documentation
72 | docs/_build/
73 |
74 | # PyBuilder
75 | target/
76 |
77 | # Jupyter Notebook
78 | .ipynb_checkpoints
79 |
80 | # IPython
81 | profile_default/
82 | ipython_config.py
83 |
84 | # pyenv
85 | .python-version
86 |
87 | # pipenv
88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies
90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not
91 | # install all needed dependencies.
92 | #Pipfile.lock
93 |
94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow
95 | __pypackages__/
96 |
97 | # Celery stuff
98 | celerybeat-schedule
99 | celerybeat.pid
100 |
101 | # SageMath parsed files
102 | *.sage.py
103 |
104 | # Environments
105 | .env
106 | .venv
107 | env/
108 | venv/
109 | ENV/
110 | env.bak/
111 | venv.bak/
112 |
113 | # Spyder project settings
114 | .spyderproject
115 | .spyproject
116 |
117 | # Rope project settings
118 | .ropeproject
119 |
120 | # mkdocs documentation
121 | /site
122 |
123 | # mypy
124 | .mypy_cache/
125 | .dmypy.json
126 | dmypy.json
127 |
128 | # Pyre type checker
129 | .pyre/
130 |
131 | # data
132 | /data
133 |
--------------------------------------------------------------------------------
/executables/data_collection.py:
--------------------------------------------------------------------------------
1 | # %%
2 | # standard imports
3 | import numpy as np
4 | import time
5 | import matplotlib.pyplot as plt
6 | import os
7 | import serial
8 |
9 | # local imports
10 | from iotools import IMU
11 |
12 | # %% Code Summary
13 | """This script is an example for how to collect data from a Yost labs
14 | bluetooth IMU sensor and display the raw data from each sensor.
15 | """
16 |
17 | # %% Initialize IMU communication
18 | '''stream timing settings (microseconds)'''
19 | interval = 10**4 # sample interval
20 | duration = 20*10**6 # recording duration
21 | delay = 1000 # delay before recording starts
22 | baudrate = 115200
23 | port = 'COM4'
24 |
25 | '''Set serial port for IMU'''
26 | imu_conn = serial.Serial(port, baudrate)
27 |
28 | # %% Data Collection
29 | start_time = time.perf_counter()
30 | end_time = time.perf_counter()
31 | row = 0 # iterate through IMU data array
32 |
33 | with imu_conn as ser:
34 |
35 | my_imu = IMU(ser) # initialize IMU connection
36 |
37 | # %% Preallocate data
38 | data_len = int(duration / interval) # length of the data array
39 | n_pts = 14 # number of points collected from each imu TODO: use length from IMU object
40 | data = np.zeros((data_len, n_pts)) # IMU data array
41 |
42 | '''Start streaming data'''
43 | my_imu.set_stream(interval, duration, delay) # set timing parameters set above
44 | my_imu.start_streaming()
45 |
46 | '''Collect data from IMU'''
47 | while (end_time - start_time) < duration * 10 ** -6: # run while run time is below "duration" set
48 |
49 | data[row, :] = my_imu.get_data()
50 | row += 1
51 | end_time = time.perf_counter() # update end time
52 |
53 | '''Stop streaming'''
54 | my_imu.stop_streaming()
55 | my_imu.software_reset()
56 |
57 | # %% Save data
58 | # TODO: Make data a dataframe so that each column has a title. Can use sensor names in build_cmd object
59 | data = data[data[:, 1] > 0, :] # Truncate zeros
60 | np.savetxt(os.path.join('data', 'data.csv'), data, delimiter=",")
61 |
62 | # %% Plot Data
63 | time_array = [(timestamp - data[0, 1]) * 10 ** -6 for timestamp in data[:, 1]] # test time in seconds
64 |
65 | plt.figure('IMU Sensors')
66 | plt.subplot(311)
67 | plt.title('Gyroscope')
68 | plt.plot(time_array, data[:, 4:7])
69 | plt.grid()
70 | plt.ylabel('rad/s')
71 | plt.legend(['X', 'Y', 'Z'])
72 |
73 | plt.subplot(312)
74 | plt.title('Accelerometer')
75 | plt.plot(time_array, data[:, 7:10])
76 | plt.grid()
77 | plt.ylabel('G')
78 | plt.legend(['X', 'Y', 'Z'])
79 |
80 | plt.subplot(313)
81 | plt.title('Magnetometer')
82 | plt.plot(time_array, data[:, 10:13])
83 | plt.grid()
84 | plt.ylabel('Norm Gauss')
85 | plt.legend(['X', 'Y', 'Z'])
86 |
87 | plt.xlabel('Time (sec)')
88 |
89 | plt.show()
90 |
--------------------------------------------------------------------------------
/trajectory/rmx.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | import math as mt
3 | import numpy.linalg as mx
4 | from typing import List
5 |
6 |
7 | def initial_quaternion(acc_avg: np.ndarray, psi: float) -> List[float]:
8 | """Calculate intial quaternion
9 | Args:
10 | acc_avg (np.ndarray): average accelerometer values
11 | psi (float): psi angle
12 | Returns:
13 | List[float]: inital quaternion vector
14 | """
15 |
16 | '''Compile accelerometer values'''
17 | ax_avg = -acc_avg[0]
18 | ay_avg = -acc_avg[1]
19 | az_avg = -acc_avg[2]
20 |
21 | '''Get initial euler angles'''
22 | theta = mt.atan(ax_avg / mt.sqrt((ay_avg**2) + (az_avg**2)))
23 |
24 | phi = mt.atan(ay_avg / az_avg)
25 | if az_avg > 0:
26 | if ay_avg > 0:
27 | phi -= mt.pi
28 | elif ay_avg < 0:
29 | phi += mt.pi
30 |
31 | '''Calculate initial quaternion'''
32 | q0_int = mt.cos(psi / 2) * mt.cos(theta / 2) * mt.cos(phi / 2) + mt.sin(psi / 2) * mt.sin(theta / 2) * mt.sin(phi / 2)
33 | q1_int = mt.cos(psi / 2) * mt.cos(theta / 2) * mt.sin(phi / 2) - mt.sin(psi / 2) * mt.sin(theta / 2) * mt.cos(phi / 2)
34 | q2_int = mt.cos(psi / 2) * mt.sin(theta / 2) * mt.cos(phi / 2) + mt.sin(psi / 2) * mt.cos(theta / 2) * mt.sin(phi / 2)
35 | q3_int = mt.sin(psi / 2) * mt.cos(theta / 2) * mt.cos(phi / 2) - mt.cos(psi / 2) * mt.sin(theta / 2) * mt.sin(phi / 2)
36 |
37 | Norm = mx.norm([q0_int, q1_int, q2_int, q3_int])
38 |
39 | Q_int = [q0_int/Norm, q1_int/Norm, q2_int/Norm, q3_int/Norm]
40 |
41 | return Q_int
42 |
43 |
44 | def instant_quaternion(gyro: np.ndarray, quat0: np.ndarray, dt: float) -> np.ndarray:
45 | """Generate instantaneous quaternion vector
46 | Args:
47 | gyro ([type]): angular velocity from gyroscope
48 | quat0 (np.ndarray): previous quaternion vector
49 | dt (float): sample interval
50 | Returns:
51 | np.ndarray: instantaneous quaternion
52 | """
53 | omega = np.array([[2, -gyro[0]*dt, -gyro[1]*dt, -gyro[2]*dt],
54 | [gyro[0]*dt, 2, gyro[2]*dt, -gyro[1]*dt],
55 | [gyro[1]*dt, -gyro[2]*dt, 2, gyro[0]*dt],
56 | [gyro[2]*dt, gyro[1]*dt, -gyro[0]*dt, 2]])
57 |
58 | instQuat = 0.5 * np.dot(omega, quat0)
59 | instQuat /= mx.norm(instQuat)
60 |
61 | return instQuat
62 |
63 |
64 | def quat2rmx(quat: np.ndarray) -> np.ndarray:
65 | """Convert quaternion to rotation matrix.
66 | Args:
67 | quat (np.ndarray): quaternion vector
68 | Returns:
69 | np.ndarray: rotation matrix
70 | """
71 | q02 = quat[0]**2
72 | q12 = quat[1]**2
73 | q22 = quat[2]**2
74 | q32 = quat[3]**2
75 |
76 | rot_mx = np.array([[2*q02-1+2*q12, 2*quat[1]*quat[2]-2*quat[0]*quat[3], 2*quat[1]*quat[3]+2*quat[0]*quat[2]],
77 | [2*quat[1]*quat[2]+2*quat[0]*quat[3], 2*q02-1+2*q22, 2*quat[2]*quat[3]-2*quat[0]*quat[1]],
78 | [2*quat[1]*quat[3]-2*quat[0]*quat[2], 2*quat[2]*quat[3]+2*quat[0]*quat[1], 2*q02-1+2*q32]])
79 | return rot_mx
80 |
81 |
82 | def global_acc(rot_mx: np.ndarray, acc: np.ndarray) -> np.ndarray:
83 | """transform IMU acceleration to global acceleration.
84 | Args:
85 | rot_mx (np.ndarray): IMU to global rotation matrix
86 | acc (np.ndarray): acceleration in the IMU frame
87 | Returns:
88 | np.ndarray: acceleration in the global frame
89 | """
90 | acc_trans = np.transpose(acc)
91 | gravity = np.transpose([0, 0, 1])
92 | glob_acc = np.transpose((np.dot(rot_mx, acc_trans) - gravity)) * 9.81 # convert to m/s^2
93 |
94 | return glob_acc
95 |
96 | # TODO: replace trapz with scipy trapezoidal integration
97 |
98 | def heading_angle(mag: np.ndarray) -> float:
99 | """Get heading angle from magnetometer
100 | Args:
101 | mag (np.ndarray): normalized magnetometer vector
102 | Returns:
103 | float: heading angle
104 | """
105 | yaw = mt.atan2(mag[1], mag[0])
106 | if yaw < 0:
107 | yaw += 2 * np.pi
108 |
109 | return yaw
110 |
111 |
112 | def rmx2eul(rot_mx: np.ndarray) -> np.ndarray:
113 | """convert rotation matrix to euler angles
114 | Args:
115 | rot_mx (np.ndarray): IMU to global rotation matrix
116 | Returns:
117 | np.ndarray: vector of euler angles
118 | """
119 | n_angles = 3 # number of euler angles
120 | eul = np.zeros(n_angles)
121 | eul[0] = mt.atan2(rot_mx[1, 2], rot_mx[2, 2])
122 | eul[1] = mt.atan2(-rot_mx[0, 2], mx.norm(rot_mx[0, 0:2]))
123 | eul[2] = mt.atan2(rot_mx[0, 1], rot_mx[0, 0])
124 |
125 | return eul
126 |
--------------------------------------------------------------------------------
/iotools/build_cmd.py:
--------------------------------------------------------------------------------
1 | from typing import Dict, NewType
2 | from struct import pack
3 | import json
4 | import os
5 |
6 | '''Load imu commands'''
7 | abs_path = os.path.dirname(os.path.realpath(__file__))
8 | config_dir = os.path.join(abs_path, 'config')
9 | data_path = os.path.join(config_dir, 'data_commands.json')
10 | resp_path = os.path.join(config_dir, 'response_commands.json')
11 |
12 | with (open(data_path) as data_file, open(resp_path) as resp_file):
13 | DATA_CMDS = json.load(data_file)
14 | RESP_CMDS = json.load(resp_file)
15 |
16 | '''Define defaults'''
17 | DEFAULT_DATA_CMDS = {key: value for key, value in DATA_CMDS.items()
18 | if key in ["button state", "gyroscope", "accelerometer", "magnetometer norm", "temperature C"]}
19 | DEFAULT_RESP_CMDS = {key: value for key, value in RESP_CMDS.items()
20 | if key in ["success bit", "timestamp", "data length"]}
21 |
22 | '''On time commands'''
23 | SET_RESP_HEAD = 221
24 | SET_SLOT = 80 # TODO: Find a way to make command format consistent
25 | EMPTY_SLOT = 255
26 | # TODO: Look into a Creation design pattern for this
27 |
28 | CmdType = NewType('CmdType', Dict[str, Dict[str, str | int]])
29 |
30 |
31 | class BuildCommands:
32 | """Class to compile IMU commands"""
33 |
34 | def __init__(self,
35 | max_slots: int = 8,
36 | data_cmds: CmdType = DEFAULT_DATA_CMDS,
37 | resp_cmds: CmdType = DEFAULT_RESP_CMDS):
38 |
39 | self._max_slots = max_slots # at the time of writing this code, yost lab IMUs only
40 | # take 8 streaming slots
41 | self.data_cmds = data_cmds
42 | self.resp_cmds = resp_cmds
43 |
44 | # TODO: Throw value error if response header is not in resp_cmds
45 |
46 | '''Define length of response header'''
47 | self._resp_num_bytes = sum([cmd["raw length"] for _, cmd in resp_cmds.items()])
48 |
49 | '''Define number of bytes from IMU's returned data'''
50 | data_num_bytes = sum([cmd['raw length'] for _, cmd in data_cmds.items()]) # TODO: Think about using calcsize
51 | self._total_num_bytes = data_num_bytes + self._resp_num_bytes
52 |
53 | # self._command_names = list(resp_cmds.keys()) + list(data_cmds.keys())
54 |
55 | @staticmethod
56 | def _check_cmd_input(user_cmds: CmdType, default_cmds: CmdType, msg: str):
57 | for _, cmd in user_cmds.items():
58 | if cmd not in default_cmds.values():
59 | raise ValueError(msg)
60 |
61 | @property
62 | def resp_cmds(self) -> CmdType:
63 | return self._resp_cmds
64 |
65 | @resp_cmds.setter
66 | def resp_cmds(self, user_cmds: CmdType):
67 | self._check_cmd_input(user_cmds, RESP_CMDS, "Response header command does not exist")
68 | self._resp_cmds = user_cmds
69 |
70 | @property
71 | def data_cmds(self) -> CmdType:
72 | return self._data_cmds
73 |
74 | @data_cmds.setter
75 | def data_cmds(self, user_cmds: CmdType):
76 | self._check_cmd_input(user_cmds, DATA_CMDS, "IMU data command does not exist")
77 |
78 | '''Check if too many commands are set'''
79 | if len(user_cmds) > self._max_slots:
80 | raise ValueError("Too many commands are set")
81 | self._data_cmds = user_cmds
82 |
83 | @property
84 | def resp_num_bytes(self) -> int:
85 | return self._resp_num_bytes
86 |
87 | @property
88 | def total_num_bytes(self) -> int:
89 | return self._total_num_bytes
90 |
91 | def pack_response_header(self) -> bytes:
92 | """Pack response header command into bytes
93 | Returns:
94 | bytes: converted response header command
95 | """
96 | int_cmd = sum([cmd["command"] for _, cmd in self._resp_cmds.items()])
97 | return pack('B', SET_RESP_HEAD) + pack('>I', int_cmd)
98 |
99 | def pack_data_cmds(self) -> bytes:
100 | """Pack hex commands into bytes to send to IMU
101 | Returns:
102 | bytes: hex commands packed into bytes
103 | """
104 |
105 | cmd_len = len(self._data_cmds) # length of data command slots
106 | n_empty = (self._max_slots - cmd_len) # number of empty slots
107 |
108 | '''compile pack characters'''
109 | pack_chars = (self._max_slots + 1) * 'B' # set whole pack to binary (add one for set slot cmd)
110 |
111 | '''compile hex commands'''
112 | # Set slot, list of data commands, fill remainder with empty slots
113 | hex_cmds = [SET_SLOT] + [cmd["command"] for _, cmd in self._data_cmds.items()] + (n_empty * [EMPTY_SLOT])
114 |
115 | return pack(pack_chars, *hex_cmds)
116 |
--------------------------------------------------------------------------------
/iotools/imu_stream.py:
--------------------------------------------------------------------------------
1 | # standard imports
2 | import numpy as np
3 | import serial
4 | from struct import pack, unpack_from
5 | import time
6 | from typing import Tuple
7 |
8 | # local imports
9 | from iotools.build_cmd import BuildCommands, CmdType
10 |
11 | # %% Code Summary
12 | """The IMU class provides the functions needed to connect and execute commands on the bluetooth IMU.
13 | """
14 |
15 | # %% Constant command variables
16 | # Define stream variables
17 | RIGHT_AXIS = pack('BB', 116, 1)
18 | SET_STREAM = pack('B', 82)
19 | START_STREAM = pack('BB', 85, 85)
20 | STOP_STREAM = pack('BB', 86, 86)
21 | RESET = pack('B', 226)
22 |
23 |
24 | class IMU:
25 |
26 | def __init__(self,
27 | ser: serial.Serial,
28 | bldg_cmd: BuildCommands = BuildCommands()):
29 | self._ser = ser
30 | self._bldg_cmd = bldg_cmd
31 |
32 | def _com_write(self, cmd: bytes, msg: str, resp_head: bool = True):
33 | """Writes and sends a command to the IMU.
34 | Args:
35 | cmd (bytes): command that will be sent to the IMU.
36 | resp_head (bool, optional): Appended response header for command. Defaults to True.
37 | Returns:
38 | Optional[bytes]: checksum used to check command"""
39 |
40 | print(msg)
41 | checksum = bytes([sum(cmd) % 256]) # create checksum for command
42 |
43 | '''Write final command to send to imu'''
44 | if resp_head: # If you would like to use the response header for the data
45 | final_cmd = b'\xf9' + cmd + checksum
46 | else:
47 | final_cmd = b'\xf7' + cmd + checksum
48 |
49 | self._ser.write(final_cmd) # send command to imu
50 |
51 | '''Print checksum from response header'''
52 | if resp_head:
53 | check = self._ser.read(self._bldg_cmd.resp_num_bytes)
54 | print('Success/Failure:', check[0])
55 |
56 | def software_reset(self) -> None:
57 | """Resets the software settings on the IMU."""
58 | print('---------------------------------------')
59 | self._com_write(RESET, msg=f'Software reset for port {self._ser.port}', resp_head=False)
60 | time.sleep(0.5)
61 |
62 | def set_stream(self, interval: int, duration: int, delay: int) -> None:
63 | """Set streaming settings for the IMU.
64 | Args:
65 | interval (int): interval between data points (microseconds)
66 | duration (int): the duration of the streaming session (microseconds)
67 | delay (int): the delay before the data streaming starts (microseconds)"""
68 |
69 | print('---------------------------------------')
70 | print('Stream setup for port', self._ser.port)
71 | self._com_write(self._bldg_cmd.pack_response_header(),
72 | msg='write response header',
73 | resp_head=False)
74 |
75 | self._com_write(RIGHT_AXIS, msg='Setting Right Hand Coordinate system')
76 |
77 | self._com_write(self._bldg_cmd.pack_data_cmds(), msg='Setting up streaming slots')
78 |
79 | timing = SET_STREAM + pack('>III', interval, duration, delay)
80 | self._com_write(timing, msg='Applying time settings')
81 |
82 | def start_streaming(self) -> None:
83 | """Start streaming the IMU with the settings defined in the set_stream function"""
84 | print('---------------------------------------')
85 | self._com_write(START_STREAM, msg=f'Start stream for port {self._ser.port}')
86 |
87 | def stop_streaming(self) -> None:
88 | """Stop streaming the IMU."""
89 | print('---------------------------------------')
90 | self._com_write(STOP_STREAM, msg=f'Stop stream for port {self._ser.port}')
91 |
92 | def _read_bytes(self,
93 | raw_data: bytes,
94 | bytes_read: int,
95 | commands: CmdType) -> Tuple[np.ndarray, int]:
96 | """Reads raw bytes returned from the IMU and unpacks them into readable data
97 | Args:
98 | raw_data (bytes): raw data from IMU
99 | bytes_read (int): number of bytes that have been read from bytes array so far
100 | commands (CmdType): IMU commands to read and unpack
101 |
102 | Returns:
103 | Tuple[np.ndarray, int]: parsed data and the updated bytes_read
104 | """
105 |
106 | parsed_data = list()
107 | for _, cmd in commands.items(): # cycle through selected commands
108 | if "unpack" in cmd.keys(): # if data needs to be unpacked
109 | parsed_data.extend(unpack_from(cmd['unpack'], raw_data, offset=bytes_read))
110 | else: # if you can read data as is
111 | parsed_data.append(raw_data[bytes_read])
112 | bytes_read += cmd['raw length']
113 |
114 | return parsed_data, bytes_read
115 |
116 | def get_data(self) -> np.ndarray:
117 | """Function used to read data for each interval during streaming.
118 | Returns:
119 | np.ndarray: row of data points for that interval"""
120 |
121 | '''Get number of bytes from data return'''
122 | raw_data = self._ser.read(self._bldg_cmd.total_num_bytes)
123 |
124 | '''Read data from IMU byte array'''
125 | bytes_read = 0 # number of bytes read so far in buffer
126 | parsed_resp, bytes_read = self._read_bytes(raw_data, bytes_read, self._bldg_cmd.resp_cmds) # read response header
127 | parsed_data, _ = self._read_bytes(raw_data, bytes_read, self._bldg_cmd.data_cmds) # read imu data
128 | data = parsed_resp + parsed_data # combine data into one array
129 |
130 | # TODO: account for when success bit is not the first header (Write into dataframe)
131 | return data if raw_data[0] == 0 else np.zeros(len(data)) # return data if success bit in checksum is 0 else return zero array
132 |
--------------------------------------------------------------------------------