Skip to content

Commit

Permalink
More hexmove data (#7)
Browse files Browse the repository at this point in the history 
* add accel and quat

* better naming

* more naming

* format

* bump version to 0.1.6
  • Loading branch information
@WT-MM
WT-MM authored Nov 15, 2024
1 parent c0fe2e5 commit 00bb5a6
Show file tree
Hide file tree
Showing 3 changed files with 152 additions and 1 deletion.
2 changes: 1 addition & 1 deletion Cargo.toml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ resolver = "2"

[workspace.package]

version = "0.1.5"
version = "0.1.6"
authors = ["Wesley Maa <[email protected]>"]
edition = "2021"
description = "IMU package"
Expand Down
43 changes: 43 additions & 0 deletions imu/bindings/src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,28 @@ impl PyHexmoveImuReader {
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,
Expand Down Expand Up @@ -102,6 +124,20 @@ struct PyHexmoveImuData {
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 {
Expand All @@ -116,6 +152,13 @@ impl From<HexmoveImuData> for PyHexmoveImuData {
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,
}
}
}
Expand Down
108 changes: 108 additions & 0 deletions imu/hexmove/src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,13 @@ pub struct ImuData {
pub x_angle_offset: f32,
pub y_angle_offset: f32,
pub z_angle_offset: f32,
pub accel_x: f32,
pub accel_y: f32,
pub accel_z: f32,
pub qw: f32,
pub qx: f32,
pub qy: f32,
pub qz: f32,
}

pub struct ImuReader {
Expand Down Expand Up @@ -68,6 +75,7 @@ impl ImuReader {
let base_id =
0x0B000000 | (serial_number as u32) << 16 | (model as u32) << 8;

// IMU angle data
if let Some(ext_id) = ExtendedId::new(base_id | 0xB1) {
if id == Id::Extended(ext_id) {
let x_angle =
Expand All @@ -92,6 +100,7 @@ impl ImuReader {
error!("Failed to create extended ID for IMU data");
}

// IMU velocity data
if let Some(ext_id) = ExtendedId::new(base_id | 0xB2) {
if id == Id::Extended(ext_id) {
let x_velocity =
Expand All @@ -115,6 +124,95 @@ impl ImuReader {
} else {
error!("Failed to create extended ID for IMU velocity data");
}

// IMU acceleration data (m/s^2)
if let Some(ext_id) = ExtendedId::new(base_id | 0xB3) {
if id == Id::Extended(ext_id) {
let accel_x =
i16::from_le_bytes([received_data[0], received_data[1]]) as f32
* 0.01;
let accel_y =
i16::from_le_bytes([received_data[2], received_data[3]]) as f32
* 0.01;
let accel_z =
i16::from_le_bytes([received_data[4], received_data[5]]) as f32
* 0.01;

if let Ok(mut imu_data) = data.write() {
imu_data.accel_x = accel_x;
imu_data.accel_y = accel_y;
imu_data.accel_z = accel_z;
} else {
error!("Failed to write to IMU data");
}
}
} else {
error!("Failed to create extended ID for IMU acceleration data");
}

// IMU quaternion data
if let Some(ext_id) = ExtendedId::new(base_id | 0xB4) {
if id == Id::Extended(ext_id) {
// Parse quaternion w component from first 4 bytes
let qw_bytes: [u8; 4] = [
received_data[0],
received_data[1],
received_data[2],
received_data[3],
];
let qw = f32::from_le_bytes(qw_bytes);

// Parse quaternion x component from last 4 bytes
let qx_bytes: [u8; 4] = [
received_data[4],
received_data[5],
received_data[6],
received_data[7],
];
let qx = f32::from_le_bytes(qx_bytes);

if let Ok(mut imu_data) = data.write() {
imu_data.qw = qw;
imu_data.qx = qx;
} else {
error!("Failed to write quaternion data to IMU data");
}
}
} else {
error!("Failed to create extended ID for IMU quaternion data");
}

// IMU quaternion data
if let Some(ext_id) = ExtendedId::new(base_id | 0xB5) {
if id == Id::Extended(ext_id) {
// Parse quaternion y component from first 4 bytes
let qy_bytes: [u8; 4] = [
received_data[0],
received_data[1],
received_data[2],
received_data[3],
];
let qy = f32::from_le_bytes(qy_bytes);

// Parse quaternion z component from last 4 bytes
let qz_bytes: [u8; 4] = [
received_data[4],
received_data[5],
received_data[6],
received_data[7],
];
let qz = f32::from_le_bytes(qz_bytes);

if let Ok(mut imu_data) = data.write() {
imu_data.qy = qy;
imu_data.qz = qz;
} else {
error!("Failed to write quaternion data to IMU data");
}
}
} else {
error!("Failed to create extended ID for IMU quaternion data");
}
}
Ok(CanFrame::Remote(_)) => {
// Ignore remote frames
Expand Down Expand Up @@ -152,6 +250,16 @@ impl ImuReader {
Ok((data.x_velocity, data.y_velocity, data.z_velocity))
}

pub fn get_accelerations(&self) -> Result<(f32, f32, f32), String> {
let data = self.get_data()?;
Ok((data.accel_x, data.accel_y, data.accel_z))
}

pub fn get_quaternion(&self) -> Result<(f32, f32, f32, f32), String> {
let data = self.get_data()?;
Ok((data.qw, data.qx, data.qy, data.qz))
}

pub fn stop(&self) -> Result<(), String> {
let mut running = self
.running
Expand Down

0 comments on commit 00bb5a6

Please sign in to comment.