From d854ac9e871bddd35710c77434a636084deae3a0 Mon Sep 17 00:00:00 2001 From: Ryan Butler Date: Thu, 6 Apr 2023 22:29:12 -0400 Subject: [PATCH] tmp --- firmware/Cargo.lock | 1 + firmware/Cargo.toml | 62 +++---------------- firmware/src/networking/ble/esp.rs | 99 ++++++++++++++++++++---------- 3 files changed, 75 insertions(+), 87 deletions(-) diff --git a/firmware/Cargo.lock b/firmware/Cargo.lock index 7cde0829..7e441552 100644 --- a/firmware/Cargo.lock +++ b/firmware/Cargo.lock @@ -1075,6 +1075,7 @@ dependencies = [ "firmware_protocol", "fugit", "futures-util", + "heapless", "load-dotenv", "mpu6050-dmp", "nalgebra", diff --git a/firmware/Cargo.toml b/firmware/Cargo.toml index 83e2d931..9c0d962c 100644 --- a/firmware/Cargo.toml +++ b/firmware/Cargo.toml @@ -14,13 +14,7 @@ rust-version.workspace = true [features] -default = [ - "mcu-esp32c3", - "imu-stubbed", - "log-rtt", - "net-wifi", - "fusion-stubbed", -] +default = ["mcu-esp32c3", "imu-stubbed", "log-rtt", "net-ble", "fusion-stubbed"] # default = [ # "mcu-nrf52840", # "imu-stubbed", @@ -32,35 +26,10 @@ default = [ # default = ["mcu-esp32", "imu-stubbed", "log-uart", "net-stubbed", "fusion-stubbed"] # Supported microcontrollers -mcu-esp32 = [ - "_mcu-f-esp32", - "dep:esp32-hal", - "defmt_esp_println/esp32", - "esp-backtrace/esp32", - "xtensa-lx/esp32", - "esp-wifi/esp32", -] -mcu-esp32c3 = [ - "_mcu-f-esp32", - "dep:esp32c3-hal", - "defmt_esp_println/esp32c3", - "esp-backtrace/esp32c3", - "dep:riscv", - "esp-wifi/esp32c3", -] -mcu-nrf52840 = [ - "_mcu-f-nrf52", - "embassy-nrf/nrf52840", - "dep:embassy-usb", - "nrf-softdevice/nrf52840", - "dep:nrf52840-pac", -] -mcu-nrf52832 = [ - "_mcu-f-nrf52", - "embassy-nrf/nrf52832", - "nrf-softdevice/nrf52832", - "dep:nrf52832-pac", -] +mcu-esp32 = ["_mcu-f-esp32", "dep:esp32-hal", "defmt_esp_println/esp32", "esp-backtrace/esp32", "xtensa-lx/esp32", "esp-wifi/esp32"] +mcu-esp32c3 = ["_mcu-f-esp32", "dep:esp32c3-hal", "defmt_esp_println/esp32c3", "esp-backtrace/esp32c3", "dep:riscv", "esp-wifi/esp32c3"] +mcu-nrf52840 = ["_mcu-f-nrf52", "embassy-nrf/nrf52840", "dep:embassy-usb", "nrf-softdevice/nrf52840", "dep:nrf52840-pac"] +mcu-nrf52832 = ["_mcu-f-nrf52", "embassy-nrf/nrf52832", "nrf-softdevice/nrf52832", "dep:nrf52832-pac"] # Wi-fi dependencies net-wifi = ["esp-wifi?/wifi"] # use wifi @@ -93,23 +62,9 @@ nrf-boot-s140 = ["nrf-softdevice/s140"] # use softdevice 140 # All features with underscores are internal only, should not be used by other crates, # and are not covered under semver guarantees. # nrf52 family -_mcu-f-nrf52 = [ - "dep:cortex-m", - "dep:cortex-m-rt", - "dep:alloc-cortex-m", - "embassy-nrf/time-driver-rtc1", - "embassy-executor/integrated-timers", - "dep:defmt-bbq", -] - -_mcu-f-esp32 = [ - "dep:esp-alloc", - "dep:embedded-svc", - "dep:embassy-net", - "dep:smoltcp", - "dep:bleps", - "dep:bleps-macros", -] +_mcu-f-nrf52 = ["dep:cortex-m", "dep:cortex-m-rt", "dep:alloc-cortex-m", "embassy-nrf/time-driver-rtc1", "embassy-executor/integrated-timers", "dep:defmt-bbq"] + +_mcu-f-esp32 = ["dep:esp-alloc", "dep:embedded-svc", "dep:embassy-net", "dep:smoltcp", "dep:bleps", "dep:bleps-macros"] [dependencies] # mcu-esp32 stuff @@ -219,6 +174,7 @@ firmware_protocol = { path = "../networking/firmware_protocol", features = [ ] } paste = "1.0" load-dotenv = "0.1" +heapless = { version = "0.7", default-features = false } [build-dependencies] feature_utils = "0.0.0" diff --git a/firmware/src/networking/ble/esp.rs b/firmware/src/networking/ble/esp.rs index c8e997df..997b7e47 100644 --- a/firmware/src/networking/ble/esp.rs +++ b/firmware/src/networking/ble/esp.rs @@ -1,3 +1,5 @@ +use core::num::Wrapping; + use bleps::{ ad_structure::{ create_advertising_data, AdStructure, BR_EDR_NOT_SUPPORTED, @@ -6,14 +8,15 @@ use bleps::{ attribute_server::AttributeServer, Ble, HciConnector, }; -use defmt::{debug, error, trace, warn}; +use defmt::{debug, error, warn}; use embassy_futures::yield_now; use esp_wifi::{self, ble::controller::BleConnector, current_millis}; +use firmware_protocol::{CbPacket, Packet, SbPacket}; use crate::aliases::ඞ::NetConcrete; use crate::networking::Packets; -pub async fn network_task(_packets: &Packets, _net: NetConcrete) -> ! { +pub async fn network_task(packets: &Packets, _net: NetConcrete) -> ! { // HCI is the host-controller interface, which lets the MCU communicate to the BLE hardware through a standard // command interface let connector = BleConnector {}; @@ -39,49 +42,77 @@ pub async fn network_task(_packets: &Packets, _net: NetConcrete) -> ! { ble.cmd_set_le_advertise_enable(true) .expect("Failed to start advertising"); - let mut rf = || &b"Hello Bare-Metal BLE"[..]; - let mut wf = |offset: u16, data: &[u8]| { - trace!("RECEIVED: {} {=[u8]:a}", offset, data); + let mut sb_packet: Option> = None; + let mut cb_packet: Option> = None; + let mut sb_buf = [0; 512]; + + let mut sb_callback = || { + if let Some(packet) = sb_packet { + let nbytes = packet.serialize_into(&mut sb_buf).unwrap(); + let data = &sb_buf[..nbytes]; + + debug!("SEND: {=[u8]}", data); + sb_packet = None; + data + } else { + warn!("Data wasn't ready when we did work."); + &[] + } }; + let mut cb_callback = |offset: u16, data: &[u8]| { + debug!("RECV: {} {=[u8]}", offset, data); - // let mut wf2 = |offset: u16, data: &[u8]| { - // trace!("RECEIVED: {} {=[u8]:a}", offset, data); - // }; - // - // let mut rf3 = || &b"Hola!"[..]; - // let mut wf3 = |offset: u16, data: &[u8]| { - // trace!("RECEIVED: Offset {}, data {=[u8]:a}", offset, data); - // }; + if cb_packet.is_some() { + panic!("This shouldn't have been possible!:: We are about to drop data."); + } - use bleps_macros::gatt; + let Ok(packet) = Packet::deserialize_from(data) else { + warn!("Discarding bogus packet"); + return; + }; + cb_packet = Some(packet); + }; - gatt!([service { + bleps_macros::gatt!([service { uuid: "133712e0-2354-11eb-9f10-fbc30a62cf38", - characteristics: [ - characteristic { - uuid: "13370000-2354-11eb-9f10-fbc30a62cf38", - read: rf, - write: wf, - }, - // characteristic { - // uuid: "13371111-2354-11eb-9f10-fbc30a62cf38", - // write: wf2, - // }, - // characteristic { - // name: "my_characteristic", - // uuid: "13372222-2354-11eb-9f10-fbc30a62cf38", - // notify: true, - // read: rf3, - // write: wf3, - // }, - ], + characteristics: [characteristic { + uuid: "13370000-2354-11eb-9f10-fbc30a62cf38", + read: sb_callback, + write: cb_callback, + },], },]); let mut srv = AttributeServer::new(&mut ble, &mut gatt_attributes); + let mut tx_seq = 0; + let mut rx_seq = 0; debug!("Starting BLE loop"); loop { - yield_now().await; + // First we *must* handle any clientbound data, BEFORE doing work. Otherwise we may + // experience data loss. + if let Some(packet) = cb_packet { + let (rx_seq_new, packet) = packet.split(); + if rx_seq_new <= rx_seq { + warn!( + "Out of order packet received: {}, we are at {} ({})", + rx_seq_new, + rx_seq, + defmt::Debug2Format(&packet) + ); + } else { + packets.clientbound.send(packet).await; + rx_seq = rx_seq_new; + } + cb_packet = None; + } + + // Next, we load up the next packet. + // TODO: Switch to debug assert or remove. + assert!(sb_packet.is_none(), "This shouldn't be possible"); + sb_packet = Some(Packet::new(tx_seq, packets.serverbound.recv().await)); + tx_seq += 1; + + // Next, we will retrieve the next data to send the server. use bleps::attribute_server::WorkResult; match srv.do_work() { Ok(WorkResult::DidWork) => {}