From cd1c6641d1ba7b14e6cb414f0e0583935f31d4de Mon Sep 17 00:00:00 2001 From: Aaron Xie Date: Wed, 1 Jan 2025 12:11:35 -0800 Subject: [PATCH] Hiwonder (#8) * hiwonder imu setup * working hiwonder imu rust code * minor * rearrange to lib and log * hiwonder imu, refactor and return data as struct * working python binding draft for hiwonder IMU * hiwonder publish * separate version * update publish flow * update python bindings * fix hiwonder python * lint * update publish workflow * update readme --------- Co-authored-by: Wesley Maa --- .github/workflows/publish.yml | 66 +++++++--- Cargo.toml | 4 +- examples/hiwonder_example.py | 26 ++++ imu/Cargo.toml | 1 + imu/__init__.py | 3 + imu/bindings/Cargo.toml | 1 + imu/bindings/src/hexmove.rs | 163 +++++++++++++++++++++++++ imu/bindings/src/hiwonder.rs | 32 +++++ imu/bindings/src/lib.rs | 167 +------------------------- imu/hexmove/Cargo.toml | 3 +- imu/hiwonder/Cargo.toml | 18 +++ imu/hiwonder/README.md | 17 +++ imu/hiwonder/src/bin/log.rs | 60 ++++++++++ imu/hiwonder/src/lib.rs | 218 ++++++++++++++++++++++++++++++++++ imu/src/lib.rs | 4 + 15 files changed, 603 insertions(+), 180 deletions(-) create mode 100644 examples/hiwonder_example.py create mode 100644 imu/bindings/src/hexmove.rs create mode 100644 imu/bindings/src/hiwonder.rs create mode 100644 imu/hiwonder/Cargo.toml create mode 100644 imu/hiwonder/README.md create mode 100644 imu/hiwonder/src/bin/log.rs create mode 100644 imu/hiwonder/src/lib.rs diff --git a/.github/workflows/publish.yml b/.github/workflows/publish.yml index 9cf4918..e2ba274 100644 --- a/.github/workflows/publish.yml +++ b/.github/workflows/publish.yml @@ -4,6 +4,19 @@ on: release: types: [created] workflow_dispatch: + inputs: + publish_imu: + description: 'Publish imu package' + type: boolean + default: false + publish_hexmove: + description: 'Publish hexmove package' + type: boolean + default: false + publish_hiwonder: + description: 'Publish hiwonder package' + type: boolean + default: false permissions: contents: read @@ -15,12 +28,19 @@ concurrency: jobs: build-wheels: + name: Build ${{ matrix.arch }} wheels + timeout-minutes: 360 + runs-on: ubuntu-latest strategy: matrix: - os: [ubuntu-latest] - name: Build and publish Python package (${{ matrix.os }}) - timeout-minutes: 10 - runs-on: ${{ matrix.os }} + arch: [x86_64, aarch64, s390x] + include: + - arch: x86_64 + skip: "pp* *-musllinux*" + - arch: aarch64 + skip: "pp* *-musllinux* cp313-*" + - arch: s390x + skip: "pp* *-musllinux* cp312-* cp313-*" steps: - name: Checkout code @@ -29,7 +49,7 @@ jobs: - name: Set up Python uses: actions/setup-python@v4 with: - python-version: "3.10" + python-version: "3.11" - name: Set up Rust uses: actions-rs/toolchain@v1 @@ -42,16 +62,20 @@ jobs: pip install cibuildwheel shell: bash + - name: Set up QEMU + uses: docker/setup-qemu-action@v2 + with: + platforms: all + - name: Build package env: - CIBW_SKIP: "pp* *-musllinux*" # Skip PyPy and musllinux builds + CIBW_SKIP: ${{ matrix.skip }} + CIBW_ARCHS_LINUX: ${{ matrix.arch }} CIBW_BEFORE_ALL_LINUX: | - yum install -y libudev-devel pkgconfig - CIBW_BEFORE_ALL_MACOS: | - brew install openssl pkg-config + yum install -y libudev-devel pkgconfig python3-devel python3-pip python3-wheel CIBW_BEFORE_BUILD: | curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y - source $HOME/.cargo/env + export PATH="$HOME/.cargo/bin:$PATH" pip install setuptools-rust CIBW_ENVIRONMENT: | PATH="/usr/local/bin:$HOME/.cargo/bin:$PATH" @@ -62,7 +86,7 @@ jobs: - name: Upload wheel artifacts uses: actions/upload-artifact@v3 with: - name: wheels-${{ matrix.os }} + name: wheels-${{ matrix.arch }} path: | dist/*.whl @@ -129,6 +153,7 @@ jobs: name: Build and publish Rust package timeout-minutes: 10 runs-on: ubuntu-latest + if: ${{ inputs.publish_imu || inputs.publish_hexmove || inputs.publish_hiwonder }} steps: - name: Checkout code @@ -160,9 +185,20 @@ jobs: restore-keys: | ${{ runner.os }}-cargo-index - - name: Publish imu and hexmove packages to crates.io + - name: Publish imu package to crates.io + if: ${{ inputs.publish_imu }} env: CARGO_REGISTRY_TOKEN: ${{ secrets.CARGO_REGISTRY_TOKEN }} - run: | - cargo publish -p imu - cargo publish -p hexmove + run: cargo publish -p imu + + - name: Publish hexmove package to crates.io + if: ${{ inputs.publish_hexmove }} + env: + CARGO_REGISTRY_TOKEN: ${{ secrets.CARGO_REGISTRY_TOKEN }} + run: cargo publish -p hexmove + + - name: Publish hiwonder package to crates.io + if: ${{ inputs.publish_hiwonder }} + env: + CARGO_REGISTRY_TOKEN: ${{ secrets.CARGO_REGISTRY_TOKEN }} + run: cargo publish -p hiwonder diff --git a/Cargo.toml b/Cargo.toml index 6cfd8ef..f889133 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -4,12 +4,12 @@ members = [ "imu", "imu/bindings", "imu/hexmove", + "imu/hiwonder", ] resolver = "2" [workspace.package] - -version = "0.1.6" +version = "0.1.7" authors = ["Wesley Maa "] edition = "2021" description = "IMU package" diff --git a/examples/hiwonder_example.py b/examples/hiwonder_example.py new file mode 100644 index 0000000..a0d6645 --- /dev/null +++ b/examples/hiwonder_example.py @@ -0,0 +1,26 @@ +"""Example usage of the Hiwonder IMU.""" + +import time + +from imu import HiwonderImu + + +def main() -> None: + # Create IMU instance + imu = HiwonderImu("/dev/ttyUSB0", 9600) + + try: + while True: + if data := imu.read_data(): + acc, gyro, angle = data + print("\033[2J\033[H") # Clear screen + print(f"Acceleration (m/s²): {acc}") + print(f"Gyroscope (deg/s): {gyro}") + print(f"Angle (degrees): {angle}") + time.sleep(0.1) # Add small delay to make output readable + except KeyboardInterrupt: + print("\nExiting...") + + +if __name__ == "__main__": + main() diff --git a/imu/Cargo.toml b/imu/Cargo.toml index c1e88d8..ab7d4fb 100644 --- a/imu/Cargo.toml +++ b/imu/Cargo.toml @@ -14,5 +14,6 @@ crate-type = ["rlib"] [dependencies] hexmove = { path = "./hexmove", version = "0.1.2" } +hiwonder = { path = "./hiwonder", version = "0.1.0" } socketcan = "3.3.0" log = "0.4" diff --git a/imu/__init__.py b/imu/__init__.py index 6d710f2..55e0196 100644 --- a/imu/__init__.py +++ b/imu/__init__.py @@ -1,6 +1,9 @@ """Defines the top-level API for the IMU package.""" from .bindings import ( + HiwonderImu, PyHexmoveImuData as HexmoveImuData, PyHexmoveImuReader as HexmoveImuReader, ) + +__all__ = ["HexmoveImuData", "HexmoveImuReader", "HiwonderImu"] diff --git a/imu/bindings/Cargo.toml b/imu/bindings/Cargo.toml index a95ff95..bc0092b 100644 --- a/imu/bindings/Cargo.toml +++ b/imu/bindings/Cargo.toml @@ -20,3 +20,4 @@ pyo3-stub-gen = ">= 0.6.0" # Other packages in the workspace. hexmove = { path = "../hexmove" } +hiwonder = { path = "../hiwonder" } diff --git a/imu/bindings/src/hexmove.rs b/imu/bindings/src/hexmove.rs new file mode 100644 index 0000000..566bea1 --- /dev/null +++ b/imu/bindings/src/hexmove.rs @@ -0,0 +1,163 @@ +use hexmove::{ImuData as HexmoveImuData, ImuReader as HexmoveImuReader}; +use pyo3::prelude::*; +use pyo3_stub_gen::derive::{gen_stub_pyclass, gen_stub_pymethods}; +use std::sync::{Arc, Mutex}; + +#[gen_stub_pyclass] +#[pyclass] +pub struct PyHexmoveImuReader { + inner: Arc>, +} + +#[gen_stub_pymethods] +#[pymethods] +impl PyHexmoveImuReader { + #[new] + fn new(interface: String, serial_number: u8, model: u8) -> PyResult { + let imu_reader = HexmoveImuReader::new(&interface, serial_number, model) + .map_err(|e| PyErr::new::(e.to_string()))?; + Ok(PyHexmoveImuReader { + inner: Arc::new(Mutex::new(imu_reader)), + }) + } + + fn get_data(&self) -> PyResult { + let imu_reader = self + .inner + .lock() + .map_err(|e| PyErr::new::(e.to_string()))?; + let data = imu_reader + .get_data() + .map_err(|e| PyErr::new::(e.to_string()))?; + Ok(PyHexmoveImuData::from(data)) + } + + fn get_angles(&self) -> PyResult<(f32, f32, f32)> { + let imu_reader = self + .inner + .lock() + .map_err(|e| PyErr::new::(e.to_string()))?; + let (x, y, z) = imu_reader + .get_angles() + .map_err(|e| PyErr::new::(e.to_string()))?; + Ok((x, y, z)) + } + + fn get_velocities(&self) -> PyResult<(f32, f32, f32)> { + let imu_reader = self + .inner + .lock() + .map_err(|e| PyErr::new::(e.to_string()))?; + let (x, y, z) = imu_reader + .get_velocities() + .map_err(|e| PyErr::new::(e.to_string()))?; + Ok((x, y, z)) + } + + fn get_accelerations(&self) -> PyResult<(f32, f32, f32)> { + let imu_reader = self + .inner + .lock() + .map_err(|e| PyErr::new::(e.to_string()))?; + let (x, y, z) = imu_reader + .get_accelerations() + .map_err(|e| PyErr::new::(e.to_string()))?; + Ok((x, y, z)) + } + + fn get_quaternion(&self) -> PyResult<(f32, f32, f32, f32)> { + let imu_reader = self + .inner + .lock() + .map_err(|e| PyErr::new::(e.to_string()))?; + let (w, x, y, z) = imu_reader + .get_quaternion() + .map_err(|e| PyErr::new::(e.to_string()))?; + Ok((w, x, y, z)) + } + + #[pyo3(signature = (duration_ms=None, max_retries=None, max_variance=None))] + fn zero_imu( + &self, + duration_ms: Option, + max_retries: Option, + max_variance: Option, + ) -> PyResult<()> { + let imu_reader = self.inner.lock().unwrap(); + imu_reader + .zero_imu(duration_ms, max_retries, max_variance) + .map_err(|e| PyErr::new::(e)) + } + + fn stop(&self) -> PyResult<()> { + let imu_reader = self + .inner + .lock() + .map_err(|e| PyErr::new::(e.to_string()))?; + imu_reader + .stop() + .map_err(|e| PyErr::new::(e.to_string()))?; + Ok(()) + } +} + +#[gen_stub_pyclass] +#[pyclass] +#[derive(Clone)] +pub struct PyHexmoveImuData { + #[pyo3(get)] + x_angle: f32, + #[pyo3(get)] + y_angle: f32, + #[pyo3(get)] + z_angle: f32, + #[pyo3(get)] + x_velocity: f32, + #[pyo3(get)] + y_velocity: f32, + #[pyo3(get)] + z_velocity: f32, + #[pyo3(get)] + x_angle_offset: f32, + #[pyo3(get)] + y_angle_offset: f32, + #[pyo3(get)] + z_angle_offset: f32, + #[pyo3(get)] + accel_x: f32, + #[pyo3(get)] + accel_y: f32, + #[pyo3(get)] + accel_z: f32, + #[pyo3(get)] + qw: f32, + #[pyo3(get)] + qx: f32, + #[pyo3(get)] + qy: f32, + #[pyo3(get)] + qz: f32, +} + +impl From for PyHexmoveImuData { + fn from(data: HexmoveImuData) -> Self { + PyHexmoveImuData { + x_angle: data.x_angle, + y_angle: data.y_angle, + z_angle: data.z_angle, + x_velocity: data.x_velocity, + y_velocity: data.y_velocity, + z_velocity: data.z_velocity, + x_angle_offset: data.x_angle_offset, + y_angle_offset: data.y_angle_offset, + z_angle_offset: data.z_angle_offset, + accel_x: data.accel_x, + accel_y: data.accel_y, + accel_z: data.accel_z, + qw: data.qw, + qx: data.qx, + qy: data.qy, + qz: data.qz, + } + } +} diff --git a/imu/bindings/src/hiwonder.rs b/imu/bindings/src/hiwonder.rs new file mode 100644 index 0000000..ed695a1 --- /dev/null +++ b/imu/bindings/src/hiwonder.rs @@ -0,0 +1,32 @@ +use hiwonder::IMU; +use pyo3::prelude::*; +use pyo3_stub_gen::derive::{gen_stub_pyclass, gen_stub_pymethods}; +use std::sync::{Arc, Mutex}; + +#[gen_stub_pyclass] +#[pyclass(name = "HiwonderImu")] +pub struct PyHiwonderImu { + inner: Arc>, +} + +#[gen_stub_pymethods] +#[pymethods] +impl PyHiwonderImu { + #[new] + fn new(interface: String, baud_rate: u32) -> PyResult { + let imu = IMU::new(&interface, baud_rate) + .map_err(|e| PyErr::new::(e.to_string()))?; + Ok(PyHiwonderImu { + inner: Arc::new(Mutex::new(imu)), + }) + } + + fn read_data(&mut self) -> PyResult> { + let mut imu = self + .inner + .lock() + .map_err(|e| PyErr::new::(e.to_string()))?; + imu.read_data() + .map_err(|e| PyErr::new::(e.to_string())) + } +} diff --git a/imu/bindings/src/lib.rs b/imu/bindings/src/lib.rs index b0d2e87..b07ddc2 100644 --- a/imu/bindings/src/lib.rs +++ b/imu/bindings/src/lib.rs @@ -1,172 +1,17 @@ -use hexmove::{ImuData as HexmoveImuData, ImuReader as HexmoveImuReader}; +mod hexmove; +mod hiwonder; + use pyo3::prelude::*; use pyo3_stub_gen::define_stub_info_gatherer; -use pyo3_stub_gen::derive::{gen_stub_pyclass, gen_stub_pymethods}; -use std::sync::{Arc, Mutex}; - -#[gen_stub_pyclass] -#[pyclass] -struct PyHexmoveImuReader { - inner: Arc>, -} - -#[gen_stub_pymethods] -#[pymethods] -impl PyHexmoveImuReader { - #[new] - fn new(interface: String, serial_number: u8, model: u8) -> PyResult { - let imu_reader = HexmoveImuReader::new(&interface, serial_number, model) - .map_err(|e| PyErr::new::(e.to_string()))?; - Ok(PyHexmoveImuReader { - inner: Arc::new(Mutex::new(imu_reader)), - }) - } - - fn get_data(&self) -> PyResult { - let imu_reader = self - .inner - .lock() - .map_err(|e| PyErr::new::(e.to_string()))?; - let data = imu_reader - .get_data() - .map_err(|e| PyErr::new::(e.to_string()))?; - Ok(PyHexmoveImuData::from(data)) - } - - fn get_angles(&self) -> PyResult<(f32, f32, f32)> { - let imu_reader = self - .inner - .lock() - .map_err(|e| PyErr::new::(e.to_string()))?; - let (x, y, z) = imu_reader - .get_angles() - .map_err(|e| PyErr::new::(e.to_string()))?; - Ok((x, y, z)) - } - - fn get_velocities(&self) -> PyResult<(f32, f32, f32)> { - let imu_reader = self - .inner - .lock() - .map_err(|e| PyErr::new::(e.to_string()))?; - let (x, y, z) = imu_reader - .get_velocities() - .map_err(|e| PyErr::new::(e.to_string()))?; - Ok((x, y, z)) - } - - fn get_accelerations(&self) -> PyResult<(f32, f32, f32)> { - let imu_reader = self - .inner - .lock() - .map_err(|e| PyErr::new::(e.to_string()))?; - let (x, y, z) = imu_reader - .get_accelerations() - .map_err(|e| PyErr::new::(e.to_string()))?; - Ok((x, y, z)) - } - fn get_quaternion(&self) -> PyResult<(f32, f32, f32, f32)> { - let imu_reader = self - .inner - .lock() - .map_err(|e| PyErr::new::(e.to_string()))?; - let (w, x, y, z) = imu_reader - .get_quaternion() - .map_err(|e| PyErr::new::(e.to_string()))?; - Ok((w, x, y, z)) - } - - #[pyo3(signature = (duration_ms=None, max_retries=None, max_variance=None))] - fn zero_imu( - &self, - duration_ms: Option, - max_retries: Option, - max_variance: Option, - ) -> PyResult<()> { - let imu_reader = self.inner.lock().unwrap(); - imu_reader - .zero_imu(duration_ms, max_retries, max_variance) - .map_err(|e| PyErr::new::(e)) - } - - fn stop(&self) -> PyResult<()> { - let imu_reader = self - .inner - .lock() - .map_err(|e| PyErr::new::(e.to_string()))?; - imu_reader - .stop() - .map_err(|e| PyErr::new::(e.to_string()))?; - Ok(()) - } -} - -#[gen_stub_pyclass] -#[pyclass] -#[derive(Clone)] -struct PyHexmoveImuData { - #[pyo3(get)] - x_angle: f32, - #[pyo3(get)] - y_angle: f32, - #[pyo3(get)] - z_angle: f32, - #[pyo3(get)] - x_velocity: f32, - #[pyo3(get)] - y_velocity: f32, - #[pyo3(get)] - z_velocity: f32, - #[pyo3(get)] - x_angle_offset: f32, - #[pyo3(get)] - y_angle_offset: f32, - #[pyo3(get)] - z_angle_offset: f32, - #[pyo3(get)] - accel_x: f32, - #[pyo3(get)] - accel_y: f32, - #[pyo3(get)] - accel_z: f32, - #[pyo3(get)] - qw: f32, - #[pyo3(get)] - qx: f32, - #[pyo3(get)] - qy: f32, - #[pyo3(get)] - qz: f32, -} - -impl From for PyHexmoveImuData { - fn from(data: HexmoveImuData) -> Self { - PyHexmoveImuData { - x_angle: data.x_angle, - y_angle: data.y_angle, - z_angle: data.z_angle, - x_velocity: data.x_velocity, - y_velocity: data.y_velocity, - z_velocity: data.z_velocity, - x_angle_offset: data.x_angle_offset, - y_angle_offset: data.y_angle_offset, - z_angle_offset: data.z_angle_offset, - accel_x: data.accel_x, - accel_y: data.accel_y, - accel_z: data.accel_z, - qw: data.qw, - qx: data.qx, - qy: data.qy, - qz: data.qz, - } - } -} +pub use hexmove::{PyHexmoveImuData, PyHexmoveImuReader}; +pub use hiwonder::PyHiwonderImu; #[pymodule] fn bindings(m: &Bound) -> PyResult<()> { m.add_class::()?; m.add_class::()?; + m.add_class::()?; Ok(()) } diff --git a/imu/hexmove/Cargo.toml b/imu/hexmove/Cargo.toml index 6beb8c6..2182d01 100644 --- a/imu/hexmove/Cargo.toml +++ b/imu/hexmove/Cargo.toml @@ -3,8 +3,7 @@ name = "hexmove" readme = "README.md" description = "Interface for interacting with Hexmove IMUs" - -version.workspace = true +version = "0.1.6" authors.workspace = true edition.workspace = true repository.workspace = true diff --git a/imu/hiwonder/Cargo.toml b/imu/hiwonder/Cargo.toml new file mode 100644 index 0000000..1a806d3 --- /dev/null +++ b/imu/hiwonder/Cargo.toml @@ -0,0 +1,18 @@ +[package] +name = "hiwonder" +version = "0.1.0" +readme = "README.md" +description = "Interface for interacting with Hiwonder IMUs" +edition = "2021" +authors.workspace = true +repository.workspace = true +license.workspace = true + +[dependencies] +serialport = "4.2.0" + +[lib] + +name = "hiwonder" +crate-type = ["rlib"] + diff --git a/imu/hiwonder/README.md b/imu/hiwonder/README.md new file mode 100644 index 0000000..fd67ab5 --- /dev/null +++ b/imu/hiwonder/README.md @@ -0,0 +1,17 @@ + +Link to [IMU](https://www.hiwonder.com/products/imu-module?variant=40375875305559): + +### HiWonder IMU Protocol +- Data Length: 11 bytes +- Header/start/SOF Byte: 0x55 +- Command Byte: 0x51 accel data, 0x52 Gyroscope data, 0x53 angle data +- Data: 8 bytes +- CheckSum Byte: 1 byte (only lowest 8 bits of summation of all bytes in packet is used (`CheckSum & 0xff`)) + +### Setup +- Install this [driver](https://github.com/WCHSoftGroup/ch341ser_linux) for the CH341 USB controller +- This should create a /dev/ttyUSB0 - you should check which one by doing ls /dev/tty*. You might need to the change the permissions +- Connect the IMU to the computer via USB +- Default Baud Rate: 9600 +- Default USB port: /dev/ttyUSB0 + diff --git a/imu/hiwonder/src/bin/log.rs b/imu/hiwonder/src/bin/log.rs new file mode 100644 index 0000000..eb66698 --- /dev/null +++ b/imu/hiwonder/src/bin/log.rs @@ -0,0 +1,60 @@ +use hiwonder::IMU; +use std::io; + +//* run by `cargo run --bin log` */ +#[derive(Debug)] +struct IMUData { + acc_x: f32, + acc_y: f32, + acc_z: f32, + gyro_x: f32, + gyro_y: f32, + gyro_z: f32, + angle_x: f32, + angle_y: f32, + angle_z: f32, +} + +impl From<([f32; 3], [f32; 3], [f32; 3])> for IMUData { + fn from((acc, gyro, angle): ([f32; 3], [f32; 3], [f32; 3])) -> Self { + IMUData { + acc_x: acc[0], + acc_y: acc[1], + acc_z: acc[2], + gyro_x: gyro[0], + gyro_y: gyro[1], + gyro_z: gyro[2], + angle_x: angle[0], + angle_y: angle[1], + angle_z: angle[2], + } + } +} + +fn main() -> io::Result<()> { + let mut imu = IMU::new("/dev/ttyUSB0", 9600)?; + + loop { + match imu.read_data() { + Ok(Some(data)) => { + let data = IMUData::from(data); + println!( + "acc: x: {: >10.3} y: {: >10.3} z: {: >10.3}\n\ + gyro: x: {: >10.3} y: {: >10.3} z: {: >10.3}\n\ + angle: x: {: >10.3} y: {: >10.3} z: {: >10.3}", + data.acc_x, + data.acc_y, + data.acc_z, + data.gyro_x, + data.gyro_y, + data.gyro_z, + data.angle_x, + data.angle_y, + data.angle_z + ); + } + Ok(None) => (), // No complete data available yet + Err(e) => eprintln!("Error reading from serial port: {}", e), + } + } +} diff --git a/imu/hiwonder/src/lib.rs b/imu/hiwonder/src/lib.rs new file mode 100644 index 0000000..f924686 --- /dev/null +++ b/imu/hiwonder/src/lib.rs @@ -0,0 +1,218 @@ +use serialport; +use std::io::{self, Read}; +use std::time::Duration; + +#[derive(Debug, PartialEq)] +enum FrameState { + Idle, + Acc, + Gyro, + Angle, +} + +pub struct IMU { + port: Box, + frame_state: FrameState, + byte_num: usize, + checksum: u8, + acc_data: [u8; 8], + gyro_data: [u8; 8], + angle_data: [u8; 8], + acc: [f32; 3], + gyro: [f32; 3], + angle: [f32; 3], +} + +impl IMU { + pub fn new(interface: &str, baud_rate: u32) -> io::Result { + let port = serialport::new(interface, baud_rate) + .timeout(Duration::from_millis(500)) + .open()?; + + Ok(IMU { + port: port, + frame_state: FrameState::Idle, + byte_num: 0, + checksum: 0, + acc_data: [0u8; 8], + gyro_data: [0u8; 8], + angle_data: [0u8; 8], + acc: [0.0; 3], + gyro: [0.0; 3], + angle: [0.0; 3], + }) + } + + pub fn read_data(&mut self) -> io::Result> { + let mut buffer = vec![0; 1024]; + match self.port.read(&mut buffer) { + Ok(bytes_read) if bytes_read > 0 => { + self.process_data(&buffer[..bytes_read]); + // Only return data when we have a complete angle reading + if self.frame_state == FrameState::Idle { + Ok(Some((self.acc, self.gyro, self.angle))) + } else { + Ok(None) + } + } + Ok(_) => Ok(None), + Err(e) => Err(e), + } + } + + pub fn process_data(&mut self, input_data: &[u8]) { + for &data in input_data { + match self.frame_state { + FrameState::Idle => { + if data == 0x55 && self.byte_num == 0 { + self.checksum = data; + self.byte_num = 1; + continue; + } else if self.byte_num == 1 { + self.checksum = self.checksum.wrapping_add(data); + match data { + 0x51 => { + self.frame_state = FrameState::Acc; + self.byte_num = 2; + } + 0x52 => { + self.frame_state = FrameState::Gyro; + self.byte_num = 2; + } + 0x53 => { + self.frame_state = FrameState::Angle; + self.byte_num = 2; + } + _ => { + self.reset(); + } + } + } + } + FrameState::Acc => { + if self.byte_num < 10 { + self.acc_data[self.byte_num - 2] = data; + self.checksum = self.checksum.wrapping_add(data); + self.byte_num += 1; + } else { + if data == (self.checksum & 0xFF) { + self.acc = Self::get_acc(&self.acc_data); + } + self.reset(); + } + } + FrameState::Gyro => { + if self.byte_num < 10 { + self.gyro_data[self.byte_num - 2] = data; + self.checksum = self.checksum.wrapping_add(data); + self.byte_num += 1; + } else { + if data == (self.checksum & 0xFF) { + self.gyro = Self::get_gyro(&self.gyro_data); + } + self.reset(); + } + } + FrameState::Angle => { + if self.byte_num < 10 { + self.angle_data[self.byte_num - 2] = data; + self.checksum = self.checksum.wrapping_add(data); + self.byte_num += 1; + } else { + if data == (self.checksum & 0xFF) { + self.angle = Self::get_angle(&self.angle_data); + } + self.reset(); + } + } + } + } + } + + fn reset(&mut self) { + self.frame_state = FrameState::Idle; + self.byte_num = 0; + self.checksum = 0; + } + + fn get_acc(datahex: &[u8; 8]) -> [f32; 3] { + let k_acc = 16.0; + let acc_x = ((u16::from(datahex[1]) << 8) | u16::from(datahex[0])) as f32 / 32768.0 * k_acc; + let acc_y = ((u16::from(datahex[3]) << 8) | u16::from(datahex[2])) as f32 / 32768.0 * k_acc; + let acc_z = ((u16::from(datahex[5]) << 8) | u16::from(datahex[4])) as f32 / 32768.0 * k_acc; + + [ + if acc_x >= k_acc { + acc_x - 2.0 * k_acc + } else { + acc_x + }, + if acc_y >= k_acc { + acc_y - 2.0 * k_acc + } else { + acc_y + }, + if acc_z >= k_acc { + acc_z - 2.0 * k_acc + } else { + acc_z + }, + ] + } + + fn get_gyro(datahex: &[u8; 8]) -> [f32; 3] { + let k_gyro = 2000.0; + let gyro_x = + ((u16::from(datahex[1]) << 8) | u16::from(datahex[0])) as f32 / 32768.0 * k_gyro; + let gyro_y = + ((u16::from(datahex[3]) << 8) | u16::from(datahex[2])) as f32 / 32768.0 * k_gyro; + let gyro_z = + ((u16::from(datahex[5]) << 8) | u16::from(datahex[4])) as f32 / 32768.0 * k_gyro; + + [ + if gyro_x >= k_gyro { + gyro_x - 2.0 * k_gyro + } else { + gyro_x + }, + if gyro_y >= k_gyro { + gyro_y - 2.0 * k_gyro + } else { + gyro_y + }, + if gyro_z >= k_gyro { + gyro_z - 2.0 * k_gyro + } else { + gyro_z + }, + ] + } + + fn get_angle(datahex: &[u8; 8]) -> [f32; 3] { + let k_angle = 180.0; + let angle_x = + ((u16::from(datahex[1]) << 8) | u16::from(datahex[0])) as f32 / 32768.0 * k_angle; + let angle_y = + ((u16::from(datahex[3]) << 8) | u16::from(datahex[2])) as f32 / 32768.0 * k_angle; + let angle_z = + ((u16::from(datahex[5]) << 8) | u16::from(datahex[4])) as f32 / 32768.0 * k_angle; + + [ + if angle_x >= k_angle { + angle_x - 2.0 * k_angle + } else { + angle_x + }, + if angle_y >= k_angle { + angle_y - 2.0 * k_angle + } else { + angle_y + }, + if angle_z >= k_angle { + angle_z - 2.0 * k_angle + } else { + angle_z + }, + ] + } +} diff --git a/imu/src/lib.rs b/imu/src/lib.rs index 904e351..6cb175c 100644 --- a/imu/src/lib.rs +++ b/imu/src/lib.rs @@ -1,3 +1,7 @@ pub mod hexmove { pub use ::hexmove::*; } + +pub mod hiwonder { + pub use ::hiwonder::*; +}