Skip to content

Commit

Permalink
Hiwonder more funcs (#15)
Browse files Browse the repository at this point in the history 
* progress add quaternion (need to write to RSW)

* Quaterion read working draft, working progress write for RS

* with freq setting

* return data as dataclass

---------

Co-authored-by: Wesley Maa <wesley.maa@gmail.com>
  • Loading branch information
@aaronxie0000 @WT-MM
aaronxie0000 and WT-MM authored Jan 17, 2025
1 parent f292619 commit b5dbc19
Show file tree
Hide file tree
Showing 5 changed files with 177 additions and 18 deletions.
8 changes: 4 additions & 4 deletions examples/hiwonder_example.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,11 @@ def main() -> None:
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}")
print(f"Acceleration (m/s²): {data.accelerometer}")
print(f"Gyroscope (deg/s): {data.gyroscope}")
print(f"Angle (degrees): {data.angle}")
print(f"Quaternion: {data.quaternion}")
time.sleep(0.1) # Add small delay to make output readable
except KeyboardInterrupt:
print("\nExiting...")
Expand Down
48 changes: 45 additions & 3 deletions imu/bindings/src/hiwonder.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,34 @@ use pyo3::prelude::*;
use pyo3_stub_gen::derive::{gen_stub_pyclass, gen_stub_pymethods};
use std::sync::{Arc, Mutex};

#[gen_stub_pyclass]
#[pyclass(name = "ImuData")]
#[derive(Clone)]
struct PyImuData {
#[pyo3(get)]
accelerometer: Vec<f32>,
#[pyo3(get)]
gyroscope: Vec<f32>,
#[pyo3(get)]
angle: Vec<f32>,
#[pyo3(get)]
quaternion: Vec<f32>,
}

#[gen_stub_pymethods]
#[pymethods]
impl PyImuData {
#[new]
fn new(accelerometer: Vec<f32>, gyroscope: Vec<f32>, angle: Vec<f32>, quaternion: Vec<f32>) -> Self {
Self {
accelerometer,
gyroscope,
angle,
quaternion,
}
}
}

#[gen_stub_pyclass]
#[pyclass(name = "HiwonderImu")]
pub struct PyHiwonderImu {
Expand All @@ -21,12 +49,26 @@ impl PyHiwonderImu {
})
}

fn read_data(&mut self) -> PyResult<Option<([f32; 3], [f32; 3], [f32; 3])>> {
fn read_data(&mut self) -> PyResult<Option<PyObject>> {
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()))

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()))
}
}
}
2 changes: 1 addition & 1 deletion imu/hiwonder/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ Link to [IMU](https://www.hiwonder.com/products/imu-module?variant=4037587530555
### HiWonder IMU Protocol
- Data Length: 11 bytes
- Header/start/SOF Byte: 0x55
- Command Byte: 0x51 accel data, 0x52 Gyroscope data, 0x53 angle data
- Command Byte: 0x51 accel data, 0x52 Gyroscope data, 0x53 angle data, 0x59 quaternion data
- Data: 8 bytes
- CheckSum Byte: 1 byte (only lowest 8 bits of summation of all bytes in packet is used (`CheckSum & 0xff`))

Expand Down
24 changes: 19 additions & 5 deletions imu/hiwonder/src/bin/read_hiwonder.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,10 +13,14 @@ struct IMUData {
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])> for IMUData {
fn from((acc, gyro, angle): ([f32; 3], [f32; 3], [f32; 3])) -> Self {
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],
Expand All @@ -27,12 +31,17 @@ impl From<([f32; 3], [f32; 3], [f32; 3])> for IMUData {
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],

}
}
}

fn main() -> io::Result<()> {
let mut imu = IMU::new("/dev/ttyUSB0", 9600)?;
let mut imu = IMU::new("/dev/ttyUSB0", 9600).map_err(|e| io::Error::new(io::ErrorKind::Other, e))?;

loop {
match imu.read_data() {
Expand All @@ -41,7 +50,8 @@ fn main() -> io::Result<()> {
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}",
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,
Expand All @@ -50,7 +60,11 @@ fn main() -> io::Result<()> {
data.gyro_z,
data.angle_x,
data.angle_y,
data.angle_z
data.angle_z,
data.quaternion_x,
data.quaternion_y,
data.quaternion_z,
data.quaternion_w
);
}
Ok(None) => (), // No complete data available yet
Expand Down
113 changes: 108 additions & 5 deletions imu/hiwonder/src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,47 @@ use serialport;
use std::io::{self, Read};
use std::time::Duration;

#[derive(Debug)]
pub enum ImuError {
SerialError(serialport::Error),
WriteError(std::io::Error),
ReadError(std::io::Error),
InvalidPacket,
}

impl From<std::io::Error> for ImuError {
fn from(error: std::io::Error) -> Self {
ImuError::WriteError(error)
}
}

impl From<serialport::Error> for ImuError {
fn from(error: serialport::Error) -> Self {
ImuError::SerialError(error)
}

}

impl std::fmt::Display for ImuError {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
match self {
ImuError::ReadError(e) => write!(f, "Read error: {}", e),
ImuError::WriteError(e) => write!(f, "Write error: {}", e),
ImuError::SerialError(e) => write!(f, "Serial error: {}", e),
ImuError::InvalidPacket => write!(f, "Invalid packet"),
}
}
}

impl std::error::Error for ImuError {}

#[derive(Debug, PartialEq)]
enum FrameState {
Idle,
Acc,
Gyro,
Angle,
Quaternion,
}

pub struct IMU {
Expand All @@ -18,39 +53,81 @@ pub struct IMU {
acc_data: [u8; 8],
gyro_data: [u8; 8],
angle_data: [u8; 8],
quaternion_data: [u8; 8],
acc: [f32; 3],
gyro: [f32; 3],
angle: [f32; 3],
quaternion: [f32; 4],
}


impl IMU {
pub fn new(interface: &str, baud_rate: u32) -> io::Result<Self> {
pub fn new(interface: &str, baud_rate: u32) -> Result<Self, ImuError> {
let port = serialport::new(interface, baud_rate)
.timeout(Duration::from_millis(500))
.open()?;

Ok(IMU {
let mut imu = IMU {
port: port,
frame_state: FrameState::Idle,
byte_num: 0,
checksum: 0,
acc_data: [0u8; 8],
gyro_data: [0u8; 8],
angle_data: [0u8; 8],
quaternion_data: [0u8; 8],
acc: [0.0; 3],
gyro: [0.0; 3],
angle: [0.0; 3],
})
quaternion: [0.0; 4],
};

imu.initialize()?;
Ok(imu)
}

fn initialize(&mut self) -> Result<(), ImuError> {

// * Set IMU Parameters to Read
// Commands as vectors
let unlock_cmd = vec![0xFF, 0xAA, 0x69, 0x88, 0xB5];
let config_cmd = vec![0xFF, 0xAA, 0x02, 0x0E, 0x02];
let save_cmd = vec![0xFF, 0xAA, 0x00, 0x00, 0x00];

// Alternative:
// let mut packet = Vec::with_capacity(5 + data.len());
// packet.push(0x55); // many of these...
// self.port.write_all(&packet)

// Send commands in sequence
self.write_command(&unlock_cmd)?;
self.write_command(&config_cmd)?;
self.write_command(&save_cmd)?;


// * Set IMU Freq
// RATE (0x03) Hz: 0x01=0.2, 0x02=0.5, 0x03=1, 0x04=2, 0x05=5, 0x06=10,
//0x07=20, 0x08=50, 0x09=100, 0x0B=200hz, 0x0C=Single, 0x0D=None
let freq_cmd = vec![0xFF, 0xAA, 0x03, 0x04, 0x00]; // 0x0B 200hz
self.write_command(&freq_cmd)?;
Ok(())
}

fn write_command(&mut self, command: &[u8]) -> Result<(), ImuError> {
self.port.write_all(command).map_err(ImuError::WriteError)?;
// 200 hz -> 5ms
std::thread::sleep(Duration::from_millis(30));
Ok(())
}

pub fn read_data(&mut self) -> io::Result<Option<([f32; 3], [f32; 3], [f32; 3])>> {
pub fn read_data(&mut self) -> io::Result<Option<([f32; 3], [f32; 3], [f32; 3], [f32; 4])>> {
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)))
Ok(Some((self.acc, self.gyro, self.angle, self.quaternion)))
} else {
Ok(None)
}
Expand Down Expand Up @@ -83,12 +160,18 @@ impl IMU {
self.frame_state = FrameState::Angle;
self.byte_num = 2;
}
0x59 => {
// println!("frame_state: {:?}", self.frame_state);
self.frame_state = FrameState::Quaternion;
self.byte_num = 2;
}
_ => {
self.reset();
}
}
}
}
// 11 bytes per packet for all, including the SOF.
FrameState::Acc => {
if self.byte_num < 10 {
self.acc_data[self.byte_num - 2] = data;
Expand Down Expand Up @@ -125,6 +208,18 @@ impl IMU {
self.reset();
}
}
FrameState::Quaternion => {
if self.byte_num < 10 {
self.quaternion_data[self.byte_num - 2] = data;
self.checksum = self.checksum.wrapping_add(data);
self.byte_num += 1;
} else {
if data == (self.checksum & 0xFF) {
self.quaternion = Self::get_quaternion(&self.quaternion_data);
}
self.reset();
}
}
}
}
}
Expand Down Expand Up @@ -215,4 +310,12 @@ impl IMU {
},
]
}

fn get_quaternion(datahex: &[u8; 8]) -> [f32; 4] {
let quaternion_x = ((u16::from(datahex[1]) << 8) | u16::from(datahex[0])) as f32 / 32768.0;
let quaternion_y = ((u16::from(datahex[3]) << 8) | u16::from(datahex[2])) as f32 / 32768.0;
let quaternion_z = ((u16::from(datahex[5]) << 8) | u16::from(datahex[4])) as f32 / 32768.0;
let quaternion_w = ((u16::from(datahex[7]) << 8) | u16::from(datahex[6])) as f32 / 32768.0;
[quaternion_x, quaternion_y, quaternion_z, quaternion_w]
}
}

0 comments on commit b5dbc19

Please sign in to comment.