feat(app): add autoware module (lib.rs)

Makefile

@@ -6,6 +6,15 @@ else
 	OPT = --features debug
 endif
 
+# KVM=1 to accelerate x86_64 QEMU with KVM (requires /dev/kvm access on the host).
+# Off by default since not every environment (CI, nested virtualization without KVM
+# passthrough, non-Linux hosts) has it available.
+ifeq ($(KVM), 1)
+	QEMU_KVM_ARGS = -enable-kvm -cpu host
+else
+	QEMU_KVM_ARGS =
+endif
+
 # 2MiB Stack
 STACKSIZE = 1024 * 1024 * 2
 
@@ -143,7 +152,7 @@ check_x86_64: $(X86ASM)
 	cargo +$(RUSTV) check_x86
 
 kernel-x86_64.elf: $(X86ASM) FORCE
-	RUSTFLAGS="$(RUSTC_MISC_ARGS)" cargo +$(RUSTV) x86 $(OPT)
+	RUSTFLAGS="$(RUSTC_MISC_ARGS)" IMU_CSV_PATH=$(IMU_CSV_PATH) VELOCITY_CSV_PATH=$(VELOCITY_CSV_PATH) cargo +$(RUSTV) x86 $(OPT)
 	python3 scripts/embed_debug_info.py $@
 
 x86_64_boot.img: kernel-x86_64.elf
@@ -157,10 +166,14 @@ $(X86ASM): FORCE
 
 OVMF_PATH := $(shell cat ${HOME}/.ovmfpath)
 
+IMU_CSV_PATH ?= $(CURDIR)/../awkernel_script/sensor_inputs/imu_raw.csv
+VELOCITY_CSV_PATH ?= $(CURDIR)/../awkernel_script/sensor_inputs/velocity_status.csv
+
 QEMU_X86_ARGS= -drive if=pflash,format=raw,readonly=on,file=${OVMF_PATH}/code.fd
 QEMU_X86_ARGS+= -drive if=pflash,format=raw,file=${OVMF_PATH}/vars_qemu.fd
 QEMU_X86_ARGS+= -drive format=raw,file=x86_64_uefi.img
 QEMU_X86_ARGS+= -machine q35
+QEMU_X86_ARGS+= $(QEMU_KVM_ARGS)
 QEMU_X86_ARGS+= -serial stdio -smp 4 -monitor telnet::5556,server,nowait
 QEMU_X86_ARGS+= -m 4G -smp cpus=16
 QEMU_X86_ARGS+= -object memory-backend-ram,size=1G,id=m0

applications/autoware/Cargo.toml

@@ -13,9 +13,9 @@ 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 }
+common_types = { path = "./common_types", default-features = false }
 imu_driver = { path = "./imu_driver", default-features = false }
 imu_corrector = { path = "./imu_corrector", default-features = false }
 vehicle_velocity_converter = { path = "./vehicle_velocity_converter", default-features = false }
 gyro_odometer = { path = "./gyro_odometer", default-features = false}
 ekf_localizer = { path = "./ekf_localizer", default-features = false }
-

applications/autoware/build.rs

@@ -0,0 +1,31 @@
+use std::{env, fs, path::PathBuf};
+
+fn main() {
+    let out_dir = PathBuf::from(env::var("OUT_DIR").expect("OUT_DIR is not set"));
+    let csv_data_path = out_dir.join("csv_data.rs");
+
+    let imu_csv = read_csv_from_env("IMU_CSV_PATH");
+    let velocity_csv = read_csv_from_env("VELOCITY_CSV_PATH");
+
+    let generated = format!(
+        "pub const IMU_CSV_DATA_STR: &str = {imu:?};\npub const VELOCITY_CSV_DATA_STR: &str = {velocity:?};\n",
+        imu = imu_csv,
+        velocity = velocity_csv,
+    );
+
+    fs::write(&csv_data_path, generated).expect("failed to write generated csv constants");
+}
+
+fn read_csv_from_env(var_name: &str) -> String {
+    let Ok(path) = env::var(var_name) else {
+        return String::new();
+    };
+
+    match fs::read_to_string(&path) {
+        Ok(contents) => contents,
+        Err(err) => {
+            println!("cargo:warning=failed to read {var_name} from {path}: {err}");
+            String::new()
+        }
+    }
+}

applications/autoware/ekf_localizer/src/lib.rs

@@ -246,7 +246,7 @@ impl Simple1DFilter {
 
         let kalman_gain = self.var / (self.var + obs_var);
         self.x += kalman_gain * (obs - self.x);
-        self.var = (1.0 - kalman_gain) * self.var;
+        self.var *= 1.0 - kalman_gain;
     }
 
     pub fn set_proc_var(&mut self, proc_var: f64) {

applications/autoware/gyro_odometer/src/lib.rs

@@ -45,7 +45,7 @@ const COV_IDX_XYZRPY_PITCH_PITCH: usize = 28;
 const COV_IDX_XYZRPY_YAW_YAW: usize = 35;
 
 pub struct GyroOdometerCore {
-    pub output_frame: String,
+    pub output_frame: &'static str,
     pub message_timeout_sec: f64,
     pub vehicle_twist_arrived: bool,
     pub imu_arrived: bool,
@@ -59,7 +59,7 @@ pub struct GyroOdometerCore {
 impl GyroOdometerCore {
     pub fn new(config: GyroOdometerConfig) -> Result<Self> {
         let queue_size = config.queue_size;
-        let output_frame = config.output_frame.clone();
+        let output_frame = config.output_frame;
         let message_timeout_sec = config.message_timeout_sec;
 
         Ok(Self {
@@ -118,8 +118,8 @@ impl GyroOdometerCore {
         }
 
         let tf = self.get_transform(
-            &self.gyro_queue.front().unwrap().header.frame_id,
-            &self.output_frame,
+            self.gyro_queue.front().unwrap().header.frame_id,
+            self.output_frame,
         )?;
 
         // In the original C++ implementation, angular_velocity_covariance is also transformed
@@ -137,7 +137,7 @@ impl GyroOdometerCore {
 
         for vehicle_twist in &self.vehicle_twist_queue {
             vx_mean += vehicle_twist.twist.twist.linear.x;
-            vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0];
+            vx_covariance_original += vehicle_twist.twist.covariance[0];
         }
         vx_mean /= self.vehicle_twist_queue.len() as f64;
         vx_covariance_original /= self.vehicle_twist_queue.len() as f64;
@@ -168,7 +168,7 @@ impl GyroOdometerCore {
 
         let mut result = TwistWithCovarianceStamped {
             header: Header {
-                frame_id: self.gyro_queue.front().unwrap().header.frame_id,
+                frame_id: self.output_frame,
                 timestamp: result_timestamp,
             },
             twist: TwistWithCovariance {
@@ -201,15 +201,22 @@ impl GyroOdometerCore {
         let dt = (current_timestamp as f64 - last_timestamp as f64) / 1_000_000_000.0;
         dt.abs() > timeout_sec
     }
-    pub fn get_transform(&self, from_frame: &str, to_frame: &str) -> Result<Transform> {
+    pub fn get_transform(&self, _from_frame: &str, _to_frame: &str) -> Result<Transform> {
+        // This exists to rotate the raw gyro's angular velocity out of the IMU's own mounting
+        // orientation (`from_frame`, e.g. `imu_link`) and into the vehicle body frame
+        // (`to_frame`, `output_frame`/"base_link"). If the IMU isn't mounted perfectly aligned
+        // with the vehicle body, its raw yaw-axis reading is not exactly the vehicle's own yaw
+        // rate -- some of it leaks in from roll/pitch, and vice versa. `measurement_update_twist`
+        // downstream assumes `wz` already IS the vehicle's base_link yaw rate, so any
+        // uncorrected mounting misalignment biases the fused estimate.
+        //
         // In the original implementation, a TF lookup failure should clear the queues and
-        // terminate processing early. This port currently returns identity because the
-        // evaluation setup uses a fixed identity transform.
-        if from_frame == to_frame || from_frame == "" || to_frame == "" {
-            Ok(Transform::identity())
-        } else {
-            Ok(Transform::identity())
-        }
+        // terminate processing early. This port currently always returns identity because the
+        // evaluation setup uses a fixed identity transform (IMU assumed perfectly aligned with
+        // base_link); there is no real TF lookup yet -- a real one would query calibrated sensor
+        // extrinsics (typically a fixed static offset for an IMU-to-base_link mount) instead of
+        // hardcoding identity here.
+        Ok(Transform::identity())
     }
 
     // The original C++ node publishes four topics: raw TwistStamped, raw TwistWithCovarianceStamped,
@@ -255,10 +262,8 @@ impl GyroOdometerCore {
         &mut self,
         current_time: u64,
     ) -> Option<TwistWithCovarianceStamped> {
-        match self.concat_gyro_and_odometer(current_time) {
-            Ok(result) => result,
-            Err(_) => None,
-        }
+        self.concat_gyro_and_odometer(current_time)
+            .unwrap_or_default()
     }
 
     pub fn get_queue_sizes(&self) -> (usize, usize) {
@@ -303,7 +308,7 @@ type Result<T> = core::result::Result<T, GyroOdometerError>;
 
 #[derive(Debug, Clone)]
 pub struct GyroOdometerConfig {
-    pub output_frame: String,
+    pub output_frame: &'static str,
     pub message_timeout_sec: f64,
     pub queue_size: usize,
     pub transform_timeout: Duration,
@@ -314,7 +319,7 @@ pub struct GyroOdometerConfig {
 impl Default for GyroOdometerConfig {
     fn default() -> Self {
         Self {
-            output_frame: String::from("base_link"),
+            output_frame: "base_link",
             message_timeout_sec: 1.0,
             queue_size: 100,
             transform_timeout: Duration::from_secs(1),
@@ -358,7 +363,7 @@ mod tests {
 
     fn get_config_with_default_params() -> GyroOdometerConfig {
         GyroOdometerConfig {
-            output_frame: String::from("base_link"),
+            output_frame: "base_link",
             message_timeout_sec: 1e12,
             ..GyroOdometerConfig::default()
         }

applications/autoware/imu_corrector/src/lib.rs

@@ -274,7 +274,7 @@ impl<T: TransformListener> ImuCorrector<T> {
     pub fn correct_imu_with_dynamic_tf(&self, imu_msg: &ImuMsg) -> Option<ImuMsg> {
         let transform = self
             .transform_listener
-            .get_latest_transform(&imu_msg.header.frame_id, self.config.output_frame)?;
+            .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())
@@ -286,7 +286,7 @@ impl<T: TransformListener> ImuCorrector<T> {
     ) -> Option<ImuWithCovariance> {
         let transform = self
             .transform_listener
-            .get_latest_transform(&imu_msg.header.frame_id, self.config.output_frame)?;
+            .get_latest_transform(imu_msg.header.frame_id, self.config.output_frame)?;
 
         Some(self.correct_imu_with_covariance(imu_msg, Some(&transform)))
     }