Skip to content

Commit

Permalink
Remove risk of thread panic (#6)
Browse files Browse the repository at this point in the history 
* denys

* error check

* fix error propagation

* format

* bump version to 0.1.5
  • Loading branch information
@WT-MM
WT-MM authored Nov 15, 2024
1 parent 30d77f7 commit c0fe2e5
Show file tree
Hide file tree
Showing 5 changed files with 121 additions and 63 deletions.
4 changes: 4 additions & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,3 +27,7 @@ Cargo.lock

# CSV files
*.csv

.DS_Store

imu/hexmove/ref/*
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.4"
version = "0.1.5"
authors = ["Wesley Maa <[email protected]>"]
edition = "2021"
description = "IMU package"
Expand Down
36 changes: 28 additions & 8 deletions imu/bindings/src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,20 +23,35 @@ impl PyHexmoveImuReader {
}

fn get_data(&self) -> PyResult<PyHexmoveImuData> {
let imu_reader = self.inner.lock().unwrap();
let data = imu_reader.get_data();
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().unwrap();
let (x, y, z) = imu_reader.get_angles();
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().unwrap();
let (x, y, z) = imu_reader.get_velocities();
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))
}

Expand All @@ -54,8 +69,13 @@ impl PyHexmoveImuReader {
}

fn stop(&self) -> PyResult<()> {
let imu_reader = self.inner.lock().unwrap();
imu_reader.stop();
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(())
}
}
Expand Down
15 changes: 6 additions & 9 deletions imu/hexmove/src/bin/log.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,19 +2,16 @@ use hexmove::ImuReader;
use std::thread;
use std::time::Duration;

fn main() {
fn main() -> Result<(), String> {
// Create a new ImuReader for the 'can0' interface
let imu_reader = match ImuReader::new("can0", 1, 1) {
Ok(reader) => reader,
Err(e) => {
eprintln!("Failed to initialize IMU reader: {}", e);
return;
}
};
let imu_reader = ImuReader::new("can0", 1, 1)
.map_err(|e| format!("Failed to initialize IMU reader: {}", e))?;

// Continuously read and print IMU data
loop {
let data = imu_reader.get_data();
let data = imu_reader
.get_data()
.map_err(|e| format!("Failed to get IMU data: {}", e))?;
println!(
"Angular Position: X={}°, Y={}°, Z={}° | Angular Velocity: X={}°/s, Y={}°/s, Z={}°/s",
data.x_angle,
Expand Down
127 changes: 82 additions & 45 deletions imu/hexmove/src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,17 @@ impl ImuReader {
let socket = Arc::clone(&self.socket);

thread::spawn(move || {
while *running.read().unwrap() {
loop {
// Check if we should continue running
if let Ok(guard) = running.read() {
if !*guard {
break;
}
} else {
error!("Failed to acquire read lock");
break;
}

match socket.read_frame() {
Ok(CanFrame::Data(data_frame)) => {
let received_data = data_frame.data();
Expand All @@ -58,38 +68,52 @@ impl ImuReader {
let base_id =
0x0B000000 | (serial_number as u32) << 16 | (model as u32) << 8;

if id == Id::Extended(ExtendedId::new(base_id | 0xB1).unwrap()) {
let x_angle = i16::from_le_bytes([received_data[0], received_data[1]])
as f32
* 0.01;
let y_angle = i16::from_le_bytes([received_data[2], received_data[3]])
as f32
* 0.01;
let z_angle = i16::from_le_bytes([received_data[4], received_data[5]])
as f32
* 0.01;

let mut imu_data = data.write().unwrap();
imu_data.x_angle = x_angle;
imu_data.y_angle = y_angle;
imu_data.z_angle = z_angle;
if let Some(ext_id) = ExtendedId::new(base_id | 0xB1) {
if id == Id::Extended(ext_id) {
let x_angle =
i16::from_le_bytes([received_data[0], received_data[1]]) as f32
* 0.01;
let y_angle =
i16::from_le_bytes([received_data[2], received_data[3]]) as f32
* 0.01;
let z_angle =
i16::from_le_bytes([received_data[4], received_data[5]]) as f32
* 0.01;

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

if id == Id::Extended(ExtendedId::new(base_id | 0xB2).unwrap()) {
let x_velocity =
i16::from_le_bytes([received_data[0], received_data[1]]) as f32
* 0.01;
let y_velocity =
i16::from_le_bytes([received_data[2], received_data[3]]) as f32
* 0.01;
let z_velocity =
i16::from_le_bytes([received_data[4], received_data[5]]) as f32
* 0.01;

let mut imu_data = data.write().unwrap();
imu_data.x_velocity = x_velocity;
imu_data.y_velocity = y_velocity;
imu_data.z_velocity = z_velocity;
if let Some(ext_id) = ExtendedId::new(base_id | 0xB2) {
if id == Id::Extended(ext_id) {
let x_velocity =
i16::from_le_bytes([received_data[0], received_data[1]]) as f32
* 0.01;
let y_velocity =
i16::from_le_bytes([received_data[2], received_data[3]]) as f32
* 0.01;
let z_velocity =
i16::from_le_bytes([received_data[4], received_data[5]]) as f32
* 0.01;

if let Ok(mut imu_data) = data.write() {
imu_data.x_velocity = x_velocity;
imu_data.y_velocity = y_velocity;
imu_data.z_velocity = z_velocity;
} else {
error!("Failed to write to IMU data");
}
}
} else {
error!("Failed to create extended ID for IMU velocity data");
}
}
Ok(CanFrame::Remote(_)) => {
Expand All @@ -106,27 +130,35 @@ impl ImuReader {
});
}

pub fn get_data(&self) -> ImuData {
self.data.read().unwrap().clone()
pub fn get_data(&self) -> Result<ImuData, String> {
let imu_data = self
.data
.read()
.map_err(|e| format!("Failed to acquire read lock: {}", e))?;
Ok(imu_data.clone())
}

pub fn get_angles(&self) -> (f32, f32, f32) {
let data = self.get_data();
(
pub fn get_angles(&self) -> Result<(f32, f32, f32), String> {
let data = self.get_data()?;
Ok((
data.x_angle - data.x_angle_offset,
data.y_angle - data.y_angle_offset,
data.z_angle - data.z_angle_offset,
)
))
}

pub fn get_velocities(&self) -> (f32, f32, f32) {
let data = self.get_data();
(data.x_velocity, data.y_velocity, data.z_velocity)
pub fn get_velocities(&self) -> Result<(f32, f32, f32), String> {
let data = self.get_data()?;
Ok((data.x_velocity, data.y_velocity, data.z_velocity))
}

pub fn stop(&self) {
let mut running = self.running.write().unwrap();
pub fn stop(&self) -> Result<(), String> {
let mut running = self
.running
.write()
.map_err(|e| format!("Failed to acquire write lock: {}", e))?;
*running = false;
Ok(())
}

pub fn zero_imu(
Expand All @@ -147,7 +179,7 @@ impl ImuReader {

// Collect samples
for _ in 0..samples {
let data = self.get_data();
let data = self.get_data()?;
x_samples.push(data.x_angle);
y_samples.push(data.y_angle);
z_samples.push(data.z_angle);
Expand All @@ -174,7 +206,12 @@ impl ImuReader {
let z_offset = z_samples.iter().sum::<f32>() / samples as f32;

// Update offsets
let mut imu_data = self.data.write().unwrap();

let mut imu_data = self
.data
.write()
.map_err(|e| format!("Failed to acquire write lock: {}", e))?;

imu_data.x_angle_offset = x_offset;
imu_data.y_angle_offset = y_offset;
imu_data.z_angle_offset = z_offset;
Expand All @@ -200,6 +237,6 @@ fn calculate_variance(x: &[f32], y: &[f32], z: &[f32]) -> (f32, f32, f32) {

impl Drop for ImuReader {
fn drop(&mut self) {
self.stop();
let _ = self.stop();
}
}

0 comments on commit c0fe2e5

Please sign in to comment.