Merge branch 'main' of https://github.com/utahrobotics/lunadev-2025

Author: manglemix

.gitignore

@@ -38,5 +38,6 @@ perf.data**
 perf.data
 
 .allow-ws
+.idea
 
 embedded/target/

Cargo.lock

@@ -38,6 +38,11 @@ workspace = false
 command = "cargo"
 args = ["run", "--features=production", "-p", "lunabot", "--profile", "optdebug", "--", "main"]
 
+[tasks.checkmain]
+workspace=false
+command = "cargo"
+args = ["check", "--features=production", "-p", "lunabot", "--profile", "optdebug"]
+
 [tasks.dataviz]
 workspace = false
 command = "cargo"
@@ -111,4 +116,4 @@ args = ["run", "-p", "lunaserver-web-client", "--", "10601"]
 [tasks.lunaserver-web-client-audio]
 workspace = false
 command = "cargo"
-args = ["run", "-p", "lunaserver-web-client", "--", "10602"]
+args = ["run", "-p", "lunaserver-web-client", "--", "10602"]

Makefile.toml

@@ -8,12 +8,20 @@
 //! updating `memory.x` ensures a rebuild of the application with the
 //! new memory settings.
 
-use std::env;
+use std::env::{self, VarError};
 use std::fs::File;
 use std::io::Write;
 use std::path::PathBuf;
 
 fn main() {
+    if let Err(e) = std::env::var("IMU_SERIAL") {
+        if e == VarError::NotPresent {
+            println!("cargo:warning=IMU_SERIAL environment variable not set");
+        } else {
+            println!("cargo:warning=IMU_SERIAL environment variable not set to a valid value");
+        }
+    }
+
     // Put `memory.x` in our output directory and ensure it's
     // on the linker search path.
     let out = &PathBuf::from(env::var_os("OUT_DIR").unwrap());

embedded/imu_chip/build.rs

@@ -39,6 +39,7 @@ rodio = { version = "0.20.1", optional = true }
 tokio-serial = { version = "5.4.5", optional = true }
 mio = { version = "1", optional = true }
 file-lock = { version = "2.1.11", optional = true }
+imu-fusion = { version = "0.2.5", optional = true }
 
 [features]
 production = [
@@ -55,5 +56,6 @@ production = [
     "mio",
     "embedded_common",
     "file-lock",
+    "imu-fusion",
 ]
 experimental = ["opus", "production", "rodio"]

lunabotics/.idea/.gitignore

@@ -1,19 +1,34 @@
-
 use std::sync::mpsc::{Receiver, SyncSender};
 
 use embedded_common::*;
 use fxhash::FxHashMap;
 use nalgebra::Vector3;
 use simple_motion::StaticImmutableNode;
-use tasker::{get_tokio_handle, tokio::{self, io::{AsyncReadExt, AsyncWriteExt, BufStream}, net::TcpListener}, BlockOn};
+use tasker::{
+    get_tokio_handle,
+    tokio::{
+        self,
+        io::{AsyncReadExt, AsyncWriteExt, BufStream},
+        net::TcpListener,
+    },
+    BlockOn,
+};
 use tokio_serial::SerialPortBuilderExt;
 use tracing::{debug, error, info, warn};
 use udev::{EventType, MonitorBuilder, Udev};
-
+use imu_fusion::{FusionAhrsSettings, Fusion};
+use crossbeam::queue::ArrayQueue;
+use std::time::Instant;
 use crate::localization::LocalizerRef;
 
 use super::udev_poll;
 
+type TIMESTAMP = f32;
+pub enum IMUReading {
+    Acceleration((Vector3<f32>, TIMESTAMP)),
+    AngularRate((Vector3<f32>, TIMESTAMP))
+}
+
 pub struct IMUInfo {
     pub node: StaticImmutableNode,
 }
@@ -23,31 +38,63 @@ pub fn enumerate_imus(
     serial_to_chain: impl IntoIterator<Item = (String, IMUInfo)>,
 ) {
     get_tokio_handle().spawn(imu_wifi_listener());
+    let data_queue: Box<ArrayQueue<IMUReading>> = Box::new(ArrayQueue::new(64));
+    let data_queue_ref:&'static ArrayQueue<IMUReading> = Box::leak(data_queue);
+
+    std::thread::spawn(move || {
+        const SAMPLE_RATE_HZ: u32 = 100;
+        let settings = FusionAhrsSettings::new();
+        let mut fusion = Fusion::new(SAMPLE_RATE_HZ, settings);
+
+        let mut reading: Option<IMUReading> = None;
+        let mut reading_accel: Option<(Vector3<f32>, TIMESTAMP)> = None;
+        let mut reading_gyro: Option<(Vector3<f32>, TIMESTAMP)> = None;
+        loop {
+            reading = data_queue_ref.pop();
+            if let Some(IMUReading::Acceleration(reading)) = reading {
+                reading_accel = Some(reading);
+            }
+            if let Some(IMUReading::AngularRate(reading)) = reading {
+                reading_gyro = Some(reading);
+            }
+
+            // not using take up here because we dont want to consume more accel readings than gyro readings or vice versa
+            if reading_accel.is_some() && reading_gyro.is_some() {
+                let (Some((accel, timestamp_accel)), Some((gyro, timestamp_gyro))) = (reading_accel.take(),reading_gyro.take()) else {
+                    continue;
+                };
+                
+                if (timestamp_accel-timestamp_gyro).abs() > 1. {
+                    tracing::warn!("acceleration and gyro timestamps were more than 1 seconds apart");
+                }
+
+                let avg = (timestamp_accel+timestamp_gyro)/2.0;
+
+                fusion.update_no_mag(gyro.into(), accel.into(), avg);
+                let acc = fusion.ahrs.linear_acc();
+                tracing::info!("x: {}, y: {}, z: {}", acc.x, acc.y, acc.z);
+            }
+        }        
+    });
+
+
     let mut threads: FxHashMap<String, SyncSender<String>> = serial_to_chain
         .into_iter()
-        .filter_map(
-            |(
-                port,
-                IMUInfo {
-                    node
-                },
-            )| {
-                let port2 = port.clone();
-                let localizer_ref = localizer_ref.clone();
-                let (tx, rx) = std::sync::mpsc::sync_channel(1);
-                std::thread::spawn(move || {
-                    let mut imu_task = IMUTask {
-                        path: rx,
-                        localizer: &localizer_ref,
-                        node,
-                    };
-                    loop {
-                        imu_task.imu_task().block_on();
-                    }
-                });
-                Some((port2, tx))
-            },
-        )
+        .filter_map(|(port, IMUInfo { node })| {
+            let port2 = port.clone();
+            let (tx, rx) = std::sync::mpsc::sync_channel(1);
+            std::thread::spawn(move || {
+                let mut imu_task = IMUTask {
+                    path: rx,
+                    node,
+                    queue: data_queue_ref
+                };
+                loop {
+                    imu_task.imu_task().block_on();
+                }
+            });
+            Some((port2, tx))
+        })
         .collect();
 
     std::thread::spawn(move || {
@@ -104,9 +151,7 @@ pub fn enumerate_imus(
             .chain(
                 udev_poll(listener)
                     .filter(|event| event.event_type() == EventType::Add)
-                    .map(|event| {
-                        event.device()
-                    }),
+                    .map(|event| event.device()),
             )
             .for_each(|device| {
                 let Some(path) = device.devnode() else {
@@ -123,10 +168,14 @@ pub fn enumerate_imus(
                     return;
                 };
                 let Some(tmp) = serial.strip_prefix("USR_IMU_") else {
+                    if serial == "USR_IMU" {
+                        warn!("IMU at path {path_str} has no serial number");
+                        return;
+                    }
                     return;
                 };
                 serial = tmp;
-                
+
                 if let Some(path_sender) = threads.get(serial) {
                     if path_sender.send(path_str.into()).is_err() {
                         threads.remove(serial);
@@ -138,13 +187,13 @@ pub fn enumerate_imus(
     });
 }
 
-struct IMUTask<'a> {
+struct IMUTask{
     path: Receiver<String>,
-    localizer: &'a LocalizerRef,
     node: StaticImmutableNode,
+    queue: &'static ArrayQueue<IMUReading>,
 }
 
-impl<'a> IMUTask<'a> {
+impl IMUTask {
     async fn imu_task(&mut self) {
         let path_str = match self.path.recv() {
             Ok(x) => x,
@@ -169,14 +218,16 @@ impl<'a> IMUTask<'a> {
         let mut imu_port = BufStream::new(imu_port);
         let mut data: [u8; 13] = [0; 13];
 
+        let start = Instant::now();
         loop {
             tokio::time::sleep(std::time::Duration::from_millis(10)).await;
             const ACK: [u8; 1] = [1];
             let result = async {
                 imu_port.write(&ACK).await?;
                 imu_port.flush().await?;
                 imu_port.read_exact(&mut data).await
-            }.await;
+            }
+            .await;
             if let Err(e) = result {
                 error!("Failed to read/write to IMU: {e}");
                 break;
@@ -190,18 +241,19 @@ impl<'a> IMUTask<'a> {
             };
             match msg {
                 FromIMU::AngularRateReading(AngularRate { x, y, z }) => {
-                    // euler angle to axis angle
-                    // transform
-                    // axis angle to euler angle
-                    // let accel: Vector3<f64> = Vector3::new(x, -z, -y).cast();
-                    
-                    tracing::info!("{x:1},{y:1},{z:1}");
-
+                    let angular_velocity = Vector3::new(-x, z, y);
+                    let transformed = self.node.get_local_isometry().cast() * angular_velocity;
+                    if let Err(e) = self.queue.push(IMUReading::AngularRate((transformed, start.elapsed().as_secs_f32()))) {
+                        tracing::warn!("couldn't push gyro reading to crossbeam queue");
+                    }
                 }
                 FromIMU::AccelerationNormReading(AccelerationNorm { x, y, z }) => {
-                    let accel: Vector3<f64> = Vector3::new(x, -z, -y).cast();
-                    self.localizer.set_acceleration(self.node.get_local_isometry() * accel);
-                    //tracing::info!("{:?}", self.node.get_local_isometry() * accel);
+                    let accel = Vector3::new(x, -z, -y);
+                    let transformed = self.node.get_local_isometry().cast() * accel;
+
+                    if let Err(e) = self.queue.push(IMUReading::Acceleration((transformed, start.elapsed().as_secs_f32()))) {
+                        tracing::warn!("couldn't push accel reading to crossbeam queue");
+                    }
                 }
                 FromIMU::NoDataReady => {
                     continue;
@@ -242,8 +294,10 @@ async fn imu_wifi_listener() {
                         let Ok(line_str) = std::str::from_utf8(&line) else {
                             continue;
                         };
-                        let Some(i) = line_str.find('\n') else { continue; };
-                        error!(target=addr.to_string(), "{}", line_str.split_at(i).0);
+                        let Some(i) = line_str.find('\n') else {
+                            continue;
+                        };
+                        error!(target = addr.to_string(), "{}", line_str.split_at(i).0);
                         line.drain(0..=i);
                     }
                     Err(e) => {
@@ -254,4 +308,4 @@ async fn imu_wifi_listener() {
             }
         });
     }
-}
+}