Hiwonder Reader

.github/workflows/test.yml

@@ -21,6 +21,11 @@ jobs:
   run-base-tests:
     timeout-minutes: 10
     runs-on: ubuntu-latest
+    env:
+      PYO3_PYTHON: /opt/hostedtoolcache/Python/3.11.11/x64/bin/python3.11
+      PYTHON_INCLUDE_DIR: /opt/hostedtoolcache/Python/3.11.11/x64/include/python3.11
+      PYTHON_LIB_DIR: /opt/hostedtoolcache/Python/3.11.11/x64/lib
+      LD_LIBRARY_PATH: /opt/hostedtoolcache/Python/3.11.11/x64/lib:$LD_LIBRARY_PATH
     steps:
       - name: Check out repository
         uses: actions/checkout@v3
@@ -32,8 +37,12 @@ jobs:
 
       - name: Install system dependencies
         run: |
+          sudo add-apt-repository ppa:deadsnakes/ppa
           sudo apt-get update
-          sudo apt-get install -y libudev-dev pkg-config
+          sudo apt-get install -y libudev-dev pkg-config python3.11-dev libpython3.11-dev
+          # Create symlinks to ensure libraries are found
+          sudo ln -sf /opt/hostedtoolcache/Python/3.11.11/x64/lib/libpython3.11.so /usr/lib/
+          sudo ln -sf /opt/hostedtoolcache/Python/3.11.11/x64/lib/libpython3.11.so.1.0 /usr/lib/
 
       - name: Restore cache
         id: restore-cache
@@ -42,10 +51,10 @@ jobs:
           path: |
             ${{ env.pythonLocation }}
             .mypy_cache/
-          key: python-requirements-${{ env.pythonLocation }}-${{ github.event.pull_request.base.sha || github.sha }}
+          key: new-python-requirements-${{ env.pythonLocation }}-${{ github.event.pull_request.base.sha || github.sha }}
           restore-keys: |
-            python-requirements-${{ env.pythonLocation }}
-            python-requirements-
+            new-python-requirements-${{ env.pythonLocation }}
+            new-python-requirements-
 
       - name: Install package
         run: |

Cargo.toml

@@ -10,7 +10,7 @@ members = [
 resolver = "2"
 
 [workspace.package]
-version = "0.2.1"
+version = "0.2.2"
 authors = ["Wesley Maa <[email protected]>"]
 edition = "2021"
 description = "IMU package"

imu/Cargo.toml

@@ -14,7 +14,7 @@ crate-type = ["rlib"]
 
 [dependencies]
 hexmove = "0.1.6"
-hiwonder = "0.2.0"
+hiwonder = "0.2.2"
 linux_bno055 = "0.0.6"
 socketcan = "3.3.0"
 log = "0.4"

imu/bindings/Cargo.toml

@@ -15,7 +15,7 @@ name = "bindings"
 crate-type = ["cdylib", "rlib"]
 
 [dependencies]
-pyo3 = { version = ">= 0.21.0", features = ["extension-module"] }
+pyo3 = { version = ">= 0.21.0", features = ["extension-module", "auto-initialize"] }
 pyo3-stub-gen = ">= 0.6.0"
 
 # Other packages in the workspace.

imu/bindings/src/hiwonder.rs

@@ -1,4 +1,4 @@
-use hiwonder::IMU;
+use hiwonder::{HiwonderReader, ImuData};
 use pyo3::prelude::*;
 use pyo3_stub_gen::derive::{gen_stub_pyclass, gen_stub_pymethods};
 use std::sync::{Arc, Mutex};
@@ -36,40 +36,65 @@ impl PyImuData {
     }
 }
 
+impl From<ImuData> for PyImuData {
+    fn from(data: ImuData) -> Self {
+        PyImuData {
+            accelerometer: data.accelerometer.to_vec(),
+            gyroscope: data.gyroscope.to_vec(),
+            angle: data.angle.to_vec(),
+            quaternion: data.quaternion.to_vec(),
+        }
+    }
+}
+
 #[gen_stub_pyclass]
 #[pyclass(name = "HiwonderImu")]
 pub struct PyHiwonderImu {
-    inner: Arc<Mutex<IMU>>,
+    inner: Arc<Mutex<HiwonderReader>>,
 }
 
 #[gen_stub_pymethods]
 #[pymethods]
 impl PyHiwonderImu {
     #[new]
     fn new(interface: String, baud_rate: u32) -> PyResult<Self> {
-        let imu = IMU::new(&interface, baud_rate)
+        let reader = HiwonderReader::new(&interface, baud_rate)
             .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
         Ok(PyHiwonderImu {
-            inner: Arc::new(Mutex::new(imu)),
+            inner: Arc::new(Mutex::new(reader)),
         })
     }
 
-    fn read_data(&mut self) -> PyResult<Option<PyObject>> {
-        let mut imu = self
+    fn get_data(&self) -> PyResult<PyImuData> {
+        let reader = self
             .inner
             .lock()
             .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        let data = reader
+            .get_data()
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        Ok(PyImuData::from(data))
+    }
 
-        match imu.read_data() {
-            Ok(Some((accel, gyro, angle, quat))) => Python::with_gil(|py| {
-                let data =
-                    PyImuData::new(accel.to_vec(), gyro.to_vec(), angle.to_vec(), quat.to_vec());
-                Ok(Some(Py::new(py, data)?.into_py(py)))
-            }),
-            Ok(None) => Ok(None),
-            Err(e) => Err(PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(
-                e.to_string(),
-            )),
-        }
+    fn reset(&self) -> PyResult<()> {
+        let reader = self
+            .inner
+            .lock()
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        reader
+            .reset()
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        Ok(())
+    }
+
+    fn stop(&self) -> PyResult<()> {
+        let reader = self
+            .inner
+            .lock()
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        reader
+            .stop()
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        Ok(())
     }
 }

imu/hiwonder/Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "hiwonder"
-version = "0.2.1"
+version = "0.2.3"
 readme = "README.md"
 description = "Interface for interacting with Hiwonder IMUs"
 edition = "2021"

imu/hiwonder/src/bin/read_hiwonder.rs

@@ -1,79 +1,43 @@
-use hiwonder::{ImuFrequency, IMU};
+use hiwonder::{ImuFrequency, HiwonderReader};
 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,
-    quaternion_x: f32,
-    quaternion_y: f32,
-    quaternion_z: f32,
-    quaternion_w: f32,
-}
-
-impl From<([f32; 3], [f32; 3], [f32; 3], [f32; 4])> for IMUData {
-    fn from((acc, gyro, angle, quaternion): ([f32; 3], [f32; 3], [f32; 3], [f32; 4])) -> 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],
-            quaternion_x: quaternion[0],
-            quaternion_y: quaternion[1],
-            quaternion_z: quaternion[2],
-            quaternion_w: quaternion[3],
-        }
-    }
-}
+use std::thread;
+use std::time::Duration;
 
 fn main() -> io::Result<()> {
-    let mut imu =
-        IMU::new("/dev/ttyUSB0", 9600).map_err(|e| io::Error::new(io::ErrorKind::Other, e))?;
+    let reader =
+        HiwonderReader::new("/dev/ttyUSB0", 9600).map_err(|e| io::Error::new(io::ErrorKind::Other, e))?;
 
-    match imu.set_frequency(ImuFrequency::Hz20) {
+    match reader.set_frequency(ImuFrequency::Hz200) {
         Ok(_) => println!("Set frequency to 200hz"),
         Err(e) => println!("Failed to set frequency: {}", e),
     }
 
     loop {
-        match imu.read_data() {
-            Ok(Some(data)) => {
-                let data = IMUData::from(data);
+        match reader.get_data() {
+            Ok(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}\n\
                      quaternion: x: {: >10.3} y: {: >10.3} z: {: >10.3} w: {: >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,
-                    data.quaternion_x,
-                    data.quaternion_y,
-                    data.quaternion_z,
-                    data.quaternion_w
+                    data.accelerometer[0],
+                    data.accelerometer[1],
+                    data.accelerometer[2],
+                    data.gyroscope[0],
+                    data.gyroscope[1],
+                    data.gyroscope[2],
+                    data.angle[0],
+                    data.angle[1],
+                    data.angle[2],
+                    data.quaternion[0],
+                    data.quaternion[1],
+                    data.quaternion[2],
+                    data.quaternion[3],
                 );
             }
-            Ok(None) => (), // No complete data available yet
-            Err(e) => eprintln!("Error reading from serial port: {}", e),
+            Err(e) => eprintln!("Error reading from IMU: {}", e),
         }
+
+        thread::sleep(Duration::from_millis(10));
     }
 }

imu/hiwonder/src/lib.rs

@@ -1,5 +1,7 @@
 use serialport;
 use std::io::{self, Read};
+use std::sync::{mpsc, Arc, RwLock};
+use std::thread;
 use std::time::Duration;
 
 #[derive(Debug)]
@@ -353,3 +355,159 @@ impl IMU {
         [quaternion_x, quaternion_y, quaternion_z, quaternion_w]
     }
 }
+
+#[derive(Debug, Clone)]
+pub struct ImuData {
+    pub accelerometer: [f32; 3],
+    pub gyroscope: [f32; 3],
+    pub angle: [f32; 3],
+    pub quaternion: [f32; 4],
+}
+
+impl Default for ImuData {
+    fn default() -> Self {
+        ImuData {
+            accelerometer: [0.0; 3],
+            gyroscope: [0.0; 3],
+            angle: [0.0; 3],
+            quaternion: [0.0; 4],
+        }
+    }
+}
+
+pub struct HiwonderReader {
+    data: Arc<RwLock<ImuData>>,
+    command_tx: mpsc::Sender<ImuCommand>,
+    running: Arc<RwLock<bool>>,
+}
+
+#[derive(Debug)]
+pub enum ImuCommand {
+    Reset,
+    Stop,
+    SetFrequency(ImuFrequency),
+}
+
+impl HiwonderReader {
+    pub fn new(interface: &str, baud_rate: u32) -> Result<Self, ImuError> {
+        let data = Arc::new(RwLock::new(ImuData::default()));
+        let running = Arc::new(RwLock::new(true));
+        let (command_tx, command_rx) = mpsc::channel();
+
+        let reader = HiwonderReader {
+            data: Arc::clone(&data),
+            command_tx,
+            running: Arc::clone(&running),
+        };
+
+        reader.start_reading_thread(interface, baud_rate, command_rx)?;
+
+        Ok(reader)
+    }
+
+    fn start_reading_thread(
+        &self,
+        interface: &str,
+        baud_rate: u32,
+        command_rx: mpsc::Receiver<ImuCommand>,
+    ) -> Result<(), ImuError> {
+        let data = Arc::clone(&self.data);
+        let running = Arc::clone(&self.running);
+        let interface = interface.to_string();
+
+        let (tx, rx) = mpsc::channel();
+
+        thread::spawn(move || {
+            // Initialize IMU inside the thread and send result back
+            let init_result = IMU::new(&interface, baud_rate);
+            if let Err(e) = init_result {
+                let _ = tx.send(Err(e));
+                return;
+            }
+            let mut imu = init_result.unwrap();
+            let _ = tx.send(Ok(()));
+
+            while let Ok(guard) = running.read() {
+                if !*guard {
+                    break;
+                }
+
+                // Check for any pending commands
+                if let Ok(command) = command_rx.try_recv() {
+                    match command {
+                        ImuCommand::Reset => {
+                            if let Err(e) = imu.initialize() {
+                                eprintln!("Failed to reset IMU: {}", e);
+                            }
+                        }
+                        ImuCommand::Stop => {
+                            if let Ok(mut guard) = running.write() {
+                                *guard = false;
+                            }
+                            break;
+                        }
+                        ImuCommand::SetFrequency(frequency) => {
+                            if let Err(e) = imu.set_frequency(frequency) {
+                                eprintln!("Failed to set frequency: {}", e);
+                            }
+                        }
+                    }
+                }
+
+                // Read IMU data
+                match imu.read_data() {
+                    Ok(Some((acc, gyro, angle, quat))) => {
+                        if let Ok(mut imu_data) = data.write() {
+                            imu_data.accelerometer = acc;
+                            imu_data.gyroscope = gyro;
+                            imu_data.angle = angle;
+                            imu_data.quaternion = quat;
+                        }
+                    }
+                    Ok(None) => (), // No complete data available yet
+                    Err(e) => eprintln!("Error reading from IMU: {}", e),
+                }
+
+                // Sleep for a short duration to prevent busy waiting
+                // Max frequency is 200hz, so 5ms is the max delay
+                thread::sleep(Duration::from_millis(5));
+            }
+        });
+
+        // Wait for initialization result before returning
+        rx.recv()
+            .map_err(|_| ImuError::InvalidPacket)?
+            .map_err(|e| e)
+    }
+
+    pub fn reset(&self) -> Result<(), ImuError> {
+        self.command_tx
+            .send(ImuCommand::Reset)
+            .map_err(|_| ImuError::WriteError(io::Error::new(io::ErrorKind::Other, "Send error")))
+    }
+
+    pub fn set_frequency(&self, frequency: ImuFrequency) -> Result<(), ImuError> {
+        self.command_tx
+            .send(ImuCommand::SetFrequency(frequency))
+            .map_err(|_| ImuError::WriteError(io::Error::new(io::ErrorKind::Other, "Send error")))
+    }
+
+    pub fn stop(&self) -> Result<(), ImuError> {
+        self.command_tx
+            .send(ImuCommand::Stop)
+            .map_err(|_| ImuError::WriteError(io::Error::new(io::ErrorKind::Other, "Send error")))
+    }
+
+    pub fn get_data(&self) -> Result<ImuData, ImuError> {
+        self.data
+            .read()
+            .map(|data| data.clone())
+            .map_err(|_| ImuError::ReadError(io::Error::new(io::ErrorKind::Other, "Lock error")))
+    }
+}
+
+impl Drop for HiwonderReader {
+    fn drop(&mut self) {
+        let _ = self.stop();
+    }
+}