From 04a0077a68e7e1c43e4b2b07495ad793f4cb9dc2 Mon Sep 17 00:00:00 2001 From: Ryan Butler Date: Mon, 16 Jan 2023 18:41:57 -0500 Subject: [PATCH] protocol task now uses sansio --- firmware/src/main.rs | 2 +- firmware/src/networking/protocol/mod.rs | 101 +++++++----------- networking/firmware_protocol/src/lib.rs | 69 ++++++------ .../firmware_protocol/src/sansio/mod.rs | 44 ++++---- .../src/{sansio => }/serialization.rs | 0 5 files changed, 96 insertions(+), 120 deletions(-) rename networking/firmware_protocol/src/{sansio => }/serialization.rs (100%) diff --git a/firmware/src/main.rs b/firmware/src/main.rs index f76c8f69..dba6fa96 100644 --- a/firmware/src/main.rs +++ b/firmware/src/main.rs @@ -60,7 +60,7 @@ fn main() -> ! { static EXECUTOR: StaticCell = 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(); diff --git a/firmware/src/networking/protocol/mod.rs b/firmware/src/networking/protocol/mod.rs index 7f7896fa..461d768c 100644 --- a/firmware/src/networking/protocol/mod.rs +++ b/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, ) -> ! { 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) { - let event = select(packets.clientbound.recv(), quat.wait()).await; +async fn while_disconnected( + state: sansio::Disconnected, + sb_buf: &mut SbBuf, + cb: &Reliable, +) -> 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, + quat: &Unreliable, +) -> 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; } diff --git a/networking/firmware_protocol/src/lib.rs b/networking/firmware_protocol/src/lib.rs index d4574fee..03b8cdb4 100644 --- a/networking/firmware_protocol/src/lib.rs +++ b/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> for SlimeQuaternion { - fn from(q: Quaternion) -> Self { - Self { - i: q.i as _, - j: q.j as _, - k: q.k as _, - w: q.w as _, + impl From> for SlimeQuaternion { + fn from(q: Quaternion) -> Self { + Self { + x: q.i, + y: q.j, + z: q.k, + w: q.w, + } } } - } - impl From for Quaternion { - fn from(q: SlimeQuaternion) -> Self { - Self::new(q.w as _, q.i as _, q.j as _, q.k as _) + impl From for Quaternion { + fn from(q: SlimeQuaternion) -> Self { + Self::new(q.w as _, q.x as _, q.y as _, q.z as _) + } } - } + + impl From> for SlimeQuaternion { + fn from(q: UnitQuaternion) -> 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> for SlimeQuaternion { - fn from(q: Quaternion) -> Self { - Self { - i: q.i as _, - j: q.j as _, - k: q.k as _, - w: q.w as _, - } - } - } - impl From for Quaternion { - 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)] diff --git a/networking/firmware_protocol/src/sansio/mod.rs b/networking/firmware_protocol/src/sansio/mod.rs index b8258129..2e496c15 100644 --- a/networking/firmware_protocol/src/sansio/mod.rs +++ b/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,12 +152,17 @@ 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, @@ -171,5 +170,6 @@ impl Connected { quat, calibration_info: 0, }); + self.into() } } diff --git a/networking/firmware_protocol/src/sansio/serialization.rs b/networking/firmware_protocol/src/serialization.rs similarity index 100% rename from networking/firmware_protocol/src/sansio/serialization.rs rename to networking/firmware_protocol/src/serialization.rs