Sans-IO firmware protocol

firmware/src/main.rs

@@ -60,7 +60,7 @@ fn main() -> ! {
 
 	static EXECUTOR: StaticCell<Executor> = StaticCell::new();
 	EXECUTOR.init(Executor::new()).run(move |s| {
-		s.spawn(crate::networking::protocol::control_task(packets, quat))
+		s.spawn(crate::networking::protocol::protocol_task(packets, quat))
 			.unwrap();
 		s.spawn(crate::networking::network_task(packets)).unwrap();
 		s.spawn(crate::imu::imu_task(quat, p.i2c, p.delay)).unwrap();

firmware/src/networking/protocol/mod.rs

@@ -5,82 +5,61 @@ pub use self::packets::Packets;
 
 use defmt::debug;
 use embassy_executor::task;
+use embassy_futures::select::select;
 use firmware_protocol::{
-	BoardType, CbPacket, ImuType, McuType, SbPacket, SensorDataType, SensorStatus,
+	sansio::{self, SbBuf},
+	CbPacket, SlimeQuaternion,
 };
 
 use crate::imu::Quat;
-use crate::utils::Unreliable;
-
-#[allow(dead_code)]
-mod v2;
+use crate::utils::{Reliable, Unreliable};
 
 #[task]
-pub async fn control_task(
+pub async fn protocol_task(
 	packets: &'static Packets,
 	quat: &'static Unreliable<Quat>,
 ) -> ! {
 	debug!("Control task!");
 	async {
+		let mut proto_state = sansio::State::new();
+		let mut sb_buf = SbBuf::new();
 		loop {
-			do_work(packets, quat).await;
+			proto_state = match proto_state {
+				sansio::State::Disconnected(s) => {
+					while_disconnected(s, &mut sb_buf, &packets.clientbound).await
+				}
+				sansio::State::Connected(s) => {
+					while_connected(s, &mut sb_buf, &packets.clientbound, quat).await
+				}
+			};
+			while !sb_buf.is_empty() {
+				let sb = sb_buf.pop().unwrap();
+				packets.serverbound.send(sb).await;
+			}
 		}
 	}
 	.await
 }
 
-async fn do_work(packets: &Packets, quat: &Unreliable<Quat>) {
-	match packets.clientbound.recv().await {
-		// Identify ourself when discovery packet is received
-		CbPacket::Discovery => {
-			packets
-				.serverbound
-				.send(SbPacket::Handshake {
-					// TODO: Compile time constants for board and MCU
-					board: BoardType::Custom,
-					// Should this IMU type be whatever the first IMU of the system is?
-					imu: ImuType::Unknown(0xFF),
-					mcu: McuType::Esp32,
-					imu_info: (0, 0, 0), // These appear to be inert
-					// Needs to be >=9 to use newer protocol, this is hard-coded in
-					// the java server :(
-					build: 10,
-					firmware: "SlimeVR-Rust".into(),
-					mac_address: [0; 6],
-				})
-				.await;
-			debug!("Handshake");
+async fn while_disconnected(
+	state: sansio::Disconnected,
+	sb_buf: &mut SbBuf,
+	cb: &Reliable<CbPacket>,
+) -> sansio::State {
+	let cb_msg = cb.recv().await;
+	state.received_msg(cb_msg, sb_buf)
+}
 
-			// After handshake, we are supposed to send `SensorInfo` only once.
-			packets
-				.serverbound
-				.send(SbPacket::SensorInfo {
-					sensor_id: 0, // First sensor (of two)
-					sensor_status: SensorStatus::Ok,
-					sensor_type: ImuType::Unknown(0xFF),
-				})
-				.await;
-			debug!("SensorInfo");
-		}
-		// When heartbeat is received, we should reply with heartbeat 0 aka Discovery
-		// The protocol is asymmetric so its a bit unintuitive.
-		CbPacket::Heartbeat => {
-			packets.serverbound.send(SbPacket::Heartbeat).await;
-		}
-		// Pings are basically like heartbeats, just echo data back
-		CbPacket::Ping { challenge } => {
-			packets.serverbound.send(SbPacket::Ping { challenge }).await;
-		}
-		_ => (),
+async fn while_connected(
+	state: sansio::Connected,
+	sb_buf: &mut SbBuf,
+	cb: &Reliable<CbPacket>,
+	quat: &Unreliable<Quat>,
+) -> sansio::State {
+	let event = select(cb.recv(), quat.wait()).await;
+	use embassy_futures::select::Either;
+	match event {
+		Either::First(cb_msg) => state.received_msg(cb_msg, sb_buf),
+		Either::Second(quat) => state.send_imu(0, SlimeQuaternion::from(quat), sb_buf),
 	}
-
-	packets
-		.serverbound
-		.send(SbPacket::RotationData {
-			sensor_id: 0,                      // First sensor
-			data_type: SensorDataType::Normal, // Rotation data without magnetometer correction.
-			quat: quat.wait().await.into_inner().into(),
-			calibration_info: 0,
-		})
-		.await;
 }

networking/firmware_protocol/Cargo.toml

@@ -6,11 +6,16 @@ edition = "2021"
 
 [dependencies]
 deku = { version = "0.15", default-features = false, features = ["alloc"] }
+heapless = { version = "0.7", default-features = false }
+replace_with = { version = "0.1", default-features = false }
+derive_more = "0.99"
+
 # We support multiple versions of nalgebra since it changes so much.
 nalgebra032 = { package = "nalgebra", version = "0.32", default-features = false, optional = true }
 nalgebra031 = { package = "nalgebra", version = "0.31", default-features = false, optional = true }
 nalgebra030 = { package = "nalgebra", version = "0.30", default-features = false, optional = true }
 
+
 [dev-dependencies]
 nalgebra032 = { package = "nalgebra", version = "0.32" }
 nalgebra031 = { package = "nalgebra", version = "0.31" }

networking/firmware_protocol/src/lib.rs

@@ -3,6 +3,8 @@
 extern crate alloc;
 
 mod clientbound;
+pub mod sansio;
+mod serialization;
 mod serverbound;
 
 pub use clientbound::*;
@@ -20,9 +22,9 @@ use deku::prelude::*;
 #[derive(Debug, PartialEq, DekuRead, DekuWrite)]
 #[deku(endian = "e", ctx = "e: deku::ctx::Endian")]
 pub struct SlimeQuaternion {
-	pub i: f32,
-	pub j: f32,
-	pub k: f32,
+	pub x: f32,
+	pub y: f32,
+	pub z: f32,
 	pub w: f32,
 }
 
@@ -33,34 +35,45 @@ macro_rules! impl_Nalgebra {
 		impl From<Quaternion<f32>> for SlimeQuaternion {
 			fn from(q: Quaternion<f32>) -> Self {
 				Self {
-					i: q.i,
-					j: q.j,
-					k: q.k,
+					x: q.i,
+					y: q.j,
+					z: q.k,
 					w: q.w,
 				}
 			}
 		}
 		impl From<SlimeQuaternion> for Quaternion<f32> {
 			fn from(q: SlimeQuaternion) -> Self {
-				Self::new(q.w, q.i, q.j, q.k)
+				Self::new(q.w, q.x, q.y, q.z)
+			}
+		}
+
+		impl From<UnitQuaternion<f32>> for SlimeQuaternion {
+			fn from(q: UnitQuaternion<f32>) -> Self {
+				Self {
+					x: q.i,
+					y: q.j,
+					z: q.k,
+					w: q.w,
+				}
 			}
 		}
 	};
 }
 
 #[cfg(any(test, feature = "nalgebra032"))]
 mod nalgebra032_impls {
-	use nalgebra032::Quaternion;
+	use nalgebra032::{Quaternion, UnitQuaternion};
 	impl_Nalgebra!();
 }
 #[cfg(any(test, feature = "nalgebra031"))]
 mod nalgebra031_impls {
-	use nalgebra031::Quaternion;
+	use nalgebra031::{Quaternion, UnitQuaternion};
 	impl_Nalgebra!();
 }
 #[cfg(any(test, feature = "nalgebra030"))]
 mod nalgebra030_impls {
-	use nalgebra030::Quaternion;
+	use nalgebra030::{Quaternion, UnitQuaternion};
 	impl_Nalgebra!();
 }