dual core setup

flake.nix

@@ -53,6 +53,7 @@ shellHook = ''
   rustc --version
   which xtensa-esp32-elf-gcc || echo "⚠️ xtensa-esp32-elf-gcc not found in PATH"
 
+  echo "cargo espflash save-image --release --chip esp32 test.bin"
   echo ""
   echo "MQTT broker test (run manually in two terminals):"
   echo "  Terminal 1:"

mqtt_display/Cargo.lock

@@ -246,16 +246,6 @@ dependencies = [
  "thiserror",
 ]
 
-[[package]]
-name = "defmt-rtt"
-version = "1.0.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b2cac3b8a5644a9e02b75085ebad3b6deafdbdbdec04bb25086523828aa4dfd1"
-dependencies = [
- "critical-section",
- "defmt 1.0.1",
-]
-
 [[package]]
 name = "delegate"
 version = "0.13.4"
@@ -570,6 +560,16 @@ dependencies = [
  "embedded-hal 1.0.0",
 ]
 
+[[package]]
+name = "embedded-hal-bus"
+version = "0.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "513e0b3a8fb7d3013a8ae17a834283f170deaf7d0eeab0a7c1a36ad4dd356d22"
+dependencies = [
+ "critical-section",
+ "embedded-hal 1.0.0",
+]
+
 [[package]]
 name = "embedded-io"
 version = "0.6.1"
@@ -821,6 +821,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "3e7e3ab41e96093d7fd307e93bfc88bd646a8ff23036ebf809e116b18869f719"
 dependencies = [
  "critical-section",
+ "defmt 1.0.1",
  "document-features",
  "esp-metadata-generated",
  "log",
@@ -1430,16 +1431,18 @@ dependencies = [
 
 [[package]]
 name = "projekt_final"
-version = "0.1.0"
+version = "0.2.0"
 dependencies = [
  "critical-section",
- "defmt-rtt",
+ "defmt 1.0.1",
  "dotenvy",
  "embassy-executor",
  "embassy-futures",
  "embassy-net",
  "embassy-sync 0.7.2",
  "embassy-time 0.5.0",
+ "embedded-hal 1.0.0",
+ "embedded-hal-bus",
  "embedded-io",
  "embedded-io-async",
  "esp-alloc",

mqtt_display/Cargo.toml

@@ -34,7 +34,7 @@ esp-backtrace = { version = "0.17.0", features = [
   "panic-handler",
   "println",
 ] }
-esp-println = { version = "0.15.0", features = ["esp32", "log-04"] }
+esp-println = { version = "0.15.0", features = ["esp32", "log-04", "defmt-espflash"] }
 # for more networking protocol support see https://crates.io/crates/edge-net
 critical-section = "1.2.0"
 embassy-executor = { version = "0.7.0", features = [
@@ -67,13 +67,15 @@ smoltcp = { version = "0.12.0", default-features = false, features = [
 ] }
 static_cell = "2.1.1"
 rust-mqtt = { version = "0.3.0", default-features = false, features = ["no_std"] }
-defmt-rtt = "1.0.0"
 embassy-futures = "0.1.2"
 embassy-sync = "0.7.2"
 heapless = "0.9.1"
 mousefood = { git = "https://github.com/j-g00da/mousefood", branch = "main", default-features = false }
 ssd1306 = "0.10.0"
 ratatui = { version = "0.30.0", default-features = false, features = ["macros", "all-widgets", "portable-atomic"] }
+embedded-hal-bus = "0.3.0"
+embedded-hal = "1.0.0"
+defmt = "1.0.1"
 
 [build-dependencies]
 dotenvy = "0.15.7"

mqtt_display/old_main.rs

@@ -0,0 +1,181 @@
+// src/bin/main.rs
+
+#![no_std]
+#![no_main]
+#![deny(
+    clippy::mem_forget,
+    reason = "mem::forget is generally not safe to do with esp_hal types"
+)]
+
+use embassy_executor::Spawner;
+use embassy_futures::select::{select, Either};
+use embassy_net::{Runner, StackResources};
+use embassy_time::{Duration, Timer};
+use esp_alloc as _;
+use esp_backtrace as _;
+use esp_hal::{clock::CpuClock, rng::Rng, timer::timg::TimerGroup};
+use esp_wifi::{
+    init,
+    wifi::{ClientConfiguration, Configuration, WifiController, WifiDevice, WifiEvent, WifiState},
+    EspWifiController,
+};
+use log::info;
+use rust_mqtt::packet::v5::publish_packet::QualityOfService;
+use projekt_final::mqtt::client::{
+    mqtt_events, mqtt_publish, mqtt_subscribe, mqtt_task, IncomingMsg,
+};
+use defmt_rtt as _;
+
+extern crate alloc;
+
+esp_bootloader_esp_idf::esp_app_desc!();
+
+macro_rules! mk_static {
+    ($t:ty,$val:expr) => {{
+        static STATIC_CELL: static_cell::StaticCell<$t> = static_cell::StaticCell::new();
+        #[deny(unused_attributes)]
+        let x = STATIC_CELL.uninit().write(($val));
+        x
+    }};
+}
+
+const SSID: &str = env!("SSID");
+const PASSWORD: &str = env!("PASSWORD");
+
+#[esp_hal_embassy::main]
+async fn main(spawner: Spawner) -> ! {
+    esp_println::logger::init_logger_from_env();
+    let config = esp_hal::Config::default().with_cpu_clock(CpuClock::max());
+    let peripherals = esp_hal::init(config);
+
+    esp_alloc::heap_allocator!(size: 72 * 1024);
+
+    let timg0 = TimerGroup::new(peripherals.TIMG0);
+    let mut rng = Rng::new(peripherals.RNG);
+
+    let esp_wifi_ctrl = &*mk_static!(
+        EspWifiController<'static>,
+        init(timg0.timer0, rng.clone()).unwrap()
+    );
+
+    let (controller, interfaces) =
+        esp_wifi::wifi::new(&esp_wifi_ctrl, peripherals.WIFI).unwrap();
+
+    let wifi_interface = interfaces.sta;
+
+    let timg1 = TimerGroup::new(peripherals.TIMG1);
+    esp_hal_embassy::init(timg1.timer0);
+
+    let config = embassy_net::Config::dhcpv4(Default::default());
+
+    let seed = (rng.random() as u64) << 32 | rng.random() as u64;
+
+    // Init network stack
+    let (stack, runner) = embassy_net::new(
+        wifi_interface,
+        config,
+        mk_static!(StackResources<3>, StackResources::<3>::new()),
+        seed,
+    );
+
+    spawner.spawn(connection(controller)).ok();
+    spawner.spawn(net_task(runner)).ok();
+
+    // Wait for link up
+    loop {
+        if stack.is_link_up() {
+            break;
+        }
+        Timer::after(Duration::from_millis(500)).await;
+    }
+
+    info!("Waiting to get IP address...");
+    loop {
+        if let Some(config) = stack.config_v4() {
+            info!("Got IP: {}", config.address);
+            break;
+        }
+        Timer::after(Duration::from_millis(500)).await;
+    }
+
+    spawner.spawn(mqtt_task(stack)).expect("failed to spawn MQTT task");
+    info!("MQTT task started");
+
+    mqtt_publish("esp32/topic", b"hello from ESP32 (init)", QualityOfService::QoS1, false).await;
+    info!("Sent initial MQTT message");
+
+    mqtt_subscribe("esp32/topic").await;
+
+    // Get a receiver for incoming MQTT messages
+    let mqtt_rx = mqtt_events();
+
+    loop {
+        // Drive both: either process an MQTT message or publish periodically
+        match select(mqtt_rx.receive(), Timer::after(Duration::from_secs(5))).await
+        {
+            // Received inbound MQTT message (from broker)
+            Either::First(msg) => {
+                handle_incoming(msg);
+            }
+            // Time-based example publish
+            Either::Second(_) => {
+                // mqtt_publish(
+                //     "esp32/topic",
+                //     b"hello from main",
+                //     QualityOfService::QoS1,
+                //     false,
+                // )
+                // .await;
+            }
+        }
+    }
+}
+
+fn handle_incoming(msg: IncomingMsg) {
+    if let Ok(txt) = core::str::from_utf8(&msg.payload) {
+        info!("MAIN RX [{}]: {}", msg.topic.as_str(), txt);
+        info!("Received MQTT message -> topic: '{}', payload: '{}'", msg.topic.as_str(), txt);
+    } else {
+        info!("MAIN RX [{}]: {:?}", msg.topic.as_str(), msg.payload);
+    }
+}
+
+#[embassy_executor::task]
+async fn connection(mut controller: WifiController<'static>) {
+    info!("start connection task");
+    info!("Device capabilities: {:?}", controller.capabilities());
+    loop {
+        match esp_wifi::wifi::wifi_state() {
+            WifiState::StaConnected => {
+                controller.wait_for_event(WifiEvent::StaDisconnected).await;
+                Timer::after(Duration::from_millis(5000)).await
+            }
+            _ => {}
+        }
+        if !matches!(controller.is_started(), Ok(true)) {
+            let client_config = Configuration::Client(ClientConfiguration {
+                ssid: SSID.into(),
+                password: PASSWORD.into(),
+                ..Default::default()
+            });
+            controller.set_configuration(&client_config).unwrap();
+            info!("Starting wifi");
+            controller.start_async().await.unwrap();
+            info!("Wifi started!");
+        }
+        info!("About to connect...");
+
+        match controller.connect_async().await {
+            Ok(_) => info!("Wifi connected!"),
+            Err(e) => {
+                info!("Failed to connect to wifi: {e:?}");
+                Timer::after(Duration::from_millis(5000)).await
+            }
+        }
+    }
+}
+
+#[embassy_executor::task]
+async fn net_task(mut runner: Runner<'static, WifiDevice<'static>>) {
+    runner.run().await
+}

mqtt_display/src/bin/main.rs

@@ -6,31 +6,50 @@
     clippy::mem_forget,
     reason = "mem::forget is generally not safe to do with esp_hal types"
 )]
+// TODO WARNING core 1 should be logic, core 0 wifi, its flipped now
 
 use embassy_executor::Spawner;
-use embassy_futures::select::{select, Either};
+use embassy_futures::select::{select3, Either3};
 use embassy_net::{Runner, StackResources};
+use embassy_sync::signal::Signal;
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
 use embassy_time::{Duration, Timer};
+use projekt_final::bus::I2cInner;
+
 use esp_alloc as _;
 use esp_backtrace as _;
-use esp_hal::{clock::CpuClock, rng::Rng, timer::timg::TimerGroup};
+
+use esp_hal::{
+    clock::CpuClock,
+    i2c::master::{Config as I2cConfig, I2c},
+    rng::Rng,
+    system::{CpuControl, Stack},
+    timer::timg::TimerGroup,
+};
 use esp_wifi::{
-    init,
     wifi::{ClientConfiguration, Configuration, WifiController, WifiDevice, WifiEvent, WifiState},
     EspWifiController,
 };
+
 use log::info;
 use rust_mqtt::packet::v5::publish_packet::QualityOfService;
-use projekt_final::mqtt::client::{
-    mqtt_events, mqtt_publish, mqtt_subscribe, mqtt_task, IncomingMsg,
+use static_cell::StaticCell;
+use core::cell::RefCell;
+
+use projekt_final::{
+    bus,
+    display,
+    mpu,
+    mqtt::client::{mqtt_events, mqtt_publish, mqtt_subscribe, mqtt_task, IncomingMsg},
 };
-use projekt_final::i2c::com::i2c_check;
-use projekt_final::i2c::com::display_task;
-use defmt_rtt as _;
 
 extern crate alloc;
+use alloc::format;
 
-esp_bootloader_esp_idf::esp_app_desc!();
+static I2C_BUS: StaticCell<RefCell<I2cInner>> = StaticCell::new();
+static APP_CORE_STACK: StaticCell<Stack<8192>> = StaticCell::new();
+static EXECUTOR_CORE1: StaticCell<esp_hal_embassy::Executor> = StaticCell::new();
+static NETWORK_READY: Signal<CriticalSectionRawMutex, ()> = Signal::new();
 
 macro_rules! mk_static {
     ($t:ty,$val:expr) => {{
@@ -43,55 +62,136 @@ macro_rules! mk_static {
 
 const SSID: &str = env!("SSID");
 const PASSWORD: &str = env!("PASSWORD");
+const MQTT_PUBLISH_DIVIDER: u32 = 10;
+
+esp_bootloader_esp_idf::esp_app_desc!();
 
 #[esp_hal_embassy::main]
 async fn main(spawner: Spawner) -> ! {
     esp_println::logger::init_logger_from_env();
+    info!("===============================");
+    info!("  ESP32 IoT Firmware Starting");
+    info!("===============================");
+
     let config = esp_hal::Config::default().with_cpu_clock(CpuClock::max());
     let peripherals = esp_hal::init(config);
-
     esp_alloc::heap_allocator!(size: 72 * 1024);
 
+    info!("Initializing I2C bus...");
+    let i2c = I2c::new(peripherals.I2C0, I2cConfig::default())
+        .expect("Failed to create I2C instance")
+        .with_sda(peripherals.GPIO21)
+        .with_scl(peripherals.GPIO22)
+        .into_async();
+
+    let i2c_bus = I2C_BUS.init(RefCell::new(i2c));
+    let display_i2c = bus::new_device(i2c_bus);
+    let mpu_i2c = bus::new_device(i2c_bus);
+
+    info!("Initializing WiFi...");
     let timg0 = TimerGroup::new(peripherals.TIMG0);
     let mut rng = Rng::new(peripherals.RNG);
 
-    let esp_wifi_ctrl = &*mk_static!(
+    let esp_wifi_ctrl = mk_static!(
         EspWifiController<'static>,
-        init(timg0.timer0, rng.clone()).unwrap()
+        esp_wifi::init(timg0.timer0, rng.clone()).unwrap()
     );
 
     let (controller, interfaces) =
-        esp_wifi::wifi::new(&esp_wifi_ctrl, peripherals.WIFI).unwrap();
+        esp_wifi::wifi::new(esp_wifi_ctrl, peripherals.WIFI).unwrap();
 
     let wifi_interface = interfaces.sta;
-
     let timg1 = TimerGroup::new(peripherals.TIMG1);
     esp_hal_embassy::init(timg1.timer0);
 
-    let config = embassy_net::Config::dhcpv4(Default::default());
-
     let seed = (rng.random() as u64) << 32 | rng.random() as u64;
 
-    // Init network stack
+    // Start core 1 for WiFi and MQTT (network stack created there)
+    let mut cpu_control = CpuControl::new(peripherals.CPU_CTRL);
+    let _guard = cpu_control.start_app_core(
+        APP_CORE_STACK.init(Stack::new()),
+        move || {
+            let executor = EXECUTOR_CORE1.init(esp_hal_embassy::Executor::new());
+            executor.run(|spawner| {
+                spawner.spawn(core1_network_task(spawner, controller, wifi_interface, seed)).ok();
+            });
+        }
+    ).unwrap();
+
+    // Wait for network to be ready (signaled from core 1)
+    NETWORK_READY.wait().await;
+    info!("Network ready, starting core 0 tasks");
+
+    // Core 0: display and MPU tasks
+    spawner.spawn(display::task::display_task(display_i2c)).expect("spawn display_task");
+    spawner.spawn(mpu::task::mpu_task(mpu_i2c)).expect("spawn mpu_task");
+
+    display::api::set_status("Booting...").await;
+    mqtt_subscribe("esp32/topic").await;
+    mqtt_publish("esp32/topic", b"online", QualityOfService::QoS1, false).await;
+
+    display::api::set_status("Running").await;
+    display::api::set_mqtt_status(true, 0).await;
+
+    let mqtt_rx = mqtt_events();
+    let imu_rx = mpu::api::events();
+    let mut imu_reading_count: u32 = 0;
+    let mut mqtt_msg_count: u32 = 0;
+
+    loop {
+        match select3(
+            mqtt_rx.receive(),
+            imu_rx.receive(),
+            Timer::after(Duration::from_secs(30)),
+        ).await {
+            Either3::First(msg) => {
+                mqtt_msg_count += 1;
+                handle_mqtt_message(msg).await;
+                display::api::set_mqtt_status(true, mqtt_msg_count).await;
+            }
+            Either3::Second(reading) => {
+                imu_reading_count += 1;
+                display::api::show_imu(reading).await;
+                if imu_reading_count % MQTT_PUBLISH_DIVIDER == 0 {
+                    let payload = format!(
+                        "{{\"ax\":{:.2},\"ay\":{:.2},\"az\":{:.2},\"t\":{:.1}}}",
+                        reading.accel_g[0], reading.accel_g[1], reading.accel_g[2], reading.temp_c
+                    );
+                    mqtt_publish("esp32/imu", payload.as_bytes(), QualityOfService::QoS0, false).await;
+                }
+            }
+            Either3::Third(_) => {
+                info!("Heartbeat: {} IMU readings", imu_reading_count);
+            }
+        }
+    }
+}
+
+// Runs on core 1 - creates and owns the network stack
+#[embassy_executor::task]
+async fn core1_network_task(
+    spawner: Spawner,
+    controller: WifiController<'static>,
+    wifi_interface: WifiDevice<'static>,
+    seed: u64,
+) {
+    spawner.spawn(connection_task(controller)).ok();
+
+    let net_config = embassy_net::Config::dhcpv4(Default::default());
     let (stack, runner) = embassy_net::new(
         wifi_interface,
-        config,
+        net_config,
         mk_static!(StackResources<3>, StackResources::<3>::new()),
         seed,
     );
 
-    spawner.spawn(connection(controller)).ok();
     spawner.spawn(net_task(runner)).ok();
 
-    // Wait for link up
+    // Wait for network
     loop {
-        if stack.is_link_up() {
-            break;
-        }
+        if stack.is_link_up() { break; }
         Timer::after(Duration::from_millis(500)).await;
     }
-
-    info!("Waiting to get IP address...");
     loop {
         if let Some(config) = stack.config_v4() {
             info!("Got IP: {}", config.address);
@@ -100,62 +200,29 @@ async fn main(spawner: Spawner) -> ! {
         Timer::after(Duration::from_millis(500)).await;
     }
 
-    spawner.spawn(mqtt_task(stack)).expect("failed to spawn MQTT task");
-    info!("MQTT task started");
+    // Signal core 0 that network is ready
+    NETWORK_READY.signal(());
 
-    spawner.spawn(display_task()).expect("failed to spawn Display task");
-    info!("I2C scan task started");
+    // Start MQTT on this core (it needs the stack)
+    spawner.spawn(mqtt_task(stack)).ok();
+}
 
-    mqtt_publish("esp32/topic", b"hello from ESP32 (init)", QualityOfService::QoS1, false).await;
-    info!("Sent initial MQTT message");
-
-    mqtt_subscribe("esp32/topic").await;
-
-    // Get a receiver for incoming MQTT messages
-    let mqtt_rx = mqtt_events();
-
-    loop {
-        // Drive both: either process an MQTT message or publish periodically
-        match select(mqtt_rx.receive(), Timer::after(Duration::from_secs(5))).await
-        {
-            // Received inbound MQTT message (from broker)
-            Either::First(msg) => {
-                handle_incoming(msg);
-            }
-            // Time-based example publish
-            Either::Second(_) => {
-                // mqtt_publish(
-                //     "esp32/topic",
-                //     b"hello from main",
-                //     QualityOfService::QoS1,
-                //     false,
-                // )
-                // .await;
-            }
+async fn handle_mqtt_message(msg: IncomingMsg) {
+    if let Ok(txt) = core::str::from_utf8(&msg.payload) {
+        match txt {
+            "clear" => { display::api::clear().await; }
+            "status" => { mqtt_publish("esp32/status", b"running", QualityOfService::QoS1, false).await; }
+            _ => {}
         }
     }
 }
 
-fn handle_incoming(msg: IncomingMsg) {
-    if let Ok(txt) = core::str::from_utf8(&msg.payload) {
-        info!("MAIN RX [{}]: {}", msg.topic.as_str(), txt);
-        info!("Received MQTT message -> topic: '{}', payload: '{}'", msg.topic.as_str(), txt);
-    } else {
-        info!("MAIN RX [{}]: {:?}", msg.topic.as_str(), msg.payload);
-    }
-}
-
 #[embassy_executor::task]
-async fn connection(mut controller: WifiController<'static>) {
-    info!("start connection task");
-    info!("Device capabilities: {:?}", controller.capabilities());
+async fn connection_task(mut controller: WifiController<'static>) {
     loop {
-        match esp_wifi::wifi::wifi_state() {
-            WifiState::StaConnected => {
-                controller.wait_for_event(WifiEvent::StaDisconnected).await;
-                Timer::after(Duration::from_millis(5000)).await
-            }
-            _ => {}
+        if esp_wifi::wifi::wifi_state() == WifiState::StaConnected {
+            controller.wait_for_event(WifiEvent::StaDisconnected).await;
+            Timer::after(Duration::from_millis(5000)).await;
         }
         if !matches!(controller.is_started(), Ok(true)) {
             let client_config = Configuration::Client(ClientConfiguration {
@@ -164,16 +231,13 @@ async fn connection(mut controller: WifiController<'static>) {
                 ..Default::default()
             });
             controller.set_configuration(&client_config).unwrap();
-            info!("Starting wifi");
+            info!("Wi-Fi starting...");
             controller.start_async().await.unwrap();
-            info!("Wifi started!");
         }
-        info!("About to connect...");
-
         match controller.connect_async().await {
             Ok(_) => info!("Wifi connected!"),
             Err(e) => {
-                info!("Failed to connect to wifi: {e:?}");
+                info!("Failed to connect to wifi: {e:#?}");
                 Timer::after(Duration::from_millis(5000)).await
             }
         }

mqtt_display/src/bus/mod.rs

@@ -0,0 +1,22 @@
+// src/bus/mod.rs
+
+use core::cell::RefCell;
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+use embassy_sync::mutex::Mutex;
+use embedded_hal_bus::i2c::RefCellDevice;
+use esp_hal::i2c::master::I2c;
+use esp_hal::Async;
+
+/// The underlying I2C peripheral type
+pub type I2cInner = I2c<'static, Async>;
+
+/// RefCell to share the bus on a single core.
+pub type SharedI2c = RefCell<I2cInner>;
+
+/// A handle to a shared I2C device.
+pub type I2cDevice = RefCellDevice<'static, I2cInner>;
+
+/// New I2C device handle from the shared bus.
+pub fn new_device(bus: &'static SharedI2c) -> I2cDevice {
+    RefCellDevice::new(bus)
+}

mqtt_display/src/contracts.rs

@@ -0,0 +1,35 @@
+// src/contracts.rs
+//! Cross-feature message contracts.
+//! 
+//! This is the ONLY coupling point between features.
+//! Features depend on these types, not on each other.
+
+use heapless::String as HString;
+
+/// IMU sensor reading from MPU6050
+#[derive(Clone, Copy, Default, Debug)]
+pub struct ImuReading {
+    /// Acceleration in g (earth gravity units)
+    pub accel_g: [f32; 3],
+    /// Angular velocity in degrees per second
+    pub gyro_dps: [f32; 3],
+    /// Temperature in Celsius
+    pub temp_c: f32,
+    /// Timestamp in milliseconds since boot
+    pub timestamp_ms: u64,
+}
+
+/// Commands that can be sent to the display actor
+#[derive(Clone, Debug)]
+pub enum DisplayCommand {
+    /// Show IMU sensor data
+    SetImu(ImuReading),
+    /// Show a status line (max 32 chars)
+    SetStatus(HString<32>),
+    /// Show an error message (max 64 chars)
+    ShowError(HString<64>),
+    /// Show MQTT connection status
+    SetMqttStatus { connected: bool, msg_count: u32 },
+    /// Clear the display to default state
+    Clear,
+}

mqtt_display/src/display/api.rs

@@ -0,0 +1,81 @@
+// src/display/api.rs
+//! Public API for the display feature.
+//!
+//! Other parts of the system use this module to send commands to the display.
+//! The actual rendering happens in `task.rs`.
+
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+use embassy_sync::channel::{Channel, Receiver, TrySendError};
+use heapless::String;
+
+use crate::contracts::{DisplayCommand, ImuReading};
+
+/// Queue size for display commands.
+/// Moderate size to handle bursts without dropping.
+const QUEUE_SIZE: usize = 8;
+
+/// Channel for sending commands to the display task.
+pub(crate) static DISPLAY_CHANNEL: Channel<CriticalSectionRawMutex, DisplayCommand, QUEUE_SIZE> =
+    Channel::new();
+
+/// Send a command to the display.
+///
+/// This is async and will wait if the queue is full.
+/// For fire-and-forget, use `try_send`.
+///
+/// # Example
+/// ```ignore
+/// display::api::send(DisplayCommand::SetStatus("Hello".try_into().unwrap())).await;
+/// ```
+pub async fn send(cmd: DisplayCommand) {
+    DISPLAY_CHANNEL.send(cmd).await;
+}
+
+/// Try to send a command without waiting.
+///
+/// Returns `Err(cmd)` if the queue is full.
+pub fn try_send(cmd: DisplayCommand) -> Result<(), DisplayCommand> {
+    DISPLAY_CHANNEL.try_send(cmd).map_err(|e| match e {
+        TrySendError::Full(command) => command,
+    })
+}
+
+/// Get a receiver for display commands (internal use).
+///
+/// Used by the display task to receive commands.
+pub(crate) fn receiver() -> Receiver<'static, CriticalSectionRawMutex, DisplayCommand, QUEUE_SIZE> {
+    DISPLAY_CHANNEL.receiver()
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// Convenience functions for common commands
+// ─────────────────────────────────────────────────────────────────────────────
+
+/// Send IMU data to the display.
+pub async fn show_imu(reading: ImuReading) {
+    send(DisplayCommand::SetImu(reading)).await;
+}
+
+/// Set the status line.
+pub async fn set_status(text: &str) {
+    let mut s = String::<32>::new();
+    let _ = s.push_str(&text[..text.len().min(32)]);
+    send(DisplayCommand::SetStatus(s)).await;
+}
+
+/// Show an error message.
+pub async fn show_error(text: &str) {
+    let mut s = String::<64>::new();
+    let _ = s.push_str(&text[..text.len().min(64)]);
+    send(DisplayCommand::ShowError(s)).await;
+}
+
+/// Update MQTT status indicator.
+pub async fn set_mqtt_status(connected: bool, msg_count: u32) {
+    send(DisplayCommand::SetMqttStatus { connected, msg_count }).await;
+}
+
+/// Clear the display.
+pub async fn clear() {
+    send(DisplayCommand::Clear).await;
+}

mqtt_display/src/display/mod.rs

@@ -0,0 +1,5 @@
+// src/display/mod.rs
+//! SSD1306 OLED display
+
+pub mod api;
+pub mod task;

mqtt_display/src/display/task.rs

@@ -0,0 +1,165 @@
+// src/display/task.rs
+//! SSD1306 display rendering task optimized for 0.91" 128x32 OLED.
+
+use embassy_time::{Duration, Timer};
+use log::{error, info};
+
+use alloc::boxed::Box;
+use alloc::format;
+use heapless::String;
+use mousefood::{EmbeddedBackend, EmbeddedBackendConfig};
+use ratatui::{
+    layout::{Constraint, Direction, Layout},
+    style::{Style, Stylize},
+    widgets::Paragraph,
+    Terminal,
+};
+use ssd1306::{
+    mode::BufferedGraphicsMode, prelude::*, I2CDisplayInterface, Ssd1306,
+};
+
+use crate::bus::I2cDevice;
+use crate::contracts::{DisplayCommand, ImuReading};
+use crate::display::api::receiver;
+
+/// Display refresh interval in milliseconds.
+const REFRESH_INTERVAL_MS: u64 = 100;
+
+/// Internal state for what to render.
+struct DisplayState {
+    status: String<32>,
+    last_imu: Option<ImuReading>,
+    last_error: Option<String<64>>,
+    mqtt_connected: bool,
+    mqtt_msg_count: u32,
+}
+
+impl Default for DisplayState {
+    fn default() -> Self {
+        Self {
+            status: String::new(),
+            last_imu: None,
+            last_error: None,
+            mqtt_connected: false,
+            mqtt_msg_count: 0,
+        }
+    }
+}
+
+impl DisplayState {
+    fn apply_command(&mut self, cmd: DisplayCommand) {
+        match cmd {
+            DisplayCommand::SetImu(reading) => {
+                self.last_imu = Some(reading);
+                self.last_error = None;
+            }
+            DisplayCommand::SetStatus(s) => {
+                self.status = s;
+            }
+            DisplayCommand::ShowError(e) => {
+                self.last_error = Some(e);
+            }
+            DisplayCommand::SetMqttStatus { connected, msg_count } => {
+                self.mqtt_connected = connected;
+                self.mqtt_msg_count = msg_count;
+            }
+            DisplayCommand::Clear => {
+                self.last_imu = None;
+                self.last_error = None;
+                self.status = String::new();
+            }
+        }
+    }
+}
+
+/// The display rendering task.
+/// Designed for 0.91" 128x32 slim OLED screen.
+#[embassy_executor::task]
+pub async fn display_task(i2c: I2cDevice) {
+    info!("Display task starting...");
+
+    // Initialize SSD1306 display for 128x32 variant
+    let interface = I2CDisplayInterface::new(i2c);
+    let mut display = Ssd1306::new(interface, DisplaySize128x32, DisplayRotation::Rotate0)
+        .into_buffered_graphics_mode();
+
+    if let Err(e) = display.init() {
+        error!("Display init failed: {:?}", e);
+        loop {
+            Timer::after(Duration::from_secs(60)).await;
+        }
+    }
+
+    info!("SSD1306 display initialized (128x32)");
+
+    // Configure mousefood backend for ratatui
+    let config = EmbeddedBackendConfig {
+        flush_callback: Box::new(
+            |d: &mut Ssd1306<_, _, BufferedGraphicsMode<DisplaySize128x32>>| {
+                let _ = d.flush();
+            },
+        ),
+        ..Default::default()
+    };
+
+    let backend = EmbeddedBackend::new(&mut display, config);
+    let mut terminal = Terminal::new(backend).expect("terminal init failed");
+
+    let mut state = DisplayState::default();
+    let rx = receiver();
+
+    info!("Display task entering render loop");
+
+    loop {
+        // Process all pending commands (non-blocking)
+        while let Ok(cmd) = rx.try_receive() {
+            state.apply_command(cmd);
+        }
+
+        // Render current state
+        render_frame(&mut terminal, &state);
+
+        // Wait before next refresh
+        Timer::after(Duration::from_millis(REFRESH_INTERVAL_MS)).await;
+    }
+}
+
+/// Render a single frame compactly (for 128x32 OLED)
+fn render_frame<B: ratatui::backend::Backend>(terminal: &mut Terminal<B>, state: &DisplayState) {
+    let _ = terminal.draw(|f| {
+        let chunks = Layout::default()
+            .direction(Direction::Vertical)
+            .constraints([
+                Constraint::Length(1),
+                Constraint::Min(0),
+            ])
+            .split(f.area());
+
+        // Header: condensed status + MQTT indicator
+        let mqtt_indicator = if state.mqtt_connected { "M" } else { "m" };
+        let header_title = format!(
+            "[{}] {} #{}",
+            mqtt_indicator,
+            state.status.as_str(),
+            state.mqtt_msg_count
+        );
+
+        f.render_widget(Paragraph::new(header_title).style(Style::default().reversed()), chunks[0]);
+
+        // Body: minimal content (no borders, short text)
+        let body_content = if let Some(ref err) = state.last_error {
+            format!("ERR: {}", err.as_str())
+        } else if let Some(ref imu) = state.last_imu {
+            format!(
+                "A:{:.1} {:.1} {:.1}\nG:{:.0} {:.0} {:.0}\nT:{:.1}C",
+                imu.accel_g[0], imu.accel_g[1], imu.accel_g[2],
+                imu.gyro_dps[0], imu.gyro_dps[1], imu.gyro_dps[2],
+                imu.temp_c
+            )
+        } else {
+            format!("Waiting for data...")
+        };
+
+        f.render_widget(Paragraph::new(body_content), chunks[1]);
+    });
+}

mqtt_display/src/i2c/com.rs

@@ -1,6 +1,5 @@
 // src/i2c/com.rs
 
-use embassy_executor::task;
 use embassy_time::{Duration, Timer};
 use esp_hal::{
     i2c::master::{Config, I2c},
@@ -9,7 +8,7 @@ use esp_hal::{
 use ssd1306::mode::BufferedGraphicsMode;
 use log::info;
 
-#[task]
+#[embassy_executor::task]
 pub async fn display_task() {
     use mousefood::{EmbeddedBackend, EmbeddedBackendConfig};
     use ratatui::{
@@ -73,7 +72,7 @@ pub async fn display_task() {
     }
 }
 
-#[task]
+#[embassy_executor::task]
 pub async fn i2c_check() {
     let peripherals = unsafe { Peripherals::steal() };
     let mut i2c = I2c::new(peripherals.I2C0, Config::default())

mqtt_display/src/lib.rs

@@ -2,4 +2,7 @@
 extern crate alloc;
 
 pub mod mqtt;
-pub mod i2c;
+pub mod bus;
+pub mod contracts;
+pub mod display;
+pub mod mpu;

mqtt_display/src/mpu/api.rs

@@ -0,0 +1,17 @@
+// src/mpu/api.rs
+//! Public API for the MPU6050 feature.
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+use embassy_sync::channel::{Channel, Receiver, Sender};
+use crate::contracts::ImuReading;
+
+const QUEUE_SIZE: usize = 4;
+
+pub(crate) static IMU_CHANNEL: Channel<CriticalSectionRawMutex, ImuReading, QUEUE_SIZE> = Channel::new();
+
+pub fn events() -> Receiver<'static, CriticalSectionRawMutex, ImuReading, QUEUE_SIZE> {
+    IMU_CHANNEL.receiver()
+}
+
+pub(crate) fn sender() -> Sender<'static, CriticalSectionRawMutex, ImuReading, QUEUE_SIZE> {
+    IMU_CHANNEL.sender()
+}

mqtt_display/src/mpu/driver.rs

@@ -0,0 +1,241 @@
+// src/mpu/driver.rs
+//! MPU6050 driver using raw register I/O.
+
+use embedded_hal::i2c::I2c;
+
+/// MPU6050 register addresses
+mod reg {
+    pub const PWR_MGMT_1: u8 = 0x6B;
+    pub const CONFIG: u8 = 0x1A;
+    pub const GYRO_CONFIG: u8 = 0x1B;
+    pub const ACCEL_CONFIG: u8 = 0x1C;
+    pub const ACCEL_XOUT_H: u8 = 0x3B; // Start of 14-byte burst read
+    pub const WHO_AM_I: u8 = 0x75;
+}
+
+/// Expected WHO_AM_I value for MPU6050
+const MPU6050_WHO_AM_I: u8 = 0x68;
+
+/// MPU6050 accelerometer full-scale range
+#[derive(Clone, Copy, Debug, Default)]
+pub enum AccelRange {
+    #[default]
+    G2 = 0,  // ±2g  -> 16384 LSB/g
+    G4 = 1,  // ±4g  -> 8192 LSB/g
+    G8 = 2,  // ±8g  -> 4096 LSB/g
+    G16 = 3, // ±16g -> 2048 LSB/g
+}
+
+impl AccelRange {
+    /// LSB per g for this range
+    pub fn sensitivity(self) -> f32 {
+        match self {
+            AccelRange::G2 => 16384.0,
+            AccelRange::G4 => 8192.0,
+            AccelRange::G8 => 4096.0,
+            AccelRange::G16 => 2048.0,
+        }
+    }
+}
+
+/// MPU6050 gyroscope full-scale range
+#[derive(Clone, Copy, Debug, Default)]
+pub enum GyroRange {
+    #[default]
+    Dps250 = 0,  // ±250°/s  -> 131 LSB/°/s
+    Dps500 = 1,  // ±500°/s  -> 65.5 LSB/°/s
+    Dps1000 = 2, // ±1000°/s -> 32.8 LSB/°/s
+    Dps2000 = 3, // ±2000°/s -> 16.4 LSB/°/s
+}
+
+impl GyroRange {
+    /// LSB per degree/second for this range
+    pub fn sensitivity(self) -> f32 {
+        match self {
+            GyroRange::Dps250 => 131.0,
+            GyroRange::Dps500 => 65.5,
+            GyroRange::Dps1000 => 32.8,
+            GyroRange::Dps2000 => 16.4,
+        }
+    }
+}
+
+/// Raw sensor data from a single read
+#[derive(Clone, Copy, Debug, Default)]
+pub struct RawReading {
+    pub accel: [i16; 3], // X, Y, Z
+    pub gyro: [i16; 3],  // X, Y, Z
+    pub temp: i16,
+}
+
+/// MPU6050 driver
+pub struct Mpu6050<I> {
+    i2c: I,
+    addr: u8,
+    accel_range: AccelRange,
+    gyro_range: GyroRange,
+}
+
+impl<I> Mpu6050<I> {
+    /// Create a new MPU6050 driver.
+    ///
+    /// Default I2C address is 0x68. Use 0x69 if AD0 pin is high.
+    pub fn new(i2c: I, addr: u8) -> Self {
+        Self {
+            i2c,
+            addr,
+            accel_range: AccelRange::default(),
+            gyro_range: GyroRange::default(),
+        }
+    }
+
+    /// Create with default address 0x68
+    pub fn new_default(i2c: I) -> Self {
+        Self::new(i2c, 0x68)
+    }
+
+    /// Release the I2C peripheral
+    pub fn release(self) -> I {
+        self.i2c
+    }
+
+    /// Get current accelerometer range
+    pub fn accel_range(&self) -> AccelRange {
+        self.accel_range
+    }
+
+    /// Get current gyroscope range
+    pub fn gyro_range(&self) -> GyroRange {
+        self.gyro_range
+    }
+}
+
+impl<I, E> Mpu6050<I>
+where
+    I: I2c<Error = E>,
+{
+    /// Initialize the sensor with default configuration.
+    ///
+    /// - Wakes up the device (exits sleep mode)
+    /// - Sets DLPF to ~44Hz bandwidth
+    /// - Sets accel range to ±2g
+    /// - Sets gyro range to ±250°/s
+    ///
+    /// Call this once after creating the driver.
+    /// Consider adding a ~50ms delay after init before first read.
+    pub fn init(&mut self) -> Result<(), E> {
+        // Wake up (clear sleep bit in PWR_MGMT_1)
+        self.write_reg(reg::PWR_MGMT_1, 0x00)?;
+
+        // Set DLPF (Digital Low Pass Filter) to ~44Hz
+        // CONFIG register, bits 2:0 = DLPF_CFG
+        self.write_reg(reg::CONFIG, 0x03)?;
+
+        // Set accelerometer range (default ±2g)
+        self.write_reg(reg::ACCEL_CONFIG, (self.accel_range as u8) << 3)?;
+
+        // Set gyroscope range (default ±250°/s)
+        self.write_reg(reg::GYRO_CONFIG, (self.gyro_range as u8) << 3)?;
+
+        Ok(())
+    }
+
+    /// Verify the device is responding and is an MPU6050.
+    ///
+    /// Returns true if WHO_AM_I register returns expected value.
+    pub fn verify(&mut self) -> Result<bool, E> {
+        let who = self.read_reg(reg::WHO_AM_I)?;
+        Ok(who == MPU6050_WHO_AM_I)
+    }
+
+    /// Set accelerometer full-scale range.
+    ///
+    /// Takes effect immediately.
+    pub fn set_accel_range(&mut self, range: AccelRange) -> Result<(), E> {
+        self.accel_range = range;
+        self.write_reg(reg::ACCEL_CONFIG, (range as u8) << 3)
+    }
+
+    /// Set gyroscope full-scale range.
+    ///
+    /// Takes effect immediately.
+    pub fn set_gyro_range(&mut self, range: GyroRange) -> Result<(), E> {
+        self.gyro_range = range;
+        self.write_reg(reg::GYRO_CONFIG, (range as u8) << 3)
+    }
+
+    /// Read all sensor data in a single burst.
+    ///
+    /// This reads 14 bytes starting at ACCEL_XOUT_H:
+    /// - Accelerometer X, Y, Z (6 bytes)
+    /// - Temperature (2 bytes)
+    /// - Gyroscope X, Y, Z (6 bytes)
+    ///
+    /// Returns raw values; use `convert_reading` for physical units.
+    pub fn read_raw(&mut self) -> Result<RawReading, E> {
+        let mut buf = [0u8; 14];
+        self.i2c.write_read(self.addr, &[reg::ACCEL_XOUT_H], &mut buf)?;
+
+        Ok(RawReading {
+            accel: [
+                i16::from_be_bytes([buf[0], buf[1]]),
+                i16::from_be_bytes([buf[2], buf[3]]),
+                i16::from_be_bytes([buf[4], buf[5]]),
+            ],
+            temp: i16::from_be_bytes([buf[6], buf[7]]),
+            gyro: [
+                i16::from_be_bytes([buf[8], buf[9]]),
+                i16::from_be_bytes([buf[10], buf[11]]),
+                i16::from_be_bytes([buf[12], buf[13]]),
+            ],
+        })
+    }
+
+    /// Convert raw reading to physical units.
+    ///
+    /// Returns (accel_g, gyro_dps, temp_c)
+    pub fn convert_reading(&self, raw: &RawReading) -> ([f32; 3], [f32; 3], f32) {
+        let accel_sens = self.accel_range.sensitivity();
+        let gyro_sens = self.gyro_range.sensitivity();
+
+        let accel_g = [
+            raw.accel[0] as f32 / accel_sens,
+            raw.accel[1] as f32 / accel_sens,
+            raw.accel[2] as f32 / accel_sens,
+        ];
+
+        let gyro_dps = [
+            raw.gyro[0] as f32 / gyro_sens,
+            raw.gyro[1] as f32 / gyro_sens,
+            raw.gyro[2] as f32 / gyro_sens,
+        ];
+
+        // Temperature formula from datasheet:
+        // Temp in °C = (TEMP_OUT / 340.0) + 36.53
+        let temp_c = (raw.temp as f32 / 340.0) + 36.53;
+
+        (accel_g, gyro_dps, temp_c)
+    }
+
+    /// Read and convert in one call.
+    ///
+    /// Convenience method that combines `read_raw` and `convert_reading`.
+    pub fn read(&mut self) -> Result<([f32; 3], [f32; 3], f32), E> {
+        let raw = self.read_raw()?;
+        Ok(self.convert_reading(&raw))
+    }
+
+    // ─────────────────────────────────────────────────────────────────
+    // Low-level register access
+    // ─────────────────────────────────────────────────────────────────
+
+    fn write_reg(&mut self, reg: u8, val: u8) -> Result<(), E> {
+        self.i2c.write(self.addr, &[reg, val])
+    }
+
+    fn read_reg(&mut self, reg: u8) -> Result<u8, E> {
+        let mut buf = [0u8];
+        self.i2c.write_read(self.addr, &[reg], &mut buf)?;
+        Ok(buf[0])
+    }
+}

mqtt_display/src/mpu/mod.rs

@@ -0,0 +1,6 @@
+// src/mpu/mod.rs
+//! MPU6050 accelerometer/gyroscope
+
+pub mod api;
+pub mod driver;
+pub mod task;

mqtt_display/src/mpu/task.rs

@@ -0,0 +1,132 @@
+// src/mpu/task.rs
+//! MPU6050 sampling task.
+//!
+//! This task:
+//! 1. Initializes the MPU6050 sensor
+//! 2. Continuously reads sensor data at a fixed rate
+//! 3. Publishes readings to the IMU channel
+
+use embassy_time::{Duration, Instant, Timer};
+use log::{error, info, warn};
+
+use crate::bus::I2cDevice;
+use crate::contracts::ImuReading;
+use crate::mpu::api::sender;
+use crate::mpu::driver::Mpu6050;
+
+/// Sampling interval in milliseconds.
+/// 50ms = 20Hz, reasonable for display updates.
+const SAMPLE_INTERVAL_MS: u64 = 50;
+
+/// MPU6050 I2C address (0x68 with AD0 low, 0x69 with AD0 high)
+const MPU_ADDR: u8 = 0x68;
+
+/// The MPU6050 sampling task.
+///
+/// # Arguments
+/// * `i2c` - An I2C device handle from the shared bus
+///
+/// # Panics
+/// Does not panic; logs errors and retries on failure.
+#[embassy_executor::task]
+pub async fn mpu_task(i2c: I2cDevice) {
+    info!("MPU task starting...");
+
+    let mut mpu = Mpu6050::new(i2c, MPU_ADDR);
+
+    // Initialize with retries
+    let mut init_attempts = 0;
+    loop {
+        init_attempts += 1;
+
+        // Verify device is present
+        match mpu.verify() {
+            Ok(true) => {
+                info!("MPU6050 detected at 0x{:02X}", MPU_ADDR);
+            }
+            Ok(false) => {
+                warn!(
+                    "Device at 0x{:02X} is not MPU6050 (attempt {})",
+                    MPU_ADDR, init_attempts
+                );
+                Timer::after(Duration::from_secs(2)).await;
+                continue;
+            }
+            Err(_e) => {
+                warn!(
+                    "I2C error verifying MPU6050 (attempt {})",
+                    init_attempts
+                );
+                Timer::after(Duration::from_secs(2)).await;
+                continue;
+            }
+        }
+
+        // Initialize sensor
+        match mpu.init() {
+            Ok(()) => {
+                info!("MPU6050 initialized successfully");
+                break;
+            }
+            Err(_e) => {
+                error!("MPU6050 init failed (attempt {})", init_attempts);
+                Timer::after(Duration::from_secs(2)).await;
+                continue;
+            }
+        }
+    }
+
+    // Allow sensor to stabilize after wake-up
+    Timer::after(Duration::from_millis(100)).await;
+
+    info!("MPU task entering sampling loop ({}ms interval)", SAMPLE_INTERVAL_MS);
+
+    let tx = sender();
+    let mut consecutive_errors = 0u32;
+
+    loop {
+        let start = Instant::now();
+
+        match mpu.read() {
+            Ok((accel_g, gyro_dps, temp_c)) => {
+                consecutive_errors = 0;
+
+                let reading = ImuReading {
+                    accel_g,
+                    gyro_dps,
+                    temp_c,
+                    timestamp_ms: start.as_millis(),
+                };
+
+                // Try to send; if queue is full, drop oldest by using try_send
+                // This ensures we never block the sampling loop
+                if tx.try_send(reading).is_err() {
+                    // Queue full - that's okay, main will get the next one
+                }
+            }
+            Err(_e) => {
+                consecutive_errors += 1;
+                if consecutive_errors == 1 || consecutive_errors % 10 == 0 {
+                    warn!("MPU read error (consecutive: {})", consecutive_errors);
+                }
+
+                // If too many errors, try to reinitialize
+                if consecutive_errors >= 50 {
+                    error!("Too many MPU errors, attempting reinit...");
+                    if mpu.init().is_ok() {
+                        info!("MPU reinit successful");
+                        consecutive_errors = 0;
+                        Timer::after(Duration::from_millis(100)).await;
+                    }
+                }
+            }
+        }
+
+        // Sleep for remainder of interval
+        let elapsed = start.elapsed();
+        let target = Duration::from_millis(SAMPLE_INTERVAL_MS);
+        if elapsed < target {
+            Timer::after(target - elapsed).await;
+        }
+    }
+}

mqtt_display/src/mqtt/client.rs

@@ -2,18 +2,17 @@
 use embassy_futures::select::{select, Either};
 use embassy_net::{tcp::TcpSocket, Stack};
 use embassy_time::{Duration, Timer};
-use log::info;
 use rust_mqtt::client::client::MqttClient;
 use rust_mqtt::client::client_config::{ClientConfig, MqttVersion};
 use rust_mqtt::packet::v5::publish_packet::QualityOfService;
 use rust_mqtt::packet::v5::reason_codes::ReasonCode;
 use rust_mqtt::utils::rng_generator::CountingRng;
-use static_cell::ConstStaticCell;
 
 use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
 use embassy_sync::channel::{Channel, Receiver};
-
-use heapless::{String as HString, Vec as HVec};
+use heapless::{String, Vec};
+use static_cell::ConstStaticCell;
+use log::info;
 
 use crate::mqtt::config::mqtt_broker_endpoint;
 
@@ -21,7 +20,7 @@ const RECONNECT_DELAY_SECS: u64 = 5;
 const KEEPALIVE_SECS: u64 = 60;
 const PING_PERIOD: Duration = Duration::from_secs(KEEPALIVE_SECS / 2);
 
-// Limits for small, static buffers (no heap)
+// Limits for static buffers
 pub const TOPIC_MAX: usize = 128;
 pub const PAYLOAD_MAX: usize = 512;
 const COMMAND_QUEUE: usize = 8;
@@ -31,7 +30,7 @@ const EVENT_QUEUE: usize = 8;
 static TCP_RX_BUFFER: ConstStaticCell<[u8; 2048]> = ConstStaticCell::new([0; 2048]);
 static TCP_TX_BUFFER: ConstStaticCell<[u8; 2048]> = ConstStaticCell::new([0; 2048]);
 
-// MQTT client buffers (separate from the TcpSocket's buffers)
+// MQTT client buffers (separate from the TcpSocket buffers)
 static MQTT_TX_BUF: ConstStaticCell<[u8; 1024]> = ConstStaticCell::new([0; 1024]);
 static MQTT_RX_BUF: ConstStaticCell<[u8; 1024]> = ConstStaticCell::new([0; 1024]);
 
@@ -40,14 +39,14 @@ type Client<'a, 'net> = MqttClient<'a, TcpSocket<'net>, 8, CountingRng>;
 
 #[derive(Clone)]
 pub struct IncomingMsg {
-    pub topic: HString<TOPIC_MAX>,
-    pub payload: HVec<u8, PAYLOAD_MAX>,
+    pub topic: String<TOPIC_MAX>,
+    pub payload: Vec<u8, PAYLOAD_MAX>,
 }
 
 #[derive(Clone)]
 struct PublishMsg {
-    topic: HString<TOPIC_MAX>,
-    payload: HVec<u8, PAYLOAD_MAX>,
+    topic: String<TOPIC_MAX>,
+    payload: Vec<u8, PAYLOAD_MAX>,
     qos: QualityOfService,
     retain: bool,
 }
@@ -55,7 +54,7 @@ struct PublishMsg {
 #[derive(Clone)]
 enum Command {
     Publish(PublishMsg),
-    Subscribe(HString<TOPIC_MAX>),
+    Subscribe(String<TOPIC_MAX>),
 }
 
 static CMD_CHAN: Channel<CriticalSectionRawMutex, Command, COMMAND_QUEUE> = Channel::new();
@@ -69,8 +68,7 @@ pub async fn mqtt_publish(topic: &str, payload: &[u8], qos: QualityOfService, re
             payload: truncate_payload(payload),
             qos,
             retain,
-        }))
-        .await;
+        })).await;
 }
 
 pub async fn mqtt_subscribe(topic: &str) {
@@ -83,73 +81,18 @@ pub fn mqtt_events(
 }
 
 // Helper functions for memory-safe truncation
-fn truncate_str<const N: usize>(s: &str) -> HString<N> {
-    let mut h = HString::new();
+fn truncate_str<const N: usize>(s: &str) -> String<N> {
+    let mut h = String::new();
     let _ = h.push_str(&s[..s.len().min(N)]);
     h
 }
 
-fn truncate_payload(data: &[u8]) -> HVec<u8, PAYLOAD_MAX> {
-    let mut v = HVec::new();
+fn truncate_payload(data: &[u8]) -> Vec<u8, PAYLOAD_MAX> {
+    let mut v = Vec::new();
     let _ = v.extend_from_slice(&data[..data.len().min(PAYLOAD_MAX)]);
     v
 }
 
-// MQTT configuration and client setup
-fn build_client_config() -> ClientConfig<'static, 8, CountingRng> {
-    let mut cfg = ClientConfig::new(MqttVersion::MQTTv5, CountingRng(0));
-    cfg.keep_alive = KEEPALIVE_SECS as u16;
-    cfg.add_client_id("esp32-client");
-    cfg
-}
-
-fn build_client<'a, 'net>(
-    socket: TcpSocket<'net>,
-    mqtt_tx: &'a mut [u8],
-    mqtt_rx: &'a mut [u8],
-) -> Client<'a, 'net> {
-    let mqtt_tx_len = mqtt_tx.len();
-    let mqtt_rx_len = mqtt_rx.len();
-    MqttClient::new(socket, mqtt_tx, mqtt_tx_len, mqtt_rx, mqtt_rx_len, build_client_config())
-}
-
-// Connection lifecycle and main session loop
-
-async fn connect_tcp<'net>(socket: &mut TcpSocket<'net>) -> Result<(), ()> {
-    match socket.connect(mqtt_broker_endpoint()).await {
-        Ok(_) => {
-            info!("Connected TCP to MQTT broker");
-            Ok(())
-        }
-        Err(e) => {
-            info!("TCP connect failed: {:?}", e);
-            Err(())
-        }
-    }
-}
-
-async fn connect_mqtt(client: &mut Client<'_, '_>) -> Result<(), ReasonCode> {
-    client.connect_to_broker().await
-}
-
-async fn run_loop(client: &mut Client<'_, '_>) -> Result<(), ReasonCode> {
-    let default_topic = "esp32/topic";
-    match client.subscribe_to_topic(default_topic).await {
-        Ok(_) => info!("Subscribed to '{}'", default_topic),
-        Err(e) => info!("Default subscribe failed: {:?}", e),
-    };
-
-    loop {
-        let net_or_ping = select(client.receive_message(), Timer::after(PING_PERIOD));
-
-        match select(CMD_CHAN.receive(), net_or_ping).await {
-            Either::First(cmd) => handle_command(client, cmd).await?,
-            Either::Second(Either::First(result)) => handle_incoming(result).await?,
-            Either::Second(Either::Second(_)) => client.send_ping().await?,
-        }
-    }
-}
-
 async fn handle_command(client: &mut Client<'_, '_>, cmd: Command) -> Result<(), ReasonCode> {
     match cmd {
         Command::Publish(msg) => {
@@ -171,8 +114,7 @@ async fn handle_incoming(result: Result<(&str, &[u8]), ReasonCode>) -> Result<()
         .send(IncomingMsg {
             topic: truncate_str::<TOPIC_MAX>(topic),
             payload: truncate_payload(payload),
-        })
-        .await;
+        }).await;
     Ok(())
 }
 
@@ -185,12 +127,21 @@ async fn run_one_session(
     mqtt_rx: &mut [u8],
 ) -> Result<(), ()> {
     let mut socket = TcpSocket::new(stack, tcp_rx, tcp_tx);
-    if connect_tcp(&mut socket).await.is_err() {
-        return Err(());
+    match socket.connect(mqtt_broker_endpoint()).await {
+        Ok(_) => info!("Connected TCP to MQTT broker"),
+        Err(e) => {
+            info!("TCP connect failed: {:#?}", e);
+            return Err(());
+        }
     }
 
-    let mut client = build_client(socket, mqtt_tx, mqtt_rx);
-    match connect_mqtt(&mut client).await {
+    // MQTT configuration and client setup
+    let mut cfg: ClientConfig<8, CountingRng> = ClientConfig::new(MqttVersion::MQTTv5, CountingRng(0));
+    cfg.keep_alive = KEEPALIVE_SECS as u16;
+    cfg.add_client_id("esp32-client");
+
+    let mut client = MqttClient::new(socket, mqtt_tx, mqtt_tx.len(), mqtt_rx, mqtt_rx.len(), cfg);
+    match client.connect_to_broker().await {
         Ok(_) => info!("MQTT CONNACK received"),
         Err(reason) => {
             info!("MQTT connect failed: {:?}", reason);
@@ -198,7 +149,16 @@ async fn run_one_session(
         }
     }
 
-    run_loop(&mut client).await.map_err(|_| ())
+    // Operational loop
+    loop {
+        let net_or_ping = select(client.receive_message(), Timer::after(PING_PERIOD));
+
+        match select(CMD_CHAN.receive(), net_or_ping).await {
+            Either::First(cmd) => handle_command(&mut client, cmd).await.map_err(|_| ())?,
+            Either::Second(Either::First(result)) => handle_incoming(result).await.map_err(|_| ())?,
+            Either::Second(Either::Second(_)) => client.send_ping().await.map_err(|_| ())?,
+        }
+    }
 }
 
 // Main MQTT embassy task
@@ -218,13 +178,9 @@ pub async fn mqtt_task(stack: Stack<'static>) {
             &mut tcp_tx[..],
             &mut mqtt_tx[..],
             &mut mqtt_rx[..],
-        )
-        .await;
+        ).await;
 
-        info!(
-            "Reconnecting in {}s after session end/failure",
-            RECONNECT_DELAY_SECS
-        );
+        info!("Reconnecting in {}s after session end/failure", RECONNECT_DELAY_SECS);
         Timer::after(Duration::from_secs(RECONNECT_DELAY_SECS)).await;
     }
 }