Add DCM fusion (#173)

A DCM Based Attitude Estimation Algorithm for Low-Cost MEMS IMUs
Heikki Hyyti & Arto Visala, 2015

Co-authored-by: Aaro Perämaa <[email protected]>

Author: TheButlah

firmware/Cargo.lock

@@ -194,6 +194,9 @@ defmt-bbq = { version = "0.1", optional = true }
 mpu6050-dmp = "0.3"
 bmi160 = "0.1"
 
+# Sensor fusion
+dcmimu = "0.2"
+
 # Other crates
 static_cell = "1"
 nb = "1"

firmware/Cargo.toml

@@ -0,0 +1,40 @@
+use dcmimu::DCMIMU;
+use embassy_time::Instant;
+use firmware_protocol::ImuType;
+
+use crate::imu::{FusedData, Fuser, Imu, Quat, UnfusedData};
+
+/// Extended Kalman filtering in direction cosine matrix formation
+pub struct Dcm {
+	dcm: DCMIMU,
+	last: Instant,
+}
+
+impl Dcm {
+	pub fn new() -> Self {
+		Self {
+			dcm: DCMIMU::new(),
+			last: Instant::now(),
+		}
+	}
+}
+
+impl Fuser for Dcm {
+	fn process(&mut self, unfused: &UnfusedData) -> FusedData {
+		let last = self.last;
+		self.last = Instant::now();
+		let elapsed = self.last - last;
+
+		let UnfusedData { accel, gyro } = unfused;
+
+		// TODO: Check that these euler angle convention matches
+		let (euler, _) = self.dcm.update(
+			(gyro.x, gyro.y, gyro.z),
+			(accel.x, accel.y, accel.z),
+			elapsed.as_micros() as f32 / 1_000_000.0,
+		);
+
+		let q = Quat::from_euler_angles(euler.roll, euler.pitch, euler.roll);
+		FusedData { q }
+	}
+}

firmware/src/imu/fusion/dcm.rs

@@ -1 +1 @@
-//! TODO: Add fusion
+pub mod dcm;