diff --git a/Cargo.toml b/Cargo.toml index 6f4af7aab..3e46c056e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -13,6 +13,7 @@ members = [ "applications/awkernel_services", "applications/awkernel_shell", "applications/awkernel_display", + "applications/autoware", "applications/rd_gen_to_dags", "applications/tests/*", "smoltcp", diff --git a/applications/autoware/Cargo.toml b/applications/autoware/Cargo.toml new file mode 100644 index 000000000..23be165ec --- /dev/null +++ b/applications/autoware/Cargo.toml @@ -0,0 +1,17 @@ +[package] +name = "autoware" +version = "0.1.0" +edition = "2021" + +[lib] +path = "src/lib.rs" +crate-type = ["rlib"] + +[dependencies] +log = "0.4" +libm = "0.2" +csv-core = "0.1" +awkernel_async_lib = { path = "../../awkernel_async_lib", default-features = false } +awkernel_lib = { path = "../../awkernel_lib", default-features = false } +imu_driver = { path = "./imu_driver", default-features = false } +imu_corrector = { path = "./imu_corrector", default-features = false } diff --git a/applications/autoware/imu_corrector/Cargo.toml b/applications/autoware/imu_corrector/Cargo.toml new file mode 100644 index 000000000..0397eed62 --- /dev/null +++ b/applications/autoware/imu_corrector/Cargo.toml @@ -0,0 +1,8 @@ +[package] +name = "imu_corrector" +version = "0.1.0" +edition = "2021" + +[dependencies] +nalgebra = { version = "0.32", default-features = false, features = ["libm"] } +imu_driver = { path = "../imu_driver", default-features = false } \ No newline at end of file diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs new file mode 100644 index 000000000..ff61b769b --- /dev/null +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -0,0 +1,488 @@ +// Ported from the following versions of the original C++ code: +// universe/autoware_universe: +// type: git +// url: https://github.com/autowarefoundation/autoware_universe.git +// version: 0.51.0 + +#![no_std] +extern crate alloc; + +use alloc::{format, string::String}; +use imu_driver::{Header, ImuMsg, Quaternion, Vector3}; +use nalgebra::{Quaternion as NQuaternion, UnitQuaternion, Vector3 as NVector3}; + +#[derive(Clone, Debug)] +pub struct Transform { + pub translation: Vector3, + pub rotation: Quaternion, +} + +impl Transform { + pub fn identity() -> Self { + Self { + translation: imu_driver::Vector3::new(0.0, 0.0, 0.0), + rotation: imu_driver::Quaternion { + x: 0.0, + y: 0.0, + z: 0.0, + w: 1.0, + }, + } + } + + fn to_nalgebra_vector3(&self, vec: &Vector3) -> NVector3 { + NVector3::new(vec.x, vec.y, vec.z) + } + + fn to_imu_vector3(&self, vec: &NVector3) -> Vector3 { + Vector3::new(vec.x, vec.y, vec.z) + } + + fn to_nalgebra_quaternion(&self, quat: &Quaternion) -> UnitQuaternion { + let n_quat = NQuaternion::new(quat.w, quat.x, quat.y, quat.z); + UnitQuaternion::from_quaternion(n_quat) + } + + pub fn apply_to_vector(&self, vec: Vector3) -> Vector3 { + let nalgebra_vec = self.to_nalgebra_vector3(&vec); + let nalgebra_quat = self.to_nalgebra_quaternion(&self.rotation); + let nalgebra_trans = self.to_nalgebra_vector3(&self.translation); + let rotated = nalgebra_quat * nalgebra_vec; + let result = rotated + nalgebra_trans; + self.to_imu_vector3(&result) + } +} + +pub trait TransformListener { + fn get_latest_transform(&self, from_frame: &str, to_frame: &str) -> Option; + fn get_transform_at_time( + &self, + from_frame: &str, + to_frame: &str, + timestamp: u64, + ) -> Option; +} + +pub struct MockTransformListener { + transforms: alloc::collections::BTreeMap, +} + +impl MockTransformListener { + pub fn new() -> Self { + Self { + transforms: alloc::collections::BTreeMap::new(), + } + } + + pub fn add_transform(&mut self, from_frame: &str, to_frame: &str, transform: Transform) { + let key = format!("{}_to_{}", from_frame, to_frame); + self.transforms.insert(key, transform); + } +} + +impl TransformListener for MockTransformListener { + fn get_latest_transform(&self, from_frame: &str, to_frame: &str) -> Option { + let key = format!("{}_to_{}", from_frame, to_frame); + self.transforms.get(&key).cloned() + } + + fn get_transform_at_time( + &self, + from_frame: &str, + to_frame: &str, + _timestamp: u64, + ) -> Option { + self.get_latest_transform(from_frame, to_frame) + } +} + +#[derive(Clone, Debug)] +pub struct ImuWithCovariance { + pub header: Header, + pub orientation: Quaternion, + pub angular_velocity: Vector3, + pub angular_velocity_covariance: [f64; 9], + pub linear_acceleration: Vector3, + pub linear_acceleration_covariance: [f64; 9], +} + +impl ImuWithCovariance { + pub fn from_imu_msg(imu_msg: &ImuMsg) -> Self { + Self { + header: imu_msg.header.clone(), + orientation: imu_msg.orientation.clone(), + angular_velocity: imu_msg.angular_velocity.clone(), + angular_velocity_covariance: [0.0; 9], + linear_acceleration: imu_msg.linear_acceleration.clone(), + linear_acceleration_covariance: [0.0; 9], + } + } + + pub fn to_imu_msg(&self) -> ImuMsg { + ImuMsg { + header: self.header.clone(), + orientation: self.orientation.clone(), + angular_velocity: self.angular_velocity.clone(), + linear_acceleration: self.linear_acceleration.clone(), + } + } +} + +pub struct ImuCorrectorConfig { + pub angular_velocity_offset_x: f64, + pub angular_velocity_offset_y: f64, + pub angular_velocity_offset_z: f64, + pub angular_velocity_stddev_xx: f64, + pub angular_velocity_stddev_yy: f64, + pub angular_velocity_stddev_zz: f64, + pub acceleration_stddev: f64, + pub output_frame: &'static str, +} + +impl Default for ImuCorrectorConfig { + fn default() -> Self { + Self { + angular_velocity_offset_x: 0.0, + angular_velocity_offset_y: 0.0, + angular_velocity_offset_z: 0.0, + angular_velocity_stddev_xx: 0.03, + angular_velocity_stddev_yy: 0.03, + angular_velocity_stddev_zz: 0.03, + acceleration_stddev: 10000.0, + output_frame: "base_link", + } + } +} + +pub struct ImuCorrector { + config: ImuCorrectorConfig, + transform_listener: T, +} + +impl ImuCorrector { + pub fn new() -> Self { + Self { + config: ImuCorrectorConfig::default(), + transform_listener: MockTransformListener::new(), + } + } + + pub fn with_transform_listener(transform_listener: MockTransformListener) -> Self { + Self { + config: ImuCorrectorConfig::default(), + transform_listener, + } + } +} + +impl ImuCorrector { + pub fn set_config(&mut self, config: ImuCorrectorConfig) { + self.config = config; + } + + fn to_nalgebra_vector3(&self, vec: &Vector3) -> NVector3 { + NVector3::new(vec.x, vec.y, vec.z) + } + + fn to_imu_vector3(&self, vec: &NVector3) -> Vector3 { + Vector3::new(vec.x, vec.y, vec.z) + } + + fn to_nalgebra_quaternion(&self, quat: &Quaternion) -> UnitQuaternion { + let n_quat = NQuaternion::new(quat.w, quat.x, quat.y, quat.z); + UnitQuaternion::from_quaternion(n_quat) + } + + fn transform_vector3(&self, vec: &Vector3, transform: &Transform) -> Vector3 { + let nalgebra_vec = self.to_nalgebra_vector3(vec); + let nalgebra_quat = self.to_nalgebra_quaternion(&transform.rotation); + let rotated = nalgebra_quat * nalgebra_vec; + self.to_imu_vector3(&rotated) + } + + fn transform_covariance(&self, cov: &[f64; 9]) -> [f64; 9] { + let max_cov = cov[0].max(cov[4]).max(cov[8]); + let mut cov_transformed = [0.0; 9]; + cov_transformed[0] = max_cov; + cov_transformed[4] = max_cov; + cov_transformed[8] = max_cov; + + cov_transformed + } + + pub fn correct_imu_with_covariance( + &self, + imu_msg: &ImuMsg, + transform: Option<&Transform>, + ) -> ImuWithCovariance { + let mut corrected_imu = ImuWithCovariance::from_imu_msg(imu_msg); + corrected_imu.angular_velocity.x -= self.config.angular_velocity_offset_x; + corrected_imu.angular_velocity.y -= self.config.angular_velocity_offset_y; + corrected_imu.angular_velocity.z -= self.config.angular_velocity_offset_z; + corrected_imu.angular_velocity_covariance[0] = + self.config.angular_velocity_stddev_xx * self.config.angular_velocity_stddev_xx; + corrected_imu.angular_velocity_covariance[4] = + self.config.angular_velocity_stddev_yy * self.config.angular_velocity_stddev_yy; + corrected_imu.angular_velocity_covariance[8] = + self.config.angular_velocity_stddev_zz * self.config.angular_velocity_stddev_zz; + let accel_var = self.config.acceleration_stddev * self.config.acceleration_stddev; + corrected_imu.linear_acceleration_covariance[0] = accel_var; + corrected_imu.linear_acceleration_covariance[4] = accel_var; + corrected_imu.linear_acceleration_covariance[8] = accel_var; + + if let Some(tf) = transform { + corrected_imu.linear_acceleration = + self.transform_vector3(&corrected_imu.linear_acceleration, tf); + corrected_imu.linear_acceleration_covariance = + self.transform_covariance(&corrected_imu.linear_acceleration_covariance); + corrected_imu.angular_velocity = + self.transform_vector3(&corrected_imu.angular_velocity, tf); + corrected_imu.angular_velocity_covariance = + self.transform_covariance(&corrected_imu.angular_velocity_covariance); + corrected_imu.header.frame_id = self.config.output_frame; + } + + corrected_imu + } + + pub fn correct_imu(&self, imu_msg: &ImuMsg, transform: Option<&Transform>) -> ImuMsg { + let corrected_with_cov = self.correct_imu_with_covariance(imu_msg, transform); + corrected_with_cov.to_imu_msg() + } + + pub fn correct_imu_with_dynamic_tf(&self, imu_msg: &ImuMsg) -> Option { + let transform = self + .transform_listener + .get_latest_transform(&imu_msg.header.frame_id, self.config.output_frame)?; + + let corrected_with_cov = self.correct_imu_with_covariance(imu_msg, Some(&transform)); + Some(corrected_with_cov.to_imu_msg()) + } + + pub fn correct_imu_with_dynamic_tf_covariance( + &self, + imu_msg: &ImuMsg, + ) -> Option { + let transform = self + .transform_listener + .get_latest_transform(&imu_msg.header.frame_id, self.config.output_frame)?; + + Some(self.correct_imu_with_covariance(imu_msg, Some(&transform))) + } + + pub fn correct_imu_simple(&self, imu_msg: &ImuMsg) -> ImuMsg { + let mut corrected_msg = imu_msg.clone(); + + corrected_msg.angular_velocity.x -= self.config.angular_velocity_offset_x; + corrected_msg.angular_velocity.y -= self.config.angular_velocity_offset_y; + corrected_msg.angular_velocity.z -= self.config.angular_velocity_offset_z; + + corrected_msg.header.frame_id = self.config.output_frame; + + corrected_msg + } + + pub fn get_covariance_config(&self) -> (f64, f64, f64, f64) { + ( + self.config.angular_velocity_stddev_xx, + self.config.angular_velocity_stddev_yy, + self.config.angular_velocity_stddev_zz, + self.config.acceleration_stddev, + ) + } +} + +pub fn transform_covariance(cov: &[f64; 9]) -> [f64; 9] { + let max_cov = cov[0].max(cov[4]).max(cov[8]); + let mut cov_transformed = [0.0; 9]; + cov_transformed[0] = max_cov; + cov_transformed[4] = max_cov; + cov_transformed[8] = max_cov; + cov_transformed +} + +#[cfg(test)] +mod tests { + use super::*; + + fn assert_almost_eq(actual: f64, expected: f64) { + assert!((actual - expected).abs() < 1e-10); + } + + fn sample_imu_msg() -> ImuMsg { + ImuMsg { + header: Header { + frame_id: "imu_link", + timestamp: 0, + }, + orientation: Quaternion { + x: 0.0, + y: 0.0, + z: 0.0, + w: 1.0, + }, + angular_velocity: Vector3::new(0.1, 0.2, 0.3), + linear_acceleration: Vector3::new(9.8, 0.0, 0.0), + } + } + + #[test] + fn static_bias_correction_updates_angular_velocity() { + let mut config = ImuCorrectorConfig::default(); + config.angular_velocity_offset_x = 0.05; + config.angular_velocity_offset_y = 0.1; + config.angular_velocity_offset_z = 0.15; + + let mut corrector = ImuCorrector::new(); + corrector.set_config(config); + + let imu_msg = sample_imu_msg(); + let corrected = corrector.correct_imu(&imu_msg, None); + + assert_eq!(corrected.angular_velocity.x, 0.05); + assert_eq!(corrected.angular_velocity.y, 0.1); + assert_eq!(corrected.angular_velocity.z, 0.15); + assert_eq!(corrected.header.frame_id, "imu_link"); + } + + #[test] + fn simple_path_sets_output_frame() { + let corrector = ImuCorrector::new(); + let imu_msg = sample_imu_msg(); + + let corrected_msg = corrector.correct_imu_simple(&imu_msg); + + assert_eq!(corrected_msg.header.frame_id, "base_link"); + assert_eq!(corrected_msg.angular_velocity.x, 0.1); + assert_eq!(corrected_msg.angular_velocity.y, 0.2); + assert_eq!(corrected_msg.angular_velocity.z, 0.3); + } + + #[test] + fn covariance_defaults_are_set_from_config() { + let corrector = ImuCorrector::new(); + let imu_msg = sample_imu_msg(); + + let corrected_with_cov = corrector.correct_imu_with_covariance(&imu_msg, None); + let expected_angular_var = 0.03 * 0.03; + assert_eq!( + corrected_with_cov.angular_velocity_covariance[0], + expected_angular_var + ); + assert_eq!( + corrected_with_cov.angular_velocity_covariance[4], + expected_angular_var + ); + assert_eq!( + corrected_with_cov.angular_velocity_covariance[8], + expected_angular_var + ); + let expected_accel_var = 10000.0 * 10000.0; + assert_eq!( + corrected_with_cov.linear_acceleration_covariance[0], + expected_accel_var + ); + assert_eq!( + corrected_with_cov.linear_acceleration_covariance[4], + expected_accel_var + ); + assert_eq!( + corrected_with_cov.linear_acceleration_covariance[8], + expected_accel_var + ); + } + + #[test] + fn covariance_transform_uses_max_diagonal() { + let corrector = ImuCorrector::new(); + let input_cov = [1.0, 0.5, 0.3, 0.5, 2.0, 0.4, 0.3, 0.4, 3.0]; + let transformed_cov = corrector.transform_covariance(&input_cov); + + assert_eq!(transformed_cov[0], 3.0); + assert_eq!(transformed_cov[4], 3.0); + assert_eq!(transformed_cov[8], 3.0); + assert_eq!(transformed_cov[1], 0.0); + assert_eq!(transformed_cov[2], 0.0); + assert_eq!(transformed_cov[3], 0.0); + assert_eq!(transformed_cov[5], 0.0); + assert_eq!(transformed_cov[6], 0.0); + assert_eq!(transformed_cov[7], 0.0); + } + + #[test] + fn public_covariance_transform_uses_max_diagonal() { + let input_cov = [1.0, 0.5, 0.3, 0.5, 2.0, 0.4, 0.3, 0.4, 3.0]; + let transformed_cov = transform_covariance(&input_cov); + assert_eq!(transformed_cov[0], 3.0); + assert_eq!(transformed_cov[4], 3.0); + assert_eq!(transformed_cov[8], 3.0); + assert_eq!(transformed_cov[1], 0.0); + assert_eq!(transformed_cov[2], 0.0); + assert_eq!(transformed_cov[3], 0.0); + assert_eq!(transformed_cov[5], 0.0); + assert_eq!(transformed_cov[6], 0.0); + assert_eq!(transformed_cov[7], 0.0); + } + + #[test] + fn transform_applies_to_vectors_and_sets_output_frame() { + let corrector = ImuCorrector::new(); + let imu_msg = sample_imu_msg(); + + let transform = Transform { + translation: Vector3::new(1.0, 2.0, 3.0), + rotation: Quaternion { + x: 0.0, + y: 0.0, + z: 0.7071067811865475, + w: 0.7071067811865476, + }, + }; + + let corrected_with_cov = corrector.correct_imu_with_covariance(&imu_msg, Some(&transform)); + assert_eq!(corrected_with_cov.header.frame_id, "base_link"); + assert_almost_eq(corrected_with_cov.linear_acceleration.x, 0.0); + assert_almost_eq(corrected_with_cov.linear_acceleration.y, 9.8); + assert_almost_eq(corrected_with_cov.linear_acceleration.z, 0.0); + assert_almost_eq(corrected_with_cov.angular_velocity.x, -0.2); + assert_almost_eq(corrected_with_cov.angular_velocity.y, 0.1); + assert_almost_eq(corrected_with_cov.angular_velocity.z, 0.3); + } + + #[test] + fn dynamic_tf_path_applies_transform() { + let mut transform_listener = MockTransformListener::new(); + let transform = Transform { + translation: Vector3::new(1.0, 0.0, 0.0), + rotation: Quaternion { + x: 0.0, + y: 0.0, + z: 0.0, + w: 1.0, + }, + }; + transform_listener.add_transform("imu_link", "base_link", transform); + + let corrector = ImuCorrector::with_transform_listener(transform_listener); + let imu_msg = sample_imu_msg(); + + let corrected_msg = corrector.correct_imu_with_dynamic_tf(&imu_msg); + assert!(corrected_msg.is_some()); + + let corrected_msg = corrected_msg.unwrap(); + assert_eq!(corrected_msg.header.frame_id, "base_link"); + assert_eq!(corrected_msg.linear_acceleration.x, 9.8); + assert_eq!(corrected_msg.linear_acceleration.y, 0.0); + assert_eq!(corrected_msg.linear_acceleration.z, 0.0); + } + + #[test] + fn dynamic_tf_path_returns_none_when_transform_is_missing() { + let transform_listener = MockTransformListener::new(); + let corrector = ImuCorrector::with_transform_listener(transform_listener); + let imu_msg = sample_imu_msg(); + + let corrected_msg = corrector.correct_imu_with_dynamic_tf(&imu_msg); + assert!(corrected_msg.is_none()); + } +} diff --git a/applications/autoware/imu_driver/Cargo.toml b/applications/autoware/imu_driver/Cargo.toml new file mode 100644 index 000000000..da816a55f --- /dev/null +++ b/applications/autoware/imu_driver/Cargo.toml @@ -0,0 +1,6 @@ +[package] +name = "imu_driver" +version = "0.1.0" +edition = "2021" + +[dependencies] diff --git a/applications/autoware/imu_driver/src/lib.rs b/applications/autoware/imu_driver/src/lib.rs new file mode 100644 index 000000000..36cf39679 --- /dev/null +++ b/applications/autoware/imu_driver/src/lib.rs @@ -0,0 +1,362 @@ +// Ported from the following versions of the original C++ code: +// tamagawa_imu_driver +// type: git +// url: https://github.com/tier4/tamagawa_imu_driver +// version: 0.1.0 + +#![no_std] +extern crate alloc; + +use alloc::{format, string::String, vec, vec::Vec}; +use core::f64::consts::PI; + +#[derive(Clone, Debug)] +pub struct ImuMsg { + pub header: Header, + pub orientation: Quaternion, + pub angular_velocity: Vector3, + pub linear_acceleration: Vector3, +} + +#[derive(Clone, Debug)] +pub struct ImuCsvRow { + pub timestamp: u64, + pub orientation: Quaternion, + pub angular_velocity: Vector3, + pub linear_acceleration: Vector3, +} + +#[derive(Clone, Debug)] +pub struct Header { + pub frame_id: &'static str, + pub timestamp: u64, +} + +#[derive(Debug, Clone)] +pub struct Quaternion { + pub x: f64, + pub y: f64, + pub z: f64, + pub w: f64, +} + +#[derive(Clone, Debug)] +pub struct Vector3 { + pub x: f64, + pub y: f64, + pub z: f64, +} + +impl Vector3 { + pub fn new(x: f64, y: f64, z: f64) -> Self { + Self { x, y, z } + } +} + +impl Default for ImuMsg { + fn default() -> Self { + Self { + header: Header { + frame_id: "imu", + timestamp: 0, + }, + orientation: Quaternion { + x: 0.0, + y: 0.0, + z: 0.0, + w: 1.0, + }, + angular_velocity: Vector3::new(0.0, 0.0, 0.0), + linear_acceleration: Vector3::new(0.0, 0.0, 0.0), + } + } +} + +pub fn build_imu_msg_from_csv_row( + row: &ImuCsvRow, + frame_id: &'static str, + timestamp: u64, +) -> ImuMsg { + ImuMsg { + header: Header { + frame_id, + timestamp, + }, + orientation: row.orientation.clone(), + angular_velocity: row.angular_velocity.clone(), + linear_acceleration: row.linear_acceleration.clone(), + } +} + +pub struct TamagawaImuParser { + frame_id: &'static str, + dummy_counter: u16, +} + +impl TamagawaImuParser { + pub fn new(frame_id: &'static str) -> Self { + Self { + frame_id, + dummy_counter: 0, + } + } + + pub fn parse_binary_data(&self, data: &[u8], timestamp: u64) -> Option { + if data.len() != 58 || data[0..9] != *b"$TSC,BIN," { + return None; + } + + let mut imu_msg = ImuMsg::default(); + imu_msg.header.frame_id = self.frame_id; + imu_msg.header.timestamp = timestamp; + + let _counter = ((data[11] as u16) << 8) | (data[12] as u16); + let raw_data = self.parse_signed_16bit(&data[15..17]); + imu_msg.angular_velocity.x = self.convert_angular_velocity(raw_data); + let raw_data = self.parse_signed_16bit(&data[17..19]); + imu_msg.angular_velocity.y = self.convert_angular_velocity(raw_data); + let raw_data = self.parse_signed_16bit(&data[19..21]); + imu_msg.angular_velocity.z = self.convert_angular_velocity(raw_data); + let raw_data = self.parse_signed_16bit(&data[21..23]); + imu_msg.linear_acceleration.x = self.convert_acceleration(raw_data); + let raw_data = self.parse_signed_16bit(&data[23..25]); + imu_msg.linear_acceleration.y = self.convert_acceleration(raw_data); + let raw_data = self.parse_signed_16bit(&data[25..27]); + imu_msg.linear_acceleration.z = self.convert_acceleration(raw_data); + + Some(imu_msg) + } + + pub fn generate_dummy_binary_data( + &mut self, + _timestamp: u64, + angular_velocity: Vector3, + linear_acceleration: Vector3, + ) -> Vec { + let mut data = vec![0u8; 58]; + + data[0..5].copy_from_slice(b"$TSC,"); + data[5] = b'B'; + data[6] = b'I'; + data[7] = b'N'; + data[8] = b','; + data[11] = (self.dummy_counter >> 8) as u8; + data[12] = (self.dummy_counter & 0xFF) as u8; + self.dummy_counter = self.dummy_counter.wrapping_add(1); + + let angular_vel_x_lsb = self.convert_angular_velocity_to_lsb(angular_velocity.x); + let angular_vel_y_lsb = self.convert_angular_velocity_to_lsb(angular_velocity.y); + let angular_vel_z_lsb = self.convert_angular_velocity_to_lsb(angular_velocity.z); + let accel_x_lsb = self.convert_acceleration_to_lsb(linear_acceleration.x); + let accel_y_lsb = self.convert_acceleration_to_lsb(linear_acceleration.y); + let accel_z_lsb = self.convert_acceleration_to_lsb(linear_acceleration.z); + + data[15] = (angular_vel_x_lsb >> 8) as u8; + data[16] = (angular_vel_x_lsb & 0xFF) as u8; + data[17] = (angular_vel_y_lsb >> 8) as u8; + data[18] = (angular_vel_y_lsb & 0xFF) as u8; + data[19] = (angular_vel_z_lsb >> 8) as u8; + data[20] = (angular_vel_z_lsb & 0xFF) as u8; + data[21] = (accel_x_lsb >> 8) as u8; + data[22] = (accel_x_lsb & 0xFF) as u8; + data[23] = (accel_y_lsb >> 8) as u8; + data[24] = (accel_y_lsb & 0xFF) as u8; + data[25] = (accel_z_lsb >> 8) as u8; + data[26] = (accel_z_lsb & 0xFF) as u8; + + data + } + + pub fn generate_static_dummy_data(&mut self, timestamp: u64) -> Vec { + let angular_velocity = Vector3::new(0.1, 0.2, 0.01); + let linear_acceleration = Vector3::new(0.0, 0.0, 9.80665); + + self.generate_dummy_binary_data(timestamp, angular_velocity, linear_acceleration) + } + + fn convert_angular_velocity_to_lsb(&self, rad_per_sec: f64) -> i16 { + let deg_per_sec = rad_per_sec * 180.0 / PI; + let lsb = deg_per_sec / (200.0 / (1 << 15) as f64); + lsb as i16 + } + + fn convert_acceleration_to_lsb(&self, m_per_sec_squared: f64) -> i16 { + let lsb = m_per_sec_squared / (100.0 / (1 << 15) as f64); + lsb as i16 + } + + fn parse_signed_16bit(&self, data: &[u8]) -> i16 { + if data.len() != 2 { + return 0; + } + let high_byte = (data[0] as i32) << 8; + let low_byte = data[1] as i32; + let result = (high_byte & 0xFFFFFF00u32 as i32) | (low_byte & 0x000000FF); + result as i16 + } + + fn convert_angular_velocity(&self, raw_data: i16) -> f64 { + let lsb_to_deg_per_sec = 200.0 / (1 << 15) as f64; + let deg_to_rad = PI / 180.0; + (raw_data as f64) * lsb_to_deg_per_sec * deg_to_rad + } + + fn convert_acceleration(&self, raw_data: i16) -> f64 { + let lsb_to_m_per_sec_squared = 100.0 / (1 << 15) as f64; + (raw_data as f64) * lsb_to_m_per_sec_squared + } + + pub fn generate_version_request() -> &'static [u8] { + b"$TSC,VER*29\r\n" + } + + pub fn generate_offset_cancel_request(offset_value: i32) -> String { + format!("$TSC,OFC,{}\r\n", offset_value) + } + + pub fn generate_heading_reset_request() -> &'static [u8] { + b"$TSC,HRST*29\r\n" + } + + pub fn generate_binary_request(rate_hz: u32) -> String { + format!("$TSC,BIN,{}\r\n", rate_hz) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use core::f64::consts::PI; + + fn assert_approx_eq(actual: f64, expected: f64, eps: f64) { + assert!( + (actual - expected).abs() <= eps, + "left: {actual}, right: {expected}" + ); + } + + fn expected_ang_vel(raw: i16) -> f64 { + let lsb_to_deg_per_sec = 200.0 / (1 << 15) as f64; + let deg_to_rad = PI / 180.0; + (raw as f64) * lsb_to_deg_per_sec * deg_to_rad + } + + fn expected_acc(raw: i16) -> f64 { + let lsb_to_m_per_sec_squared = 100.0 / (1 << 15) as f64; + (raw as f64) * lsb_to_m_per_sec_squared + } + + fn put_i16_be(buf: &mut [u8], value: i16) { + let raw = value as u16; + buf[0] = (raw >> 8) as u8; + buf[1] = (raw & 0xFF) as u8; + } + + #[test] + fn parse_rejects_invalid_length() { + let parser = TamagawaImuParser::new("imu_link"); + let data = [0u8; 57]; + + assert!(parser.parse_binary_data(&data, 1).is_none()); + } + + #[test] + fn parse_rejects_non_bin_header() { + let parser = TamagawaImuParser::new("imu_link"); + let mut data = [0u8; 58]; + data[0..5].copy_from_slice(b"$TSC,"); + data[5..9].copy_from_slice(b"XIN,"); + + assert!(parser.parse_binary_data(&data, 1).is_none()); + } + + #[test] + fn parse_rejects_invalid_tsc_prefix() { + let parser = TamagawaImuParser::new("imu_link"); + let mut data = [0u8; 58]; + data[0..9].copy_from_slice(b"@TSC,BIN,"); + + assert!(parser.parse_binary_data(&data, 1).is_none()); + } + + #[test] + fn parse_extracts_fields_and_converts_units() { + let parser = TamagawaImuParser::new("imu_link"); + let timestamp = 123456789u64; + + let mut data = [0u8; 58]; + data[0..5].copy_from_slice(b"$TSC,"); + data[5..9].copy_from_slice(b"BIN,"); + + let gx: i16 = 1; + let gy: i16 = -2; + let gz: i16 = 1234; + let ax: i16 = -300; + let ay: i16 = 0; + let az: i16 = 32767; + + put_i16_be(&mut data[15..17], gx); + put_i16_be(&mut data[17..19], gy); + put_i16_be(&mut data[19..21], gz); + put_i16_be(&mut data[21..23], ax); + put_i16_be(&mut data[23..25], ay); + put_i16_be(&mut data[25..27], az); + + let imu = parser + .parse_binary_data(&data, timestamp) + .expect("should parse"); + + assert_eq!(imu.header.frame_id, "imu_link"); + assert_eq!(imu.header.timestamp, timestamp); + + let eps = 1e-12; + assert_approx_eq(imu.angular_velocity.x, expected_ang_vel(gx), eps); + assert_approx_eq(imu.angular_velocity.y, expected_ang_vel(gy), eps); + assert_approx_eq(imu.angular_velocity.z, expected_ang_vel(gz), eps); + + assert_approx_eq(imu.linear_acceleration.x, expected_acc(ax), eps); + assert_approx_eq(imu.linear_acceleration.y, expected_acc(ay), eps); + assert_approx_eq(imu.linear_acceleration.z, expected_acc(az), eps); + } + + #[test] + fn generate_dummy_data_roundtrip_is_close() { + let mut parser = TamagawaImuParser::new("imu_link"); + let timestamp = 42u64; + + let input_angular_velocity = Vector3::new(0.5, -0.3, 1.2); + let input_linear_acceleration = Vector3::new(8.5, 2.1, 10.2); + + let data = parser.generate_dummy_binary_data( + timestamp, + input_angular_velocity.clone(), + input_linear_acceleration.clone(), + ); + let imu = parser + .parse_binary_data(&data, timestamp) + .expect("should parse"); + + let ang_eps = (200.0 / (1 << 15) as f64) * (PI / 180.0); + let acc_eps = 100.0 / (1 << 15) as f64; + + assert_approx_eq(imu.angular_velocity.x, input_angular_velocity.x, ang_eps); + assert_approx_eq(imu.angular_velocity.y, input_angular_velocity.y, ang_eps); + assert_approx_eq(imu.angular_velocity.z, input_angular_velocity.z, ang_eps); + + assert_approx_eq( + imu.linear_acceleration.x, + input_linear_acceleration.x, + acc_eps, + ); + assert_approx_eq( + imu.linear_acceleration.y, + input_linear_acceleration.y, + acc_eps, + ); + assert_approx_eq( + imu.linear_acceleration.z, + input_linear_acceleration.z, + acc_eps, + ); + } +} diff --git a/applications/autoware/src/lib.rs b/applications/autoware/src/lib.rs new file mode 100644 index 000000000..438b2d083 --- /dev/null +++ b/applications/autoware/src/lib.rs @@ -0,0 +1,4 @@ +#![no_std] +extern crate alloc; + +pub async fn run() {} diff --git a/userland/Cargo.toml b/userland/Cargo.toml index 664ec35f3..6b0d5ed84 100644 --- a/userland/Cargo.toml +++ b/userland/Cargo.toml @@ -14,6 +14,10 @@ path = "../awkernel_async_lib" [dependencies.awkernel_services] path = "../applications/awkernel_services" +[dependencies.autoware] +path = "../applications/autoware" +optional = true + [dependencies.rd_gen_to_dags] path = "../applications/rd_gen_to_dags" optional = true @@ -80,6 +84,7 @@ perf = ["awkernel_services/perf"] # Evaluation applications rd_gen_to_dags = ["dep:rd_gen_to_dags"] +autoware = ["dep:autoware"] # Test applications test_network = ["dep:test_network"] diff --git a/userland/src/lib.rs b/userland/src/lib.rs index 01a66fe18..bdad77b5b 100644 --- a/userland/src/lib.rs +++ b/userland/src/lib.rs @@ -7,6 +7,9 @@ use alloc::borrow::Cow; pub async fn main() -> Result<(), Cow<'static, str>> { awkernel_services::run().await; + #[cfg(feature = "autoware")] + autoware::run().await; // run the autoware application + #[cfg(feature = "rd_gen_to_dags")] rd_gen_to_dags::run().await; // run the rd_gen_to_dags application