hiwonder imu, refactor and return data as struct

imu/hiwonder/Cargo.toml

@@ -4,6 +4,4 @@ version = "0.1.0"
 edition = "2021"
 
 [dependencies]
-serialport = "4.2.0"
-lazy_static = "1.4.0"
-tokio = { version = "1.0", features = ["full"] }
+serialport = "4.2.0"

imu/hiwonder/src/bin/log.rs

@@ -1,32 +1,55 @@
 use hiwonder::IMU;
-use std::time::Duration;
-use std::io::{self, Read};
+use std::io;
 
 //* run by  `cargo run --bin log` */
 
-fn main() -> io::Result<()> {    
-    let port_name = "/dev/ttyUSB0";
-    let baud_rate = 9600;
-
-    let mut port = serialport::new(port_name, baud_rate)
-        .timeout(Duration::from_millis(500)) // 0.5 second timeout
-        .open()
-        .expect("Failed to open serial port");
+#[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],
+        }
+    }
+}
 
-    let mut imu = IMU::new();
-    let mut buffer: Vec<u8> = vec![0; 1024];
+fn main() -> io::Result<()> {    
+    let mut imu = IMU::new("/dev/ttyUSB0", 9600)?;
 
     loop {
-        match port.read(&mut buffer) {
-            Ok(bytes_read) => {
-                if bytes_read > 0 {
-                    imu.process_data(&buffer[..bytes_read]);
-                }
-            }
-            Err(e) => {
-                eprintln!("Error reading from serial port: {}", e);
+        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),
         }
     }
 }

imu/hiwonder/src/lib.rs

@@ -1,6 +1,8 @@
+use std::io::{self, Read};
+use std::time::Duration;
+use serialport;
 
-
-#[derive(Debug)]
+#[derive(Debug, PartialEq)]
 enum FrameState {
     Idle,
     Acc,
@@ -9,6 +11,7 @@ enum FrameState {
 }
 
 pub struct IMU {
+    port: Box<dyn serialport::SerialPort>,
     frame_state: FrameState,
     byte_num: usize,
     checksum: u8,
@@ -20,9 +23,16 @@ pub struct IMU {
     angle: [f32; 3],
 }
 
+
+
 impl IMU {
-    pub fn new() -> Self {
-        IMU {
+    pub fn new(interface: &str, baud_rate: u32) -> io::Result<Self> {
+        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,
@@ -32,6 +42,23 @@ impl IMU {
             acc: [0.0; 3],
             gyro: [0.0; 3],
             angle: [0.0; 3],
+        })
+    }
+
+    pub fn read_data(&mut self) -> io::Result<Option<([f32; 3], [f32; 3], [f32; 3])>> {
+        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),
         }
     }