radio: rewrite the networking stack, and implement some BLE magic

Cargo.toml

@@ -15,9 +15,10 @@ real-output = []
 dual-core = []
 simulation = ["dep:rmp"]
 radio = [
-  "dep:bleps",
   "dep:esp-radio",
-  "dep:reqwless"
+  "dep:reqwless",
+  "dep:trouble-host",
+  "esp-rtos/esp-radio"
 ]
 motion = ["mpu", "gps"]
 max-usb-power = []
@@ -53,7 +54,6 @@ embassy-executor = { version = "0.9.0", features = [
 ] }
 embassy-time = { version = "0.5.0", features = ["log"] }
 esp-rtos = { version = "0.2.0", features = [
-  "esp-radio",
   "embassy",
   "esp-alloc",
   "esp32s3",
@@ -85,9 +85,9 @@ esp-radio = { version = "*", optional = true, features = [
   "coex",
   "unstable"
 ] }
-bleps =  { git = "https://github.com/bjoernQ/bleps", optional = true, package = "bleps", rev = "a5148d8ae679e021b78f53fd33afb8bb35d0b62e", features = [ "macros", "async"] }
+embassy-net = { version = "0.7.1", features = ["alloc", "dns", "medium-ethernet", "proto-ipv4", "tcp", "udp", "dhcpv4"] }
-embedded-graphics = { version = "0.8.1", features = ["nalgebra_support"] }
+reqwless = { version = "0.13.0", optional = true, features = ["log", "alloc"] }
-ssd1306 = { version = "0.10.0", features = ["async"], optional = true }
+trouble-host = { version = "0.5.1", optional = true, features = ["log"] }
 
 # Sensors
 nmea = { version = "0.7.0", optional = true, default-features = false, features = [
@@ -127,6 +127,9 @@ lto              = 'fat'
 opt-level        = 's'
 overflow-checks  = false
 
+[profile.dev.package.esp-radio]
+opt-level = 3
+
 [build-dependencies]
 image = "0.25"
 rmp = { path = "../msgpack-rust/rmp/" }

src/bin/main.rs

@@ -7,13 +7,13 @@
 )]
 
 
-use core::ptr::addr_of_mut;
+use core::{num::{self, Wrapping}, ptr::addr_of_mut};
 
-use alloc::sync::Arc;
+use alloc::{string::String, sync::Arc};
 use embassy_executor::Spawner;
 use embassy_time::{Instant, Timer};
 
-use esp_hal::{gpio::{Output, OutputConfig, Pin}, time::Rate};
+use esp_hal::{gpio::{Output, OutputConfig, Pin}, time::Rate, xtensa_lx::debug_break};
 use esp_hal::{
     clock::CpuClock, system::{AppCoreGuard, CpuControl, Stack}, timer::{systimer::SystemTimer, timg::{TimerGroup, Wdt}}
 };
@@ -22,7 +22,7 @@ use embassy_sync::{
     pubsub::PubSubChannel,
     blocking_mutex::raw::NoopRawMutex
 };
-
+use static_cell::ConstStaticCell;
 use log::*;
 use renderbug_embassy::{events::Prediction, graphics::display::DisplayControls, logging::RenderbugLogger, tasks::{oled::{OledUI, OledUiSurfacePool, oled_ui}, safetyui::{SafetyUi, safety_ui_main}, ui::UiSurfacePool}};
 use renderbug_embassy::events::Measurement;
@@ -45,9 +45,6 @@ extern crate alloc;
 // For more information see: <https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/system/app_image_format.html#application-description>
 esp_bootloader_esp_idf::esp_app_desc!();
 
-static STATIC_HI_EXEC: StaticCell<InterruptExecutor<2>> = StaticCell::new();
-static CORE2_EXEC: StaticCell<Executor> = StaticCell::new();
-static mut CORE2_STACK: Stack<16384> = Stack::new();
 #[cfg(feature="radio")]
 static WIFI_INIT: StaticCell<esp_radio::Controller<'static>> = StaticCell::new();
 
@@ -83,14 +80,14 @@ async fn main(spawner: Spawner) {
     let timer0 = TimerGroup::new(peripherals.TIMG0);
     let mut wdt = timer0.wdt;
     wdt.set_timeout(esp_hal::timer::timg::MwdtStage::Stage0, esp_hal::time::Duration::from_secs(5));
-    //wdt.enable();
+    wdt.enable();
-
-    let swi = SoftwareInterruptControl::new(peripherals.SW_INTERRUPT);
-    //let hi_exec = STATIC_HI_EXEC.init(InterruptExecutor::new(swi.software_interrupt2));
-    //let hi_spawn = hi_exec.start(esp_hal::interrupt::Priority::max());
 
+    // Spawn the rendering task as soon as possible so it can start pushing pixels
     spawner.must_spawn(renderbug_embassy::tasks::render::render(peripherals.RMT, peripherals.GPIO5.degrade(), surfaces, safety_surfaces, display_controls, wdt));
 
+    // Wait one scheduler tick for the rendering task to get initialized
+    Timer::after_ticks(1).await;
+
     #[cfg(feature="motion")]
     {
         use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
@@ -131,7 +128,7 @@ async fn main(spawner: Spawner) {
         let mut storage = SharedFlash::new(FlashStorage::new());
         let mut buf = [8; 1024];
         let partitions = esp_bootloader_esp_idf::partitions::read_partition_table(&mut storage, &mut buf).unwrap();
-        for sim_data in SimDataTable::open(storage, partitions).expect("Could not find partition for sim data!") {
+        for sim_data in SimDataTable::open(storage, partitions).expect("Could not find sim data!") {
             let srcid = sim_data.srcid();
             info!("Found simulation data for {srcid:?}");
             if spawner.spawn(renderbug_embassy::tasks::simulation::simulation_task(sim_data, motion_bus.dyn_sender())).is_err() {
@@ -141,15 +138,23 @@ async fn main(spawner: Spawner) {
     }
 
     #[cfg(feature="radio")]
-    let wifi_init = {
+    let (wifi, network_device, ble) = {
         info!("Configuring wifi");
-        WIFI_INIT.init_with(|| {esp_radio::init().expect("Failed to initialize radio controller")})
+        esp_radio::wifi_set_log_verbose();
+        let wifi_init = WIFI_INIT.init_with(|| {esp_radio::init().expect("Failed to initialize radio controller")});
+
+        let ble = esp_radio::ble::controller::BleConnector::new(wifi_init, peripherals.BT, esp_radio::ble::Config::default()).unwrap();
+
+        let (wifi, interfaces) = esp_radio::wifi::new(wifi_init, peripherals.WIFI, esp_radio::wifi::Config::default())
+            .expect("Failed to initialize WIFI!");
+
+        (wifi, interfaces.sta, ble)
     };
 
-    info!("Starting core 2");
-
     let core2_main = |spawner: Spawner| {
-        static PREDICTIONS: StaticCell<PubSubChannel<NoopRawMutex, Prediction, 15, 5, 1>> = StaticCell::new();
+        info!("Starting application tasks");
+
+        static PREDICTIONS: StaticCell<PubSubChannel<NoopRawMutex, Prediction, 15, 6, 1>> = StaticCell::new();
         let predictions = PREDICTIONS.init(PubSubChannel::new());
 
         #[cfg(not(feature="demo"))]
@@ -173,8 +178,26 @@ async fn main(spawner: Spawner) {
 
         #[cfg(feature="radio")]
         {
-            info!("Launching networking stack");
+            use embassy_net::StackResources;
-            spawner.must_spawn(renderbug_embassy::tasks::wifi::wireless_task(predictions.dyn_subscriber().unwrap(), wifi_init, peripherals.WIFI));
+            use esp_hal::rng::Rng;
+            use static_cell::ConstStaticCell;
+
+            info!("Setting up networking stack");
+            static RESOURCES: ConstStaticCell<StackResources<5>> = ConstStaticCell::new(StackResources::new());
+            let config = embassy_net::Config::dhcpv4(Default::default());
+            let seed = Rng::new().random() as i32;
+            let (stack, runner) = embassy_net::new(network_device, config, RESOURCES.take(), seed as u64);
+            info!("Launching network services");
+            //spawner.must_spawn(renderbug_embassy::tasks::wifi::net_task(runner));
+
+            info!("Starting connectivity task");
+            //spawner.must_spawn(renderbug_embassy::tasks::wifi::wifi_connect_task(wifi, stack, motion_bus.dyn_sender()));
+
+            info!("Launching HTTP telemetry");
+            //spawner.must_spawn(renderbug_embassy::tasks::wifi::http_telemetry_task(predictions.dyn_subscriber().unwrap(), stack));
+
+            info!("Starting BLE services");
+            spawner.must_spawn(renderbug_embassy::tasks::ble::ble_task(ble, predictions.dyn_subscriber().unwrap(), spawner));
         }
 
         #[cfg(feature="dual-core")]
@@ -194,13 +217,21 @@ async fn main(spawner: Spawner) {
     };
 
     #[cfg(feature="dual-core")]
+    {
+        static mut CORE2_STACK: Stack<16384> = Stack::new();
+        let swi = SoftwareInterruptControl::new(peripherals.SW_INTERRUPT);
         esp_rtos::start_second_core(peripherals.CPU_CTRL, swi.software_interrupt0, swi.software_interrupt1, unsafe { &mut *addr_of_mut!(CORE2_STACK) }, || {
+            info!("Second CPU core started");
+            static CORE2_EXEC: StaticCell<Executor> = StaticCell::new();
             let exec = CORE2_EXEC.init_with(|| { Executor::new() });
             exec.run(core2_main);
         });
+    }
 
     #[cfg(not(feature="dual-core"))]
     core2_main(spawner);
+
+    info!("Ready to rock and roll");
 }
 
 #[embassy_executor::task]
@@ -216,7 +247,11 @@ async fn wdt_task(mut wdt: Wdt<esp_hal::peripherals::TIMG1<'static>>) {
 
 #[embassy_executor::task]
 async fn print_telemetry(mut events: DynSubscriber<'static, Prediction>) {
+    info!("telemetry ready");
+    let mut num_events = Wrapping(0usize);
     loop {
-        info!("predict={:?}", events.next_message_pure().await);
+        let next = events.next_message_pure().await;
+        trace!("idx={} predict={next:?}", num_events.0);
+        num_events += 1;        
     }
 }

src/events.rs

@@ -74,10 +74,14 @@ pub enum SensorSource {
     IMU,
     GPS,
 
+    // Connectivity related
+    Wifi,
+
     // Fusion outputs
     GravityReference,
     ForwardsReference,
     Location,
+    Cloud,
 
     // Simulated sensors
     Demo,

src/graphics/oled_ui.rs

@@ -115,6 +115,10 @@ const SENSOR_IMAGES: &[SensorImage] = &[
         source: SensorSource::Location,
         on: &images::LOCATION_ON, off: &images::LOCATION_OFF,
     },
+    SensorImage {
+        source: SensorSource::Wifi,
+        on: &images::ONLINE_CONNECTING, off: &images::OFFLINE
+    },
     #[cfg(feature="demo")]
     SensorImage {
         source: SensorSource::Demo,

src/tasks/ble.rs

@@ -0,0 +1,224 @@
+
+use core::ptr::slice_from_raw_parts;
+
+use embassy_executor::Spawner;
+use embassy_futures::select::Either;
+use embassy_sync::{blocking_mutex::raw::NoopRawMutex, pubsub::{DynSubscriber, PubSubChannel, Publisher}};
+use esp_radio::ble::controller::BleConnector;
+use static_cell::{ConstStaticCell, StaticCell};
+use trouble_host::{attribute, prelude::*, types::gatt_traits::FromGattError};
+use log::*;
+
+use crate::{backoff::Backoff, events::Prediction};
+
+#[gatt_server]
+struct SerialServer {
+    serial_service: SerialService,
+    location_service: LocationService,
+    renderbug_service: RenderbugService
+}
+
+#[gatt_service(uuid="00000000-f43a-48c4-8411-efe6a38a5d75")]
+struct RenderbugService {
+    // 32 bit float, 0 exponent, m/s velocity unit
+    #[descriptor(uuid=descriptors::CHARACTERISTIC_PRESENTATION_FORMAT, read, value = [0x14, 0x0, 0x27, 0x12, 0x01, 0x00, 0x00])]
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, read, value = "Speed (m/s)")]
+    #[characteristic(uuid="00000001-f43a-48c4-8411-efe6a38a5d75", notify)]
+    speed: f32,
+
+    // 32 bit float, 0 exponent, unitless
+    #[descriptor(uuid=descriptors::CHARACTERISTIC_PRESENTATION_FORMAT, read, value = [0x14, 0x0, 0x27, 0x00, 0x01, 0x00, 0x00])]
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, read, value = "Latitude")]
+    #[characteristic(uuid="00000002-f43a-48c4-8411-efe6a38a5d75", notify)]
+    latitude: f32,
+
+    // 32 bit float, 0 exponent, unitless
+    #[descriptor(uuid=descriptors::CHARACTERISTIC_PRESENTATION_FORMAT, read, value = [0x14, 0x0, 0x27, 0x00, 0x01, 0x00, 0x00])]
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, read, value = "Longitude")]
+    #[characteristic(uuid="00000003-f43a-48c4-8411-efe6a38a5d75", notify)]
+    longitude: f32,
+}
+
+#[gatt_service(uuid=service::LOCATION_AND_NAVIGATION)]
+struct LocationService {
+    #[characteristic(uuid=characteristic::LOCATION_AND_SPEED, notify)]
+    location_data: LocationData,
+    #[characteristic(uuid=characteristic::LN_FEATURE, read, value=0b0000101)] // Location and speed fields
+    features: u8,
+}
+
+#[derive(Default, Debug)]
+struct LocationData {
+    coordinates: [i32; 2],
+    velocity: u16
+}
+
+impl FixedGattValue for LocationData {
+    fn as_gatt(&self) -> &[u8] {
+        let databuf = [0;Self::SIZE];
+        unsafe { core::slice::from_raw_parts(self as *const Self as *const u8, Self::SIZE) }
+    }
+    
+    const SIZE: usize = core::mem::size_of::<Self>();
+    
+    fn from_gatt(data: &[u8]) -> Result<Self, FromGattError> {
+        if data.len() != Self::SIZE {
+            Err(FromGattError::InvalidLength)
+        } else {
+            // SAFETY
+            // - Pointer is considered "valid" as per the rules outlined for validity in std::ptr v1.82.0
+            // - Pointer was generated from a slice of bytes matching the size of the type implementing Primitive, and all types implementing Primitive are valid for all possible configurations of bits
+            // - Primitive trait is constrained to require Copy
+            unsafe { Ok((data.as_ptr() as *const Self).read_unaligned()) }
+        }
+    }
+}
+
+#[gatt_service(uuid="6E400001-B5A3-F393-E0A9-E50E24DCCA9E")]
+struct SerialService {
+    // The outside world writes here
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, read, value = "RX")]
+    #[characteristic(uuid = "6E400002-B5A3-F393-E0A9-E50E24DCCA9E", write)]
+    rx: heapless::String<128>,
+
+    // The outside world reads from here
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, read, value = "TX")]
+    #[characteristic(uuid = "6E400003-B5A3-F393-E0A9-E50E24DCCA9E", notify)]
+    tx: heapless::String<128>
+}
+
+#[embassy_executor::task]
+async fn client_prediction_task(mut src: DynSubscriber<'static, Prediction>, sink: Publisher<'static, NoopRawMutex, Prediction, 5, 1, 1>) {
+    debug!("Started BLE client prediction stream");
+    loop {
+        sink.publish(src.next_message_pure().await).await;
+    }
+}
+
+static STATIC_RESOURCES: StaticCell<HostResources<DefaultPacketPool, 1, 1>> = StaticCell::new();
+static STATIC_STACK: StaticCell<Stack<'static, ExternalController<BleConnector<'static>, 1>, DefaultPacketPool>> = StaticCell::new();
+static STATIC_SERVER: StaticCell<SerialServer> = StaticCell::new();
+static STATIC_CLIENT_PREDICTIONS: StaticCell<PubSubChannel<NoopRawMutex, Prediction, 5, 1, 1>> = StaticCell::new();
+
+#[embassy_executor::task]
+pub async fn ble_task(ble: BleConnector<'static>, predictions: DynSubscriber<'static, Prediction>, spawner: Spawner) {
+    info!("Starting BLE stack");
+    let server = STATIC_SERVER.init(SerialServer::new_with_config(GapConfig::Peripheral(PeripheralConfig { name: "Renderbug", appearance: &appearance::light_source::LED_ARRAY })).unwrap());
+    let control: ExternalController<esp_radio::ble::controller::BleConnector<'_>, 1> = ExternalController::new(ble);
+    let stack = STATIC_STACK.init(trouble_host::new(control, STATIC_RESOURCES.init(HostResources::new())));
+    let Host { mut peripheral, mut runner, .. } = stack.build();
+
+    let client_predictions = STATIC_CLIENT_PREDICTIONS.init(PubSubChannel::new());
+    spawner.must_spawn(client_prediction_task(predictions, client_predictions.publisher().unwrap()));
+
+    // The host task must be started and ticking beore we can start advertising, so we use a join() here instead of a separate task
+    let _ = embassy_futures::join::join(
+        runner.run(),
+        async {
+            loop {
+                let advertiser = Backoff::from_secs(5).forever().attempt(async || {
+                    info!("Starting BLE advertising");
+                    let mut adv_data = [0; 64];
+                    let len = AdStructure::encode_slice(
+                        &[
+                            AdStructure::CompleteLocalName("Renderbug".as_bytes()),
+                            AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED),
+                            AdStructure::ServiceUuids128(&[
+                                [ // Serial service
+                                    0x6E, 0x40, 0x00, 0x01,
+                                    0xB5, 0xA3,
+                                    0xF3, 0x93,
+                                    0xE0, 0xA9,
+                                    0xE5, 0x0E, 0x24, 0xDC, 0xCA, 0x9E
+                                    //0x9E, 0xCA, 0xDC, 0x24, 0x0E, 0xE5, 0xA9, 0xE0, 0x93, 0xF3, 0xA3, 0xB5,
+                                    //0x01, 0x00, 0x40, 0x6E,
+                                ],
+                                [ // Renderbug mesh service
+                                    0x00, 0x00, 0x00, 0x00,
+                                    0xf4, 0x3a,
+                                    0x48, 0xc4,
+                                    0x84, 0x11,
+                                    0xef, 0xe6, 0xa3, 0x8a, 0x5d, 0x75
+                                ]
+                            ]),
+                            AdStructure::ServiceUuids16(&[
+                                // Location and navigation
+                                [0x18, 0x19]
+                            ])
+                        ],
+                        &mut adv_data[..],
+                    ).unwrap();
+                    peripheral.advertise(
+                        &Default::default(),
+                        Advertisement::ConnectableScannableUndirected { adv_data: &adv_data[..len], scan_data: &[] }
+                    ).await
+                }).await.unwrap();
+                info!("Waiting for connection");
+                match advertiser.accept().await.and_then(|raw| { raw.with_attribute_server(server) }) {
+                    Ok(conn) => {
+                        if spawner.spawn(ble_connection(conn, server, client_predictions.dyn_subscriber().unwrap())).is_err() {
+                            error!("Unable to spawn task for handling BLE connection! Please increase the embassy task pool size!");
+                        }
+                    },
+                    Err(e) => {
+                        error!("Failed to accept new connection: {e:?}");
+                    },
+                }
+            }
+        }
+    ).await;
+}
+
+#[embassy_executor::task]
+async fn ble_connection(conn: GattConnection<'static, 'static, DefaultPacketPool>, server: &'static SerialServer<'static>, mut predictions: DynSubscriber<'static, Prediction>) {
+    info!("Started handling new connection with {:?}", conn.raw().peer_identity());
+    let mut location_data = LocationData::default();
+    loop {
+        let next = embassy_futures::select::select(conn.next(), predictions.next_message_pure()).await;
+        match next {
+            Either::First(gatt_event) => {
+                match gatt_event {
+                    GattConnectionEvent::Disconnected { reason } => {
+                        warn!("BLE Disconnected: {reason:?}");
+                        return;
+                    },
+                    GattConnectionEvent::Gatt { event } => {
+                        if let GattEvent::Write(event) = event {
+                            if event.handle() == server.serial_service.rx.handle {
+                                let buf = event.value(&server.serial_service.rx).unwrap();
+                                info!("Received serial data {buf:?}");
+                            }
+                        }
+                    },
+                    _ => trace!("GATT event")
+                }
+            },
+            Either::Second(prediction) => {
+                let evt_str = heapless::format!("{prediction:?}\n").unwrap();
+                server.serial_service.tx.notify(&conn, &evt_str).await.unwrap();
+                let update_location = match prediction {
+                    Prediction::Velocity(v) => {
+                        location_data.velocity = (v * 100.0) as u16;
+                        server.renderbug_service.speed.notify(&conn, &v).await.unwrap();
+                        true
+                    },
+                    Prediction::Location(coords) => {
+                        location_data.coordinates[0] = (coords.x * 1e7) as i32;
+                        location_data.coordinates[1] = (coords.y * 1e7) as i32;
+                        server.renderbug_service.latitude.notify(&conn, &(coords.x as f32)).await.unwrap();
+                        server.renderbug_service.longitude.notify(&conn, &(coords.y as f32)).await.unwrap();
+                        true
+                    },
+                    _ => false
+                };
+
+                if update_location {
+                    server.location_service.location_data.notify(&conn, &location_data).await.unwrap();
+                    // TODO: write out NMEA sentences
+                    //let mut nmea_str = heapless::String::from_utf8("foo");
+                    //server.nmea_service.tx.notify(&conn, "foo").await.unwrap();
+                }
+            }
+        }
+    }
+}

src/tasks/mod.rs

@@ -4,6 +4,8 @@ pub mod mpu;
 pub mod gps;
 #[cfg(feature="radio")]
 pub mod wifi;
+#[cfg(feature="radio")]
+pub mod ble;
 #[cfg(feature="simulation")]
 pub mod simulation;
 #[cfg(feature="demo")]

src/tasks/wifi.rs

@@ -1,71 +1,83 @@
 use alloc::string::ToString;
-use embassy_executor::Spawner;
+use embassy_sync::channel::DynamicSender;
 use embassy_sync::pubsub::DynSubscriber;
-use esp_radio::Controller;
+use embassy_time::{Duration, Instant, WithTimeout};
-use esp_radio::wifi::{ClientConfig, WifiDevice};
+use esp_hal::rng::Rng;
+use esp_radio::wifi::{ClientConfig, ScanConfig, WifiController, WifiDevice, WifiEvent};
 use log::*;
 use alloc::format;
 
 use embassy_net::dns::DnsSocket;
 use embassy_net::tcp::client::{TcpClient, TcpClientState};
-use embassy_net::{Config, StackResources};
+use embassy_net::Stack;
 use nalgebra::Vector2;
 use reqwless::client::{HttpClient, TlsConfig};
-use static_cell::StaticCell;
 
+use crate::ego::engine::gps_to_local_meters_haversine;
+use crate::events::{Measurement, SensorSource, SensorState};
 use crate::{backoff::Backoff, events::{Prediction}};
 
 #[embassy_executor::task]
-async fn net_task(mut runner: embassy_net::Runner<'static, WifiDevice<'static>>) {
+pub async fn net_task(mut runner: embassy_net::Runner<'static, WifiDevice<'static>>) {
-    info!("Network stack is running");
+    info!("Starting network stack");
     runner.run().await
 }
 
-static RESOURCES: StaticCell<StackResources<5>> = StaticCell::new();
-
-// TODO: Wifi task needs to know when there is data to upload, so it only connects when needed.
 #[embassy_executor::task]
-pub async fn wireless_task(mut predictions: DynSubscriber<'static, Prediction>, wifi_init: &'static mut Controller<'static>, wifi_device: esp_hal::peripherals::WIFI<'static>) {
+pub async fn wifi_connect_task(mut wifi: WifiController<'static>, stack: Stack<'static>, motion: DynamicSender<'static, Measurement>) {
-    let (mut wifi, interfaces) = esp_radio::wifi::new(wifi_init, wifi_device, esp_radio::wifi::Config::default())
-            .expect("Failed to initialize WIFI!");
     wifi.set_config(&esp_radio::wifi::ModeConfig::Client(
         ClientConfig::default()
-            .with_ssid("The Frequencey".to_string())
+            .with_ssid("The Frequency".to_string())
             .with_auth_method(esp_radio::wifi::AuthMethod::Wpa2Personal)
             .with_password("thepasswordkenneth".to_string())
     )).unwrap();
     wifi.set_mode(esp_radio::wifi::WifiMode::Sta).unwrap();
     wifi.set_power_saving(esp_radio::wifi::PowerSaveMode::Maximum).unwrap();
-    wifi.start_async().await.unwrap();
-
-    let device = interfaces.sta;
-    // TODO: Somehow grab a real random seed from main()
-    let seed = 0;
-
-    let config = Config::dhcpv4(Default::default());
-    let (stack, runner) = embassy_net::new(device, config, RESOURCES.init_with(|| { StackResources::new() }), seed as u64);
-    info!("Launching network task");
-    unsafe { Spawner::for_current_executor().await }.must_spawn(net_task(runner));
 
     loop {
         Backoff::from_secs(3).forever().attempt(async || {
             info!("Connecting to wifi...");
+            wifi.start_async().await.unwrap();
+            motion.send(Measurement::SensorHardwareStatus(SensorSource::Wifi, crate::events::SensorState::AcquiringFix)).await;
+            let networks = wifi.scan_with_config_async(ScanConfig::default().with_show_hidden(true)).await.unwrap();
+            for network in networks {
+                info!("wifi: {} @ {}db", network.ssid, network.signal_strength);
+            }
             match wifi.connect_async().await {
                 Ok(_) => Ok(()),
                 Err(e) => {
-                    error!("Wifi error: {e:?}");
+                    error!("Unable to connect to wifi: {e:?}");
+                    wifi.stop_async().await.unwrap();
                     Err(())
                 }
             }
         }).await.unwrap();
 
-        info!("Waiting for DHCP");
+        info!("Waiting for DHCP...");
-        stack.wait_config_up().await;
+        motion.send(Measurement::SensorHardwareStatus(SensorSource::Wifi, SensorState::Degraded)).await;
-
+        if stack.wait_config_up().with_timeout(Duration::from_secs(5)).await.is_ok() {
             info!("Online!");
+            motion.send(Measurement::SensorHardwareStatus(SensorSource::Wifi, SensorState::Online)).await;
             let ip_cfg = stack.config_v4().unwrap();
             info!("ip={ip_cfg:?}");
+            wifi.wait_for_event(WifiEvent::ApStaDisconnected).await;
+            info!("Wifi disconnected!");
+        } else {
+            warn!("DHCP timed out after 5 seconds. Disconnecting wifi");
+            wifi.disconnect_async().await.unwrap();
+        }
+        warn!("Stopping wifi device");
+        wifi.stop_async().await.unwrap();
+        motion.send(Measurement::SensorHardwareStatus(SensorSource::Wifi, SensorState::Offline)).await;
+    }
+}
 
+// TODO: Wifi task needs to know when there is data to upload, so it only connects when needed.
+#[embassy_executor::task]
+pub async fn http_telemetry_task(mut predictions: DynSubscriber<'static, Prediction>, stack: Stack<'static>) {
+    // TODO: should wait for wifi disconnect event somehow and use that to restart sending the wifi around
+
+    let seed = Rng::new().random() as i32;
     let mut rx_buf = [0; 4096];
     let mut tx_buf = [0; 4096];
     let dns = DnsSocket::new(stack);
@@ -80,21 +92,33 @@ pub async fn wireless_task(mut predictions: DynSubscriber<'static, Prediction>,
 
     let mut client = HttpClient::new_with_tls(&tcp, &dns, tls);
 
+    // TODO: Only should upload a data point after some distance has occurred
+    let mut last_location = Vector2::default();
+    let mut last_push = Instant::from_ticks(0);
+
     loop {
         if let Prediction::Location(coords) = predictions.next_message_pure().await {
-                if let Err(e) = push_location(&mut client, coords).await {
+
-                    error!("HTTP error in publishing location: {e:?}");
+            if stack.is_config_up() {
-                    break
+                // Only push to HTTP if we have an ip config etc
+                if last_push.elapsed().as_secs() >= 5 || gps_to_local_meters_haversine(&last_location, &coords).norm() >= 10.0 {
+                    last_location = coords;
+                    last_push = Instant::now();
+                    if let Err(e) = Backoff::from_secs(3).attempt(async || {
+                        push_location(&mut client, coords, Instant::now().as_millis()).await
+                    }).await {
+                        warn!("Could not submit location! {e:?}");
+                    }
                 }
             }
         }
     }
 }
 
-async fn push_location(client: &mut HttpClient<'_, TcpClient<'_, 1, 4096, 4096>, DnsSocket<'_>>, location: Vector2<f64>) -> Result<(), reqwless::Error> {
+async fn push_location(client: &mut HttpClient<'_, TcpClient<'_, 1, 4096, 4096>, DnsSocket<'_>>, location: Vector2<f64>, timestamp: u64) -> Result<(), reqwless::Error> {
     let mut buffer = [0u8; 4096];
     let base = "https://nextcloud.malloc.hackerbots.net/nextcloud/index.php/apps/phonetrack/logGet/a062000067304e9dee6590f1b8f9e0db/renderbug";
-    let url = format!("{base}?lat={}&lon={}", location.y, location.x);
+    let url = format!("{base}?lon={}&lat={}&timestamp={}", location.y, location.x, timestamp);
     info!("Pushing to {url}");
     let mut http_req = client
         .request(
@@ -104,10 +128,9 @@ async fn push_location(client: &mut HttpClient<'_, TcpClient<'_, 1, 4096, 4096>,
         .await?;
     let response = http_req.send(&mut buffer).await?;
 
-    info!("Got response");
     let res = response.body().read_to_end().await?;
 
     let content = core::str::from_utf8(res).unwrap();
-    info!("{content}");
+    debug!("HTTP response: {content}");
     Ok(())
 }