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 <[email protected]>
kscalelabs · Jan 1, 2025 · cd1c664 · cd1c664
1 parent 00bb5a6
commit cd1c664
Show file tree

Hide file tree

Showing 15 changed files with 603 additions and 180 deletions.
diff --git 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
@@ -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 <[email protected]>"]
 edition = "2021"
 description = "IMU package"

diff --git a/examples/hiwonder_example.py 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
@@ -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
@@ -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
@@ -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
@@ -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<Mutex<HexmoveImuReader>>,
+}
+
+#[gen_stub_pymethods]
+#[pymethods]
+impl PyHexmoveImuReader {
+    #[new]
+    fn new(interface: String, serial_number: u8, model: u8) -> PyResult<Self> {
+        let imu_reader = HexmoveImuReader::new(&interface, serial_number, model)
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        Ok(PyHexmoveImuReader {
+            inner: Arc::new(Mutex::new(imu_reader)),
+        })
+    }
+
+    fn get_data(&self) -> PyResult<PyHexmoveImuData> {
+        let imu_reader = self
+            .inner
+            .lock()
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        let data = imu_reader
+            .get_data()
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(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::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        let (x, y, z) = imu_reader
+            .get_angles()
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(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::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        let (x, y, z) = imu_reader
+            .get_velocities()
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(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::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        let (x, y, z) = imu_reader
+            .get_accelerations()
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(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::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        let (w, x, y, z) = imu_reader
+            .get_quaternion()
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(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<u64>,
+        max_retries: Option<u32>,
+        max_variance: Option<f32>,
+    ) -> PyResult<()> {
+        let imu_reader = self.inner.lock().unwrap();
+        imu_reader
+            .zero_imu(duration_ms, max_retries, max_variance)
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e))
+    }
+
+    fn stop(&self) -> PyResult<()> {
+        let imu_reader = self
+            .inner
+            .lock()
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        imu_reader
+            .stop()
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(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<HexmoveImuData> 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
@@ -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<Mutex<IMU>>,
+}
+
+#[gen_stub_pymethods]
+#[pymethods]
+impl PyHiwonderImu {
+    #[new]
+    fn new(interface: String, baud_rate: u32) -> PyResult<Self> {
+        let imu = IMU::new(&interface, baud_rate)
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        Ok(PyHiwonderImu {
+            inner: Arc::new(Mutex::new(imu)),
+        })
+    }
+
+    fn read_data(&mut self) -> PyResult<Option<([f32; 3], [f32; 3], [f32; 3])>> {
+        let mut imu = self
+            .inner
+            .lock()
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        imu.read_data()
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
+    }
+}
Original file line number	Diff line number	Diff line change
Expand Up		@@ -20,3 +20,4 @@ pyo3-stub-gen = ">= 0.6.0"

		# Other packages in the workspace.
		hexmove = { path = "../hexmove" }
		hiwonder = { path = "../hiwonder" }