protocol task now uses sansio

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

@@ -7,88 +7,59 @@ 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>) {
-	let event = select(packets.clientbound.recv(), quat.wait()).await;
+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)
+}
+
+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_packet) => todo!(),
-		Either::Second(quat) => todo!(),
-	}
-
-	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");
-
-			// 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;
-		}
-		_ => (),
+		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/src/lib.rs

@@ -4,6 +4,7 @@ extern crate alloc;
 
 mod clientbound;
 pub mod sansio;
+mod serialization;
 mod serverbound;
 
 pub use clientbound::*;
@@ -27,47 +28,51 @@ pub struct SlimeQuaternion {
 	pub w: f32,
 }
 
-#[cfg(any(test, feature = "nalgebra031"))]
-mod nalgebra031_impls {
-	use super::*;
-	use nalgebra031::Quaternion;
+#[cfg(any(test, feature = "nalgebra031", feature = "nalgebra030"))]
+macro_rules! impl_from {
+	($nalg_mod:path) => {
+		use $nalg_mod::{Quaternion, UnitQuaternion};
 
-	impl From<Quaternion<f32>> for SlimeQuaternion {
-		fn from(q: Quaternion<f32>) -> Self {
-			Self {
-				i: q.i as _,
-				j: q.j as _,
-				k: q.k as _,
-				w: q.w as _,
+		impl From<Quaternion<f32>> for SlimeQuaternion {
+			fn from(q: Quaternion<f32>) -> Self {
+				Self {
+					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 as _, q.i as _, q.j as _, q.k as _)
+		impl From<SlimeQuaternion> for Quaternion<f32> {
+			fn from(q: SlimeQuaternion) -> Self {
+				Self::new(q.w as _, q.x as _, q.y as _, q.z as _)
+			}
 		}
-	}
+
+		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 = "nalgebra031"))]
+mod nalgebra031_impls {
+	use super::*;
+
+	impl_from!(::nalgebra031);
 }
 #[cfg(any(test, feature = "nalgebra030"))]
 mod nalgebra030_impls {
 	use super::*;
-	use nalgebra030::Quaternion;
 
-	impl From<Quaternion<f32>> for SlimeQuaternion {
-		fn from(q: Quaternion<f32>) -> Self {
-			Self {
-				i: q.i as _,
-				j: q.j as _,
-				k: q.k as _,
-				w: q.w as _,
-			}
-		}
-	}
-	impl From<SlimeQuaternion> for Quaternion<f32> {
-		fn from(q: SlimeQuaternion) -> Self {
-			Self::new(q.w as _, q.i as _, q.j as _, q.k as _)
-		}
-	}
+	impl_from!(::nalgebra030);
 }
 
 #[derive(PartialEq, Eq, Debug, DekuRead, DekuWrite)]

networking/firmware_protocol/src/sansio/mod.rs

@@ -11,8 +11,6 @@ use crate::{
 
 use replace_with::replace_with;
 
-mod serialization;
-
 /// A buffer of `SbPacket` that *must all be sent* before other functions on the
 /// protocol are invoked.
 #[derive(Debug)]
@@ -53,17 +51,16 @@ macro_rules! assert_empty {
 }
 
 #[derive(Debug, derive_more::From)]
-pub enum ProtocolState {
+pub enum State {
 	Disconnected(Disconnected),
 	Connected(Connected),
 }
-impl ProtocolState {
+impl State {
 	pub const fn new() -> Self {
 		Self::Disconnected(Disconnected)
 	}
 
-	/// Process a newly received message. This will update (and possibly transition) to
-	/// a new state, while enqueueing any serverbound packets that should be sent into
+	/// Process a newly received message, enqueueing any serverbound messages in
 	/// `sb_buf`.
 	///
 	/// # Panics
@@ -81,8 +78,8 @@ impl ProtocolState {
 			|taken| {
 				// Now we can use the owned value in functions that expect an owned type.
 				match taken {
-					ProtocolState::Disconnected(s) => s.received_msg(cb, sb_buf),
-					ProtocolState::Connected(s) => s.received_msg(cb, sb_buf),
+					State::Disconnected(s) => s.received_msg(cb, sb_buf),
+					State::Connected(s) => s.received_msg(cb, sb_buf),
 				}
 			},
 		);
@@ -92,18 +89,13 @@ impl ProtocolState {
 #[derive(Debug)]
 pub struct Disconnected;
 impl Disconnected {
-	/// Process a newly received message. This will update (and possibly transition) to
-	/// a new state, while pushing any serverbound packets that should be sent to
+	/// Process a newly received message, enqueueing any serverbound messages in
 	/// `sb_buf`.
 	///
 	/// # Panics
 	/// Panics if `!sb_buf.is_empty()`. You are only supposed to call this function when
 	/// all serverbound packets have already been sent.
-	pub fn received_msg<'b>(
-		self,
-		cb: CbPacket,
-		sb_buf: &'b mut SbBuf,
-	) -> ProtocolState {
+	pub fn received_msg<'b>(self, cb: CbPacket, sb_buf: &'b mut SbBuf) -> State {
 		assert_empty!(sb_buf);
 		match cb {
 			CbPacket::Discovery => {
@@ -137,11 +129,13 @@ impl Disconnected {
 #[derive(Debug)]
 pub struct Connected {}
 impl Connected {
-	pub fn received_msg<'b>(
-		self,
-		cb: CbPacket,
-		sb_buf: &'b mut SbBuf,
-	) -> ProtocolState {
+	/// Process a newly received message, enqueueing any serverbound messages in
+	/// `sb_buf`.
+	///
+	/// # Panics
+	/// Panics if `!sb_buf.is_empty()`. You are only supposed to call this function when
+	/// all serverbound packets have already been sent.
+	pub fn received_msg<'b>(self, cb: CbPacket, sb_buf: &'b mut SbBuf) -> State {
 		assert_empty!(sb_buf);
 		match cb {
 			// When heartbeat is received, we should reply with another heartbeat.
@@ -158,18 +152,24 @@ impl Connected {
 		}
 	}
 
+	/// Enqueues an imu update in `SbBuf`.
+	///
+	/// # Panics
+	/// Panics if `!sb_buf.is_empty()`. You are only supposed to call this function when
+	/// all serverbound packets have already been sent.
 	pub fn send_imu(
-		&mut self,
+		self,
 		sensor_id: u8,
 		quat: SlimeQuaternion,
 		sb_buf: &mut SbBuf,
-	) {
+	) -> State {
 		assert_empty!(sb_buf);
 		sb_buf.push(SbPacket::RotationData {
 			sensor_id,
 			data_type: SensorDataType::Normal,
 			quat,
 			calibration_info: 0,
 		});
+		self.into()
 	}
 }