working now

mqtt_display/Cargo.lock

@@ -1073,9 +1073,9 @@ dependencies = [
 
 [[package]]
 name = "heapless"
-version = "0.9.1"
+version = "0.9.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b1edcd5a338e64688fbdcb7531a846cfd3476a54784dcb918a0844682bc7ada5"
+checksum = "2af2455f757db2b292a9b1768c4b70186d443bcb3b316252d6b540aec1cd89ed"
 dependencies = [
  "hash32",
  "stable_deref_trait",
@@ -1305,6 +1305,13 @@ dependencies = [
  "autocfg",
 ]
 
+[[package]]
+name = "pages-tui"
+version = "0.1.0"
+dependencies = [
+ "heapless 0.9.2",
+]
+
 [[package]]
 name = "paste"
 version = "1.0.15"
@@ -1452,9 +1459,10 @@ dependencies = [
  "esp-hal-embassy",
  "esp-println",
  "esp-wifi",
- "heapless 0.9.1",
+ "heapless 0.9.2",
  "log",
  "mousefood",
+ "pages-tui",
  "ratatui",
  "rust-mqtt",
  "smoltcp",

mqtt_display/Cargo.toml

@@ -9,6 +9,7 @@ name = "projekt_final"
 path = "./src/bin/main.rs"
 
 [dependencies]
+pages-tui = { path = "../../../pages-tui" }
 esp-bootloader-esp-idf = { version = "0.2.0", features = ["esp32"] }
 esp-hal = { version = "=1.0.0-rc.0", features = [
   "esp32",

mqtt_display/src/bin/main.rs

@@ -6,20 +6,27 @@
     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::{select3, Either3};
+use embassy_futures::select::{select, Either, select3, Either3};
 use embassy_net::{Runner, StackResources};
-use embassy_time::{Duration, Timer};
+use embassy_sync::signal::Signal;
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+use embassy_time::{Duration, Timer, Instant};
 use projekt_final::bus::I2cInner;
+use projekt_final::mqtt::client::mqtt_set_imu;
 
 use esp_alloc as _;
 use esp_backtrace as _;
 
 use esp_hal::{
+    gpio::InputConfig,
     clock::CpuClock,
+    gpio::{Input, Pull},
     i2c::master::{Config as I2cConfig, I2c},
     rng::Rng,
+    system::{CpuControl, Stack},
     timer::timg::TimerGroup,
 };
 use esp_wifi::{
@@ -27,6 +34,7 @@ use esp_wifi::{
     EspWifiController,
 };
 
+use pages_tui::input::Key;
 use log::info;
 use rust_mqtt::packet::v5::publish_packet::QualityOfService;
 use static_cell::StaticCell;
@@ -36,13 +44,16 @@ use projekt_final::{
     bus,
     display,
     mpu,
-    mqtt::client::{mqtt_events, mqtt_publish, mqtt_subscribe, mqtt_task, IncomingMsg},
+    mqtt::client::{mqtt_events, mqtt_try_publish, mqtt_publish, mqtt_subscribe, mqtt_task, IncomingMsg},
 };
 
 extern crate alloc;
 use alloc::format;
 
 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) => {{
@@ -97,27 +108,36 @@ async fn main(spawner: Spawner) -> ! {
     let timg1 = TimerGroup::new(peripherals.TIMG1);
     esp_hal_embassy::init(timg1.timer0);
 
-    let net_config = embassy_net::Config::dhcpv4(Default::default());
     let seed = (rng.random() as u64) << 32 | rng.random() as u64;
 
-    let (stack, runner) = embassy_net::new(
-        wifi_interface,
-        net_config,
-        mk_static!(StackResources<3>, StackResources::<3>::new()),
-        seed,
-    );
+    // 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();
 
-    spawner.spawn(connection_task(controller)).expect("spawn connection_task");
-    spawner.spawn(net_task(runner)).expect("spawn net_task");
-    wait_for_network(stack).await;
+    // Wait for network to be ready (signaled from core 1)
+    NETWORK_READY.wait().await;
+    info!("Network ready, starting core 0 tasks");
 
-    spawner.spawn(mqtt_task(stack)).expect("spawn mqtt_task");
+    let config = InputConfig::default().with_pull(Pull::Down);
+    let button_select = Input::new(peripherals.GPIO32, config);
+    let button_next = Input::new(peripherals.GPIO35, config);
+    spawner.spawn(button_detection_task(button_select, button_next)).unwrap();
+
+    // 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;
+    mqtt_subscribe("esp32/read").await;
+    mqtt_publish("esp32/imu", b"online", QualityOfService::QoS1, false).await;
 
     display::api::set_status("Running").await;
     display::api::set_mqtt_status(true, 0).await;
@@ -126,37 +146,75 @@ async fn main(spawner: Spawner) -> ! {
     let imu_rx = mpu::api::events();
     let mut imu_reading_count: u32 = 0;
     let mut mqtt_msg_count: u32 = 0;
+    let mut mqtt_publish_drops: u32 = 0;
+    let mut last_mqtt_publish = Instant::now();
+    let mqtt_publish_interval = Duration::from_secs(3);
 
     loop {
         match select3(
             mqtt_rx.receive(),
             imu_rx.receive(),
-            Timer::after(Duration::from_secs(30)),
+            Timer::after(Duration::from_secs(5)),
         ).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) => {
+            Either3::Second(mut reading) => {
+                // Drain zabezpečuje, že 'reading' je najčerstvejšia možná hodnota
+                let mut drained = 0;
+                while let Ok(next) = imu_rx.try_receive() {
+                    reading = next;
+                    drained += 1;
+                }
+                
                 imu_reading_count += 1;
-                display::api::show_imu(reading).await;
-                if imu_reading_count % MQTT_PUBLISH_DIVIDER == 0 {
+                display::api::show_imu(reading);
+
+                // 3. Nahraďte pôvodnú podmienku týmto časovým zámkom
+                if last_mqtt_publish.elapsed() >= mqtt_publish_interval {
                     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;
+                    mqtt_set_imu(payload.as_bytes());
+                    
+                    // Aktualizujeme čas posledného odoslania
+                    last_mqtt_publish = Instant::now();
+                    log::info!("MQTT IMU payload buffered (freshest data)");
                 }
             }
             Either3::Third(_) => {
-                info!("Heartbeat: {} IMU readings", imu_reading_count);
+                crate::mpu::api::IMU_CHANNEL.clear();
+                info!("IMU heartbeat: force-cleared queue, {} readings total, {} mqtt drops", 
+                      imu_reading_count, mqtt_publish_drops);
             }
         }
     }
 }
 
-async fn wait_for_network(stack: embassy_net::Stack<'static>) {
+// 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,
+        net_config,
+        mk_static!(StackResources<3>, StackResources::<3>::new()),
+        seed,
+    );
+
+    spawner.spawn(net_task(runner)).ok();
+
+    // Wait for network
     loop {
         if stack.is_link_up() { break; }
         Timer::after(Duration::from_millis(500)).await;
@@ -168,6 +226,11 @@ async fn wait_for_network(stack: embassy_net::Stack<'static>) {
         }
         Timer::after(Duration::from_millis(500)).await;
     }
+
+    // Signal core 0 that network is ready
+    NETWORK_READY.signal(());
+
+    spawner.spawn(mqtt_task(stack)).ok();
 }
 
 async fn handle_mqtt_message(msg: IncomingMsg) {
@@ -175,7 +238,7 @@ async fn handle_mqtt_message(msg: IncomingMsg) {
         match txt {
             "clear" => { display::api::clear().await; }
             "status" => { mqtt_publish("esp32/status", b"running", QualityOfService::QoS1, false).await; }
-            _ => {}
+            _ => { display::api::add_chat_message(txt).await; }
         }
     }
 }
@@ -211,3 +274,24 @@ async fn connection_task(mut controller: WifiController<'static>) {
 async fn net_task(mut runner: Runner<'static, WifiDevice<'static>>) {
     runner.run().await
 }
+
+#[embassy_executor::task]
+async fn button_detection_task(mut select_btn: Input<'static>, mut next_btn: Input<'static>) {
+    loop {
+        match select(
+            select_btn.wait_for_rising_edge(),
+            next_btn.wait_for_rising_edge(),
+        ).await {
+            Either::First(_) => {
+                info!("Detection: GPIO 32 (Select) triggered!");
+                display::api::push_key(Key::enter()).await;
+            }
+            Either::Second(_) => {
+                info!("Detection: GPIO 35 (Next) triggered!");
+                display::api::push_key(Key::tab()).await
+            }
+        }
+        // Debounce: prevent mechanical bouncing from double-triggering
+        embassy_time::Timer::after(embassy_time::Duration::from_millis(200)).await;
+    }
+}

mqtt_display/src/bus/mod.rs

@@ -1,23 +1,20 @@
 // 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 for esp-hal 1.0.0-rc.0
+/// The underlying I2C peripheral type
 pub type I2cInner = I2c<'static, Async>;
 
-/// We use RefCell to share the bus on a single core.
-/// It's zero-overhead and safe because Embassy tasks don't preempt each other.
+/// 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>;
 
-/// Create a new I2C device handle from the shared bus.
+/// New I2C device handle from the shared bus.
 pub fn new_device(bus: &'static SharedI2c) -> I2cDevice {
     RefCellDevice::new(bus)
 }

mqtt_display/src/contracts.rs

@@ -5,15 +5,13 @@
 //! Features depend on these types, not on each other.
 
 use heapless::String as HString;
+use pages_tui::input::Key;
 
 /// 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,
@@ -32,4 +30,7 @@ pub enum DisplayCommand {
     SetMqttStatus { connected: bool, msg_count: u32 },
     /// Clear the display to default state
     Clear,
+
+    PushKey(Key),
+    AddChatMessage(HString<24>),
 }

mqtt_display/src/display/api.rs

@@ -1,81 +1,66 @@
 // 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 as HString;
+use heapless::String;
 
 use crate::contracts::{DisplayCommand, ImuReading};
+use pages_tui::input::Key;
 
-/// 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;
+/// Show IMU data - NON-BLOCKING to prevent backpressure deadlock
+pub fn show_imu(reading: ImuReading) {
+    // CRITICAL FIX: Use try_send instead of send().await
+    // If display is slow, we drop the reading rather than blocking the main loop
+    let _ = try_send(DisplayCommand::SetImu(reading));
 }
 
-/// Set the status line.
 pub async fn set_status(text: &str) {
-    let mut s = HString::<32>::new();
+    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 = HString::<64>::new();
+    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;
 }
+
+pub async fn push_key(key: Key) {
+    send(DisplayCommand::PushKey(key)).await;
+}
+
+/// Add a chat message (for Chat page)
+pub async fn add_chat_message(msg: &str) {
+    let mut s = String::<24>::new();
+    let _ = s.push_str(&msg[..msg.len().min(24)]);
+    send(DisplayCommand::AddChatMessage(s)).await;
+}

mqtt_display/src/display/mod.rs

@@ -3,3 +3,4 @@
 
 pub mod api;
 pub mod task;
+pub mod tui;

mqtt_display/src/display/task.rs

@@ -1,104 +1,37 @@
 // 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 as HString;
 use mousefood::{EmbeddedBackend, EmbeddedBackendConfig};
-use ratatui::{
-    layout::{Constraint, Direction, Layout},
-    style::{Style, Stylize},
-    widgets::Paragraph,
-    Terminal,
-};
-use ssd1306::{
-    mode::BufferedGraphicsMode, prelude::*, I2CDisplayInterface, Ssd1306,
-};
+use ratatui::Terminal;
+use ssd1306::{mode::BufferedGraphicsMode, prelude::*, I2CDisplayInterface, Ssd1306};
 
 use crate::bus::I2cDevice;
-use crate::contracts::{DisplayCommand, ImuReading};
 use crate::display::api::receiver;
+use crate::display::tui::{render_frame, next_page_id, prev_page_id, DisplayState, Screen, ScreenEvent};
+use crate::contracts::DisplayCommand;
+use pages_tui::prelude::*;
 
-/// Display refresh interval in milliseconds.
 const REFRESH_INTERVAL_MS: u64 = 100;
 
-/// Internal state for what to render.
-struct DisplayState {
-    status: HString<32>,
-    last_imu: Option<ImuReading>,
-    last_error: Option<HString<64>>,
-    mqtt_connected: bool,
-    mqtt_msg_count: u32,
-}
-
-impl Default for DisplayState {
-    fn default() -> Self {
-        Self {
-            status: HString::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 = HString::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;
-        }
+        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>>| {
+        flush_callback: Box::new(|d: &mut Ssd1306<_, _, BufferedGraphicsMode<DisplaySize128x32>>| {
             let _ = d.flush();
-            },
-        ),
+        }),
         ..Default::default()
     };
 
@@ -106,60 +39,62 @@ pub async fn display_task(i2c: I2cDevice) {
     let mut terminal = Terminal::new(backend).expect("terminal init failed");
 
     let mut state = DisplayState::default();
+    let mut orchestrator = Orchestrator::<Screen>::new();
     let rx = receiver();
 
-    info!("Display task entering render loop");
+    // Register pages
+    orchestrator.register_page("menu".into(), Screen::Menu);
+    orchestrator.register_page("imu".into(), Screen::Imu);
+    orchestrator.register_page("chat".into(), Screen::Chat);
+
+    orchestrator.bind(Key::tab(), ComponentAction::Next);
+    orchestrator.bind(Key::enter(), ComponentAction::Select);
+
+    let _ = orchestrator.navigate_to("menu".into());
+
+    info!("Display ready");
 
     loop {
-        // Process all pending commands (non-blocking)
         while let Ok(cmd) = rx.try_receive() {
-            state.apply_command(cmd);
+            match cmd {
+                DisplayCommand::PushKey(key) => {
+                    if key == Key::tab() {
+                        orchestrator.focus_manager_mut().wrap_next();
+                    } else if key == Key::enter() {
+                        if let Ok(events) = orchestrator.process_frame(key) {
+                            for event in events {
+                                match event {
+                                    ScreenEvent::GoToImu => {
+                                        let _ = orchestrator.navigate_to("imu".into());
+                                    }
+                                    ScreenEvent::GoToChat => {
+                                        let _ = orchestrator.navigate_to("chat".into());
+                                    }
+                                    ScreenEvent::NavigatePrev => {
+                                        if let Some(cur) = orchestrator.current_id() {
+                                            let prev = prev_page_id(cur.as_str());
+                                            let _ = orchestrator.navigate_to(prev.into());
+                                        }
+                                    }
+                                    ScreenEvent::NavigateNext => {
+                                        if let Some(cur) = orchestrator.current_id() {
+                                            let next = next_page_id(cur.as_str());
+                                            let _ = orchestrator.navigate_to(next.into());
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+                _ => state.apply_command(cmd),
+            }
         }
 
-        // Render current state
-        render_frame(&mut terminal, &state);
+        if let Some(screen) = orchestrator.current() {
+            render_frame(&mut terminal, screen, orchestrator.focus_manager().current(), &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/display/tui.rs

@@ -0,0 +1,263 @@
+// src/display/tui.rs
+use crate::contracts::{DisplayCommand, ImuReading};
+use alloc::format;
+use heapless::String as HString;
+use pages_tui::prelude::*;
+use ratatui::{
+    layout::Rect,
+    style::Stylize,
+    widgets::Paragraph,
+    Terminal,
+};
+use log::info;
+
+/// Focus targets - different per screen
+#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
+pub enum PageFocus {
+    MenuImu,
+    MenuChat,
+    NavPrev,
+    NavNext,
+}
+
+impl FocusId for PageFocus {}
+
+const MENU_TARGETS: &[PageFocus] = &[PageFocus::MenuImu, PageFocus::MenuChat];
+const NAV_TARGETS: &[PageFocus] = &[PageFocus::NavPrev, PageFocus::NavNext];
+
+/// Display state
+pub struct DisplayState {
+    pub(crate) status: HString<32>,
+    pub(crate) last_imu: Option<ImuReading>,
+    pub(crate) last_error: Option<HString<64>>,
+    pub(crate) mqtt_connected: bool,
+    pub(crate) mqtt_msg_count: u32,
+    pub(crate) chat_msg1: HString<24>,
+    pub(crate) chat_msg2: HString<24>,
+}
+
+impl Default for DisplayState {
+    fn default() -> Self {
+        Self {
+            status: HString::new(),
+            last_imu: None,
+            last_error: None,
+            mqtt_connected: false,
+            mqtt_msg_count: 0,
+            chat_msg1: HString::new(),
+            chat_msg2: HString::new(),
+        }
+    }
+}
+
+impl DisplayState {
+    pub 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 = HString::new();
+            }
+            DisplayCommand::PushKey(_) => {}
+            DisplayCommand::AddChatMessage(msg) => {
+                self.add_chat_message(msg.as_str());
+            }
+        }
+    }
+
+    pub fn add_chat_message(&mut self, msg: &str) {
+        self.chat_msg2 = self.chat_msg1.clone();
+        self.chat_msg1.clear();
+        let _ = self.chat_msg1.push_str(&msg[..msg.len().min(24)]);
+    }
+}
+
+#[derive(Clone, Debug, PartialEq, Eq)]
+pub enum Screen {
+    Menu,
+    Imu,
+    Chat,
+}
+
+#[derive(Clone, Debug)]
+pub enum ScreenEvent {
+    GoToImu,
+    GoToChat,
+    NavigatePrev,
+    NavigateNext,
+}
+
+impl Component for Screen {
+    type Action = ComponentAction;
+    type Event = ScreenEvent;
+    type Focus = PageFocus;
+
+    fn handle(
+        &mut self,
+        focus: &Self::Focus,
+        action: Self::Action,
+    ) -> Result<Option<Self::Event>, ComponentError> {
+        if let ComponentAction::Select = action {
+            info!("Select {:?} focus:{:?}", self, focus);
+            return Ok(Some(match focus {
+                PageFocus::MenuImu => ScreenEvent::GoToImu,
+                PageFocus::MenuChat => ScreenEvent::GoToChat,
+                PageFocus::NavPrev => ScreenEvent::NavigatePrev,
+                PageFocus::NavNext => ScreenEvent::NavigateNext,
+            }));
+        }
+        Ok(None)
+    }
+
+    fn targets(&self) -> &[Self::Focus] {
+        match self {
+            Screen::Menu => MENU_TARGETS,
+            Screen::Imu | Screen::Chat => NAV_TARGETS,
+        }
+    }
+
+    fn on_enter(&mut self) -> Result<(), ComponentError> {
+        info!("Enter: {:?}", self);
+        Ok(())
+    }
+
+    fn on_exit(&mut self) -> Result<(), ComponentError> {
+        Ok(())
+    }
+}
+
+// PAGE ORDER
+const PAGE_ORDER: &[&str] = &["menu", "imu", "chat"];
+
+pub fn next_page_id(current: &str) -> &'static str {
+    let idx = PAGE_ORDER.iter().position(|&p| p == current).unwrap_or(0);
+    PAGE_ORDER[(idx + 1) % PAGE_ORDER.len()]
+}
+
+pub fn prev_page_id(current: &str) -> &'static str {
+    let idx = PAGE_ORDER.iter().position(|&p| p == current).unwrap_or(0);
+    if idx == 0 { PAGE_ORDER[PAGE_ORDER.len() - 1] } else { PAGE_ORDER[idx - 1] }
+}
+
+// RENDERING
+/// Get row rect (0-3) for 128x32 display
+#[inline]
+fn row(area: Rect, n: u16) -> Rect {
+    Rect::new(area.x, area.y + n, area.width, 1)
+}
+
+pub fn render_frame<B: ratatui::backend::Backend>(
+    terminal: &mut Terminal<B>,
+    current_screen: &Screen,
+    focused: Option<&PageFocus>,
+    state: &DisplayState,
+) {
+    let _ = terminal.draw(|f| {
+        let area = f.area();
+        
+        if let Some(ref err) = state.last_error {
+            f.render_widget(Paragraph::new(format!("ERR:{}", err.as_str())).white().bold(), area);
+            return;
+        }
+
+        match current_screen {
+            Screen::Menu => render_menu(f, area, focused, state),
+            Screen::Imu => render_imu(f, area, focused, state),
+            Screen::Chat => render_chat(f, area, focused, state),
+        }
+    });
+}
+
+fn render_menu(f: &mut ratatui::Frame, area: Rect, focused: Option<&PageFocus>, state: &DisplayState) {
+    // Row 0 Title
+    let conn = if state.mqtt_connected { "*" } else { "x" };
+    f.render_widget(
+        Paragraph::new(format!("-- ESP32 Hub {} --", conn)).white().bold(),
+        row(area, 0),
+    );
+
+    // Row 1 Sensors
+    let imu_sel = focused == Some(&PageFocus::MenuImu);
+    f.render_widget(
+        Paragraph::new(if imu_sel { " > Sensors" } else { "   Sensors" })
+            .style(if imu_sel { ratatui::style::Style::default().white().bold() } else { ratatui::style::Style::default().white() }),
+        row(area, 1),
+    );
+
+    // Row 2 Messages  
+    let chat_sel = focused == Some(&PageFocus::MenuChat);
+    f.render_widget(
+        Paragraph::new(if chat_sel { " > Messages" } else { "   Messages" })
+            .style(if chat_sel { ratatui::style::Style::default().white().bold() } else { ratatui::style::Style::default().white() }),
+        row(area, 2),
+    );
+}
+
+fn render_imu(f: &mut ratatui::Frame, area: Rect, focused: Option<&PageFocus>, state: &DisplayState) {
+    if let Some(ref imu) = state.last_imu {
+        // Row 0 Gyro
+        f.render_widget(
+            Paragraph::new(format!("G:{:+5.0}{:+5.0}{:+5.0}", imu.gyro_dps[0], imu.gyro_dps[1], imu.gyro_dps[2])).cyan(),
+            row(area, 0),
+        );
+        // Row 1 Accel
+        f.render_widget(
+            Paragraph::new(format!("A:{:+.1} {:+.1} {:+.1}", imu.accel_g[0], imu.accel_g[1], imu.accel_g[2])).green(),
+            row(area, 1),
+        );
+        // Row 2 Temp
+        f.render_widget(
+            Paragraph::new(format!("T: {:.1}C", imu.temp_c)).yellow(),
+            row(area, 2),
+        );
+    } else {
+        f.render_widget(Paragraph::new("Waiting for MPU...").white(), row(area, 1));
+    }
+
+    // Row 3 Nav bar
+    // render_nav_bar(f, row(area, 3), focused);
+}
+
+fn render_chat(f: &mut ratatui::Frame, area: Rect, focused: Option<&PageFocus>, state: &DisplayState) {
+    // Row 0 Message 1
+    if state.chat_msg1.is_empty() {
+        f.render_widget(Paragraph::new("(no messages)").white(), row(area, 0));
+    } else {
+        f.render_widget(Paragraph::new(format!(">{}", state.chat_msg1.as_str())).green(), row(area, 0));
+    }
+
+    // Row 1 Message 2
+    if !state.chat_msg2.is_empty() {
+        f.render_widget(Paragraph::new(format!(">{}", state.chat_msg2.as_str())).white(), row(area, 1));
+    }
+
+    // Row 3 Nav bar
+    render_nav_bar(f, row(area, 2), focused);
+}
+
+/// Nav bar: --[<]-------[>]--
+fn render_nav_bar(f: &mut ratatui::Frame, area: Rect, focused: Option<&PageFocus>) {
+    let prev_sel = focused == Some(&PageFocus::NavPrev);
+    let next_sel = focused == Some(&PageFocus::NavNext);
+    
+    let prev = if prev_sel { "[<]" } else { " < " };
+    let next = if next_sel { "[>]" } else { " > " };
+    
+    // Build the line: --[<]-------[>]--
+    let line = format!("--{}-------{}--", prev, next);
+    
+    f.render_widget(
+        Paragraph::new(line).style(ratatui::style::Style::default().white()),
+        area,
+    );
+}

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/mpu/api.rs

@@ -4,9 +4,9 @@ use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
 use embassy_sync::channel::{Channel, Receiver, Sender};
 use crate::contracts::ImuReading;
 
-const QUEUE_SIZE: usize = 4;
+const QUEUE_SIZE: usize = 16;
 
-pub(crate) static IMU_CHANNEL: Channel<CriticalSectionRawMutex, ImuReading, QUEUE_SIZE> = Channel::new();
+pub static IMU_CHANNEL: Channel<CriticalSectionRawMutex, ImuReading, QUEUE_SIZE> = Channel::new();
 
 pub fn events() -> Receiver<'static, CriticalSectionRawMutex, ImuReading, QUEUE_SIZE> {
     IMU_CHANNEL.receiver()

mqtt_display/src/mpu/task.rs

@@ -15,7 +15,7 @@ use crate::mpu::api::sender;
 use crate::mpu::driver::Mpu6050;
 
 /// Sampling interval in milliseconds.
-/// 50ms = 20Hz, reasonable for display updates.
+/// 50ms = 20Hz
 const SAMPLE_INTERVAL_MS: u64 = 50;
 
 /// MPU6050 I2C address (0x68 with AD0 low, 0x69 with AD0 high)
@@ -52,7 +52,7 @@ pub async fn mpu_task(i2c: I2cDevice) {
                 Timer::after(Duration::from_secs(2)).await;
                 continue;
             }
-            Err(_e) => {
+            Err(_) => {
                 warn!(
                     "I2C error verifying MPU6050 (attempt {})",
                     init_attempts
@@ -68,7 +68,7 @@ pub async fn mpu_task(i2c: I2cDevice) {
                 info!("MPU6050 initialized successfully");
                 break;
             }
-            Err(_e) => {
+            Err(_) => {
                 error!("MPU6050 init failed (attempt {})", init_attempts);
                 Timer::after(Duration::from_secs(2)).await;
                 continue;
@@ -78,11 +78,14 @@ pub async fn mpu_task(i2c: I2cDevice) {
 
     // 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);
+    info!(
+        "MPU task entering sampling loop ({}ms interval)",
+        SAMPLE_INTERVAL_MS
+    );
 
     let tx = sender();
     let mut consecutive_errors = 0u32;
+    let mut sent_count: u32 = 0;
 
     loop {
         let start = Instant::now();
@@ -98,13 +101,20 @@ pub async fn mpu_task(i2c: I2cDevice) {
                     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
+                sent_count = sent_count.wrapping_add(1);
+                if tx.try_send(reading).is_ok() {
+                    if sent_count % 20 == 0 {
+                        info!(
+                            "IMU send#{} ax:{:.2} ay:{:.2} az:{:.2} t:{:.1}",
+                            sent_count, accel_g[0], accel_g[1], accel_g[2], temp_c
+                        );
+                    }
+                } else if sent_count % 20 == 0 {
+                    info!("IMU drop: channel full ({} sent)", sent_count);
                 }
             }
-            Err(_e) => {
+
+            Err(_) => {
                 consecutive_errors += 1;
                 if consecutive_errors == 1 || consecutive_errors % 10 == 0 {
                     warn!("MPU read error (consecutive: {})", consecutive_errors);
@@ -122,7 +132,7 @@ pub async fn mpu_task(i2c: I2cDevice) {
             }
         }
 
-        // Sleep for remainder of interval
+        // Maintain a steady period
         let elapsed = start.elapsed();
         let target = Duration::from_millis(SAMPLE_INTERVAL_MS);
         if elapsed < target {

mqtt_display/src/mqtt/client.rs

@@ -1,7 +1,8 @@
 // src/mqtt/client.rs
+
 use embassy_futures::select::{select, Either};
 use embassy_net::{tcp::TcpSocket, Stack};
-use embassy_time::{Duration, Timer};
+use embassy_time::{Duration, Timer, with_timeout, Instant};
 use rust_mqtt::client::client::MqttClient;
 use rust_mqtt::client::client_config::{ClientConfig, MqttVersion};
 use rust_mqtt::packet::v5::publish_packet::QualityOfService;
@@ -9,16 +10,18 @@ use rust_mqtt::packet::v5::reason_codes::ReasonCode;
 use rust_mqtt::utils::rng_generator::CountingRng;
 
 use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+use embassy_sync::mutex::Mutex;
 use embassy_sync::channel::{Channel, Receiver};
 use heapless::{String, Vec};
 use static_cell::ConstStaticCell;
-use log::info;
+use log::{info, warn};
 
 use crate::mqtt::config::mqtt_broker_endpoint;
 
 const RECONNECT_DELAY_SECS: u64 = 5;
 const KEEPALIVE_SECS: u64 = 60;
 const PING_PERIOD: Duration = Duration::from_secs(KEEPALIVE_SECS / 2);
+const PING_TIMEOUT: Duration = Duration::from_secs(2);
 
 // Limits for static buffers
 pub const TOPIC_MAX: usize = 128;
@@ -57,10 +60,17 @@ enum Command {
     Subscribe(String<TOPIC_MAX>),
 }
 
+// Command/info channels
 static CMD_CHAN: Channel<CriticalSectionRawMutex, Command, COMMAND_QUEUE> = Channel::new();
 static EVT_CHAN: Channel<CriticalSectionRawMutex, IncomingMsg, EVENT_QUEUE> = Channel::new();
 
-// Public API
+/// Shared latest IMU payload (non-blocking, latest-value semantics)
+static IMU_LATEST: Mutex<CriticalSectionRawMutex, Option<Vec<u8, PAYLOAD_MAX>>> =
+    Mutex::new(None);
+
+/// Public API
+
+/// Blocking publish for command/response messages.
 pub async fn mqtt_publish(topic: &str, payload: &[u8], qos: QualityOfService, retain: bool) {
     CMD_CHAN
         .send(Command::Publish(PublishMsg {
@@ -68,11 +78,35 @@ pub async fn mqtt_publish(topic: &str, payload: &[u8], qos: QualityOfService, re
             payload: truncate_payload(payload),
             qos,
             retain,
-        })).await;
+        }))
+        .await;
+}
+
+/// Non-blocking publish for other traffic (fire-and-forget)
+pub fn mqtt_try_publish(topic: &str, payload: &[u8], qos: QualityOfService, retain: bool) -> bool {
+    CMD_CHAN
+        .try_send(Command::Publish(PublishMsg {
+            topic: truncate_str::<TOPIC_MAX>(topic),
+            payload: truncate_payload(payload),
+            qos,
+            retain,
+        }))
+        .is_ok()
+}
+
+/// Set latest IMU telemetry payload (non-blocking, overwrites previous)
+pub fn mqtt_set_imu(payload: &[u8]) {
+    if let Ok(mut guard) = IMU_LATEST.try_lock() {
+        let mut buf: Vec<u8, PAYLOAD_MAX> = Vec::new();
+        let _ = buf.extend_from_slice(&payload[..payload.len().min(PAYLOAD_MAX)]);
+        *guard = Some(buf);
+    }
 }
 
 pub async fn mqtt_subscribe(topic: &str) {
-    CMD_CHAN.send(Command::Subscribe(truncate_str::<TOPIC_MAX>(topic))).await;
+    CMD_CHAN
+        .send(Command::Subscribe(truncate_str::<TOPIC_MAX>(topic)))
+        .await;
 }
 
 pub fn mqtt_events(
@@ -80,10 +114,24 @@ pub fn mqtt_events(
     EVT_CHAN.receiver()
 }
 
-// Helper functions for memory-safe truncation
+/// Internals
+
 fn truncate_str<const N: usize>(s: &str) -> String<N> {
     let mut h = String::new();
-    let _ = h.push_str(&s[..s.len().min(N)]);
+    if N == 0 {
+        return h;
+    }
+    if s.len() <= N {
+        let _ = h.push_str(s);
+        return h;
+    }
+
+    // Cut on a UTF-8 char boundary to avoid panics.
+    let mut cut = N;
+    while cut > 0 && !s.is_char_boundary(cut) {
+        cut -= 1;
+    }
+    let _ = h.push_str(&s[..cut]);
     h
 }
 
@@ -96,29 +144,52 @@ fn truncate_payload(data: &[u8]) -> Vec<u8, PAYLOAD_MAX> {
 async fn handle_command(client: &mut Client<'_, '_>, cmd: Command) -> Result<(), ReasonCode> {
     match cmd {
         Command::Publish(msg) => {
-            client
-                .send_message(msg.topic.as_str(), &msg.payload, msg.qos, msg.retain)
+            match with_timeout(
+                Duration::from_secs(5),
+                client.send_message(msg.topic.as_str(), &msg.payload, msg.qos, msg.retain),
+            )
             .await
+            {
+                Ok(result) => result,
+                Err(_) => {
+                    warn!("MQTT send timed out, forcing reconnect");
+                    Err(ReasonCode::UnspecifiedError)
+                }
+            }
         }
         Command::Subscribe(topic) => {
-            client.subscribe_to_topic(topic.as_str()).await?;
+            match with_timeout(
+                Duration::from_secs(5),
+                client.subscribe_to_topic(topic.as_str()),
+            )
+            .await
+            {
+                Ok(result) => {
+                    result?;
                     info!("Subscribed to '{}'", topic);
                     Ok(())
                 }
+                Err(_) => {
+                    warn!("MQTT subscribe timed out");
+                    Err(ReasonCode::UnspecifiedError)
+                }
+            }
+        }
     }
 }
 
 async fn handle_incoming(result: Result<(&str, &[u8]), ReasonCode>) -> Result<(), ReasonCode> {
     let (topic, payload) = result?;
-    EVT_CHAN
-        .send(IncomingMsg {
+    let msg = IncomingMsg {
         topic: truncate_str::<TOPIC_MAX>(topic),
         payload: truncate_payload(payload),
-        }).await;
+    };
+    if EVT_CHAN.try_send(msg).is_err() {
+        warn!("MQTT EVT queue full, dropping incoming message");
+    }
     Ok(())
 }
 
-// Session and reconnect control
 async fn run_one_session(
     stack: Stack<'static>,
     tcp_rx: &mut [u8],
@@ -127,6 +198,7 @@ async fn run_one_session(
     mqtt_rx: &mut [u8],
 ) -> Result<(), ()> {
     let mut socket = TcpSocket::new(stack, tcp_rx, tcp_tx);
+    socket.set_timeout(Some(Duration::from_secs(20)));
     match socket.connect(mqtt_broker_endpoint()).await {
         Ok(_) => info!("Connected TCP to MQTT broker"),
         Err(e) => {
@@ -136,11 +208,14 @@ async fn run_one_session(
     }
 
     // MQTT configuration and client setup
-    let mut cfg: ClientConfig<8, CountingRng> = ClientConfig::new(MqttVersion::MQTTv5, CountingRng(0));
+    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);
+    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) => {
@@ -149,19 +224,79 @@ async fn run_one_session(
         }
     }
 
-    // Operational loop
-    loop {
-        let net_or_ping = select(client.receive_message(), Timer::after(PING_PERIOD));
+    let mut next_ping_at = Instant::now() + 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(|_| ())?,
+    loop {
+        // Send latest IMU payload if available
+        if let Ok(mut guard) = IMU_LATEST.try_lock() {
+            if let Some(payload) = guard.take() {
+                drop(guard);
+
+                log::info!("MQTT IMU TX start ({} bytes)", payload.len());
+
+                // Limit send to max 2 seconds to catch network stalls
+                let send_res = with_timeout(
+                    Duration::from_secs(2),
+                    client.send_message("esp32/imu", &payload, QualityOfService::QoS0, false),
+                )
+                    .await;
+
+                match send_res {
+                    Ok(Ok(_)) => {
+                        log::info!("MQTT IMU TX ok");
+                        next_ping_at = Instant::now() + PING_PERIOD;
+                    }
+                    Ok(Err(e)) => {
+                        log::warn!("MQTT IMU TX failed: {:?}", e);
+                        return Err(());
+                    }
+                    Err(_) => {
+                        log::warn!("MQTT IMU TX timed out, restarting session");
+                        return Err(());
+                    }
+                }
+            }
+        }
+
+        // Process any queued control commands
+        while let Ok(cmd) = CMD_CHAN.try_receive() {
+            handle_command(&mut client, cmd).await.map_err(|_| ())?;
+            next_ping_at = Instant::now() + PING_PERIOD;
+        }
+
+        // Check for incoming messages with timeout
+        match with_timeout(Duration::from_millis(100), client.receive_message()).await {
+            Ok(Ok((topic, payload))) => {
+                // Handle incoming message
+                let _ = handle_incoming(Ok((topic, payload))).await;
+                next_ping_at = Instant::now() + PING_PERIOD;
+            }
+            Ok(Err(e)) => {
+                log::warn!("MQTT receive error: {:?}", e);
+                return Err(());
+            }
+            Err(_) => {
+            }
+        }
+        if Instant::now() >= next_ping_at {
+            match with_timeout(PING_TIMEOUT, client.send_ping()).await {
+                Ok(Ok(_)) => {
+                    next_ping_at = Instant::now() + PING_PERIOD;
+                }
+                Ok(Err(e)) => {
+                    warn!("MQTT ping failed: {:?}", e);
+                    return Err(());
+                }
+                Err(_) => {
+                    warn!("MQTT ping timed out, restarting session");
+                    return Err(());
+                }
+            }
         }
     }
 }
 
-// Main MQTT embassy task
+/// Main MQTT embassy task
 #[embassy_executor::task]
 pub async fn mqtt_task(stack: Stack<'static>) {
     info!("MQTT task starting...");
@@ -178,9 +313,13 @@ 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;
     }
 }