From c6ef9559ecf34a2f4138fcff5920ed570261b9c2 Mon Sep 17 00:00:00 2001 From: Stephen Waits Date: Wed, 15 Apr 2026 00:57:36 -0600 Subject: [PATCH] refactor: drive radio task via hsmc statechart MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replaces the imperative Mode enum + if-branches in the radio loop with a declarative hsmc statechart mirroring the display: Radio ├── Unconfigured └── Configured ├── Idle ├── Receiving └── Transmitting ├── FromIdle └── FromRx The `TransmittingFrom{Idle,Rx}` substates encode "where to return after TX" as state identity, killing the `was_receiving` bookkeeping. Entry actions announce `RadioEvent`s on the display channel; RF I/O stays in the task loop because `rx_once` blocks indefinitely and cannot live inside an action. Radio-init failure runs a Ping-only degraded loop without ever instantiating the machine. --- src/radio.rs | 570 +++++++++++++++++++++++++++++++++------------------ 1 file changed, 375 insertions(+), 195 deletions(-) diff --git a/src/radio.rs b/src/radio.rs index aab73d1..ee41c9f 100644 --- a/src/radio.rs +++ b/src/radio.rs @@ -1,23 +1,36 @@ -//! LoRa radio task: SX1262 state machine driven by host commands. -//! -//! Owns the radio peripheral exclusively. Receives [`Command`]s from the -//! host task, drives the SX1262 via [`lora_phy`], and sends [`Response`]s -//! back. Publishes typed [`RadioEvent`]s to the display task at every state -//! transition and RF event — no shared status snapshot, no diffing consumer. -//! -//! # State machine +//! LoRa radio task: SX1262 statechart driven by host commands. //! //! ```text -//! Idle ──StartRx──► Receiving ──StopRx──► Idle -//! │ │ -//! └──Transmit──► Transmitting ──TxDone──► (previous state) +//! Radio +//! ├── Unconfigured (default) no config cached → no RF ops possible +//! └── Configured +//! ├── Idle (default) ready, waiting +//! ├── Receiving continuous RX +//! └── Transmitting +//! ├── FromIdle (default) TransmitDone → Idle +//! └── FromRx TransmitDone → Receiving //! ``` +//! +//! The machine owns two pieces of radio state: which mode we're in and +//! the cached [`RadioConfig`]. Entry/exit actions broadcast typed +//! [`RadioEvent`]s to the display. RF I/O (`rx_once`, `do_tx`, +//! `enter_standby`) can't live in actions — `rx_once` blocks +//! indefinitely — so the task's main loop stays imperative and drives +//! the machine via `machine.send` + `drain`, branching on +//! `machine.current_state()`. +//! +//! The `Transmitting` substates encode "where to return after TX" as +//! state identity; no `was_receiving` bookkeeping. Radio init happens +//! **before** the machine ever steps — success falls through into the +//! default `Unconfigured`, failure early-returns into a small +//! Ping-only degraded loop that doesn't involve the machine at all. use defmt::{error, info, warn}; use embassy_executor::task; use embassy_futures::select::{select, Either}; use embassy_time::Delay; -use lora_phy::mod_params::RadioError; +use hsmc::{statechart, Duration}; +use lora_phy::mod_params::{PacketStatus, RadioError}; use lora_phy::{LoRa, RxMode}; use crate::board::{Board, LoRaBoard, RadioDriver, RadioParts}; @@ -26,42 +39,163 @@ use crate::protocol::{self, Command, ErrorCode, RadioConfig, Response}; const MAX_PAYLOAD: usize = protocol::MAX_PAYLOAD; -type Radio = LoRa; +type LoRaDriver = LoRa; // ── Fixed LoRa radio parameters ───────────────────────────────────── const IMPLICIT_HEADER: bool = false; const CRC_ON: bool = true; const IQ_INVERTED: bool = false; -/// Internal mode, just for tracking what the radio is doing right now. -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -enum Mode { - Idle, - Receiving, - Transmitting, +/// Internal events that drive `Radio` state transitions. Kept distinct +/// from [`crate::channel::RadioEvent`], which is the display-facing +/// broadcast bus. +#[derive(Debug, Clone)] +pub enum RadioInput { + /// Host issued `SetConfig`; driver already validated + resolved. + ConfigSet(RadioConfig), + /// `start_rx` returned `Ok(())` — radio is now in RX mode. + StartRxOk, + /// `enter_standby` completed. + StopRxOk, + /// About to call `do_tx` — parks the machine in `Transmitting.*`. + TransmitBegin, + /// `do_tx` returned (success or failure). Either way, leave Transmitting. + TransmitDone, + /// `start_rx` failed mid-Receiving; fall back to Idle. + RxRestartFailed, + /// A packet was just received off-air with these link metrics. + PacketReceived { rssi: i16, snr: i16 }, } -/// Runtime state not exposed outside this module. -struct RadioState { - mode: Mode, +/// Machine context — config cache + the outbound event channel. The +/// `LoRa` driver, RX buffer, and command-validation plumbing stay local +/// to `radio_task` because their async operations can't live inside +/// statechart actions. +pub struct RadioContext { + events: &'static RadioEventChannel, config: Option, } -impl RadioState { - fn new() -> Self { - Self { - mode: Mode::Idle, - config: None, +impl RadioContext { + fn announce(&self, ev: RadioEvent) { + if self.events.try_send(ev).is_err() { + warn!("radio event dropped: downstream queue full"); } } } -/// Emit a radio event to the display subscriber. Best-effort: if the -/// downstream queue is full we'd rather drop a UI update than stall the -/// radio task. -fn emit(events: &RadioEventChannel, ev: RadioEvent) { - if events.try_send(ev).is_err() { - warn!("radio event dropped: downstream queue full"); +statechart! { + Radio { + context: RadioContext; + events: RadioInput; + default(Unconfigured); + + state Unconfigured { + // Two declarations for the same event: the first catches the + // payload to cache it, the second transitions. `ConfigSet` + // from this state always means "first config ever". + on(ConfigSet(cfg: RadioConfig)) => cache_config; + on(ConfigSet) => Configured; + } + + state Configured { + // Re-configure from any Configured substate (driver rejects + // mid-TX, so this never fires during Transmitting). + on(ConfigSet(cfg: RadioConfig)) => cache_config; + default(Idle); + + state Idle { + entry: announce_idle; + on(StartRxOk) => Receiving; + on(TransmitBegin) => TransmittingFromIdle; + } + + state Receiving { + entry: announce_entered_rx; + on(PacketReceived { rssi: i16, snr: i16 }) => announce_packet_rx; + on(StopRxOk) => Idle; + on(TransmitBegin) => TransmittingFromRx; + on(RxRestartFailed) => Idle; + } + + state Transmitting { + entry: announce_entered_tx; + exit: announce_packet_tx; + default(TransmittingFromIdle); + + state TransmittingFromIdle { + on(TransmitDone) => Idle; + } + + state TransmittingFromRx { + on(TransmitDone) => Receiving; + on(RxRestartFailed) => Idle; + } + } + } + } +} + +impl RadioActions for RadioActionContext<'_> { + async fn cache_config(&mut self, cfg: RadioConfig) { + self.config = Some(cfg); + self.announce(RadioEvent::ConfigChanged(cfg)); + } + + async fn announce_idle(&mut self) { + self.announce(RadioEvent::Idle); + } + + async fn announce_entered_rx(&mut self) { + self.announce(RadioEvent::EnteredRx); + } + + async fn announce_entered_tx(&mut self) { + self.announce(RadioEvent::EnteredTx); + } + + async fn announce_packet_rx(&mut self, rssi: i16, snr: i16) { + self.announce(RadioEvent::PacketRx { + rssi, + snr: Some(snr), + }); + } + + async fn announce_packet_tx(&mut self) { + self.announce(RadioEvent::PacketTx); + } +} + +/// Drain the machine's internal queue. Call after every `push`. +async fn drain(m: &mut Radio) { + while m.has_pending_events() { + m.step(Duration::ZERO).await; + } +} + +/// Push an event into the machine's internal queue and immediately drain +/// so `current_state()` reflects the transition before we return. +/// Overflow here is a real bug — the queue is capacity 8 and we never +/// batch — so we panic rather than silently drop. +async fn push(m: &mut Radio, ev: RadioInput) { + m.send(ev).expect("radio machine queue overflow"); + drain(m).await; +} + +/// Post-TX invariant: if the machine transitioned back into `Receiving`, +/// the hardware is still in standby from `do_tx`; put it back in RX. On +/// failure, tell the machine to fall through to `Idle`. +async fn restore_rx_if_needed(machine: &mut Radio, lora: &mut LoRaDriver) { + if machine.current_state() != RadioState::Receiving { + return; + } + let Some(cfg) = machine.context().config else { + // Can't happen: Receiving implies config is cached. + return; + }; + if let Err(e) = start_rx(lora, &cfg).await { + warn!("post-TX RX restart failed: {}", e); + push(machine, RadioInput::RxRestartFailed).await; } } @@ -72,124 +206,119 @@ pub async fn radio_task( responses: &'static ResponseChannel, events: &'static RadioEventChannel, ) { - let mut state = RadioState::new(); - + // Init the radio BEFORE the machine exists in any meaningful way — + // a failure here never touches the statechart; we just run a + // Ping-only loop forever. let mut lora = match LoRa::new(parts.driver, false, parts.delay).await { - Ok(l) => l, + Ok(l) => { + info!("radio initialized"); + l + } Err(e) => { error!("radio init failed: {}", e); responses .send(Response::Error(ErrorCode::InvalidConfig)) .await; - // Radio is non-functional — respond to commands but can't do RF. - loop { - let cmd = commands.receive().await; - if let Command::Ping = cmd { - responses.send(Response::Pong).await; - } else { - responses - .send(Response::Error(ErrorCode::InvalidConfig)) - .await; - } - } + degraded_loop(commands, responses).await; + return; } }; - info!("radio initialized"); + let mut machine = Radio::new_local(RadioContext { + events, + config: None, + }); + // First step enters the default path: Unconfigured. + machine.step(Duration::ZERO).await; + let mut rx_buf = [0u8; MAX_PAYLOAD]; loop { - if state.mode == Mode::Receiving { - // `Mode::Receiving` is only set after `start_rx` succeeded with - // a validated config; we rely on that here. - let cfg = match state.config { - Some(c) => c, - None => { - warn!("BUG: receiving without config, returning to idle"); - state.mode = Mode::Idle; - emit(events, RadioEvent::Idle); - continue; - } - }; - - match select(rx_once(&mut lora, &cfg, &mut rx_buf), commands.receive()).await { - Either::First(rx_result) => match rx_result { - Ok((len, pkt_status)) => { - emit( - events, - RadioEvent::PacketRx { - rssi: pkt_status.rssi, - snr: Some(pkt_status.snr), - }, - ); - - let copy_len = (len as usize).min(MAX_PAYLOAD); - if (len as usize) > copy_len { - warn!("RX payload truncated: {} > {}", len, MAX_PAYLOAD); - } - let mut payload = heapless::Vec::new(); - let _ = payload.extend_from_slice(&rx_buf[..copy_len]); - - if responses - .try_send(Response::RxPacket { - rssi: pkt_status.rssi, - snr: pkt_status.snr, - payload, - }) - .is_err() - { - warn!("RX packet dropped: response channel full"); - } - - if let Err(e) = start_rx(&mut lora, &cfg).await { - warn!("restart RX failed: {}", e); - state.mode = Mode::Idle; - emit(events, RadioEvent::Idle); - let _ = responses.try_send(Response::Error(ErrorCode::RadioBusy)); - } + match machine.current_state() { + RadioState::Unconfigured | RadioState::Idle => { + let cmd = commands.receive().await; + handle_cmd(cmd, &mut machine, &mut lora, responses).await; + } + RadioState::Receiving => { + // Config presence is a state invariant: we can't be in + // Receiving without having passed through ConfigSet. + let cfg = machine + .context() + .config + .expect("Receiving requires cached config"); + match select( + rx_once(&mut lora, &cfg, &mut rx_buf), + commands.receive(), + ) + .await + { + Either::First(rx_result) => { + handle_rx_result( + rx_result, + &mut machine, + &mut lora, + &cfg, + &mut rx_buf, + responses, + ) + .await; } - Err(e) => { - warn!("RX error: {}", e); - if start_rx(&mut lora, &cfg).await.is_err() { - state.mode = Mode::Idle; - emit(events, RadioEvent::Idle); - let _ = responses.try_send(Response::Error(ErrorCode::RadioBusy)); - } + Either::Second(cmd) => { + handle_cmd(cmd, &mut machine, &mut lora, responses).await; } - }, - Either::Second(cmd) => { - handle_cmd(cmd, &mut lora, &mut state, responses, events).await; } } - } else { - let cmd = commands.receive().await; - handle_cmd(cmd, &mut lora, &mut state, responses, events).await; + // `Transmitting.*` is transient — entered and exited inside + // `handle_cmd`'s Transmit branch. `Configured` and + // `Transmitting` are composite states that `current_state()` + // never returns once `descend_defaults` has completed. If + // any of these land here it's a bug; log + yield so we + // don't tight-spin. + RadioState::TransmittingFromIdle + | RadioState::TransmittingFromRx + | RadioState::Configured + | RadioState::Transmitting => { + warn!( + "radio main loop reached transient/composite state; yielding" + ); + embassy_time::Timer::after_millis(10).await; + } } } } - +/// Init-failure fallback. Answers Ping; everything else errors. +async fn degraded_loop(commands: &CommandChannel, responses: &ResponseChannel) { + loop { + let cmd = commands.receive().await; + if let Command::Ping = cmd { + responses.send(Response::Pong).await; + } else { + responses + .send(Response::Error(ErrorCode::InvalidConfig)) + .await; + } + } +} async fn handle_cmd( cmd: Command, - lora: &mut Radio, - state: &mut RadioState, + machine: &mut Radio, + lora: &mut LoRaDriver, responses: &ResponseChannel, - events: &RadioEventChannel, ) { match cmd { Command::Ping => { responses.send(Response::Pong).await; } - Command::GetConfig => { - if let Some(cfg) = state.config { - responses.send(Response::Config(cfg)).await; - } else { + Command::GetConfig => match machine.context().config { + Some(cfg) => responses.send(Response::Config(cfg)).await, + None => { responses .send(Response::Error(ErrorCode::NotConfigured)) .await; } - } + }, Command::SetConfig(cfg) => { if let Err(reason) = cfg.validate(Board::TX_POWER_RANGE) { warn!("SetConfig rejected: {}", reason); @@ -198,93 +327,144 @@ async fn handle_cmd( .await; } else { let resolved = cfg.resolve(Board::TX_POWER_RANGE); - state.config = Some(resolved); - emit(events, RadioEvent::ConfigChanged(resolved)); + push(machine, RadioInput::ConfigSet(resolved)).await; responses.send(Response::Ok).await; } } Command::StartRx => { - if let Some(cfg) = state.config { - match start_rx(lora, &cfg).await { - Ok(()) => { - state.mode = Mode::Receiving; - emit(events, RadioEvent::EnteredRx); - responses.send(Response::Ok).await; - } - Err(e) => { - warn!("StartRx failed: {}", e); - responses - .send(Response::Error(ErrorCode::InvalidConfig)) - .await; - } - } - } else { - responses - .send(Response::Error(ErrorCode::NotConfigured)) - .await; - } - } - Command::StopRx => { - let _ = lora.enter_standby().await; - if state.mode != Mode::Idle { - state.mode = Mode::Idle; - emit(events, RadioEvent::Idle); - } - let _ = responses.try_send(Response::Ok); - } - Command::DisplayOn | Command::DisplayOff | Command::GetMac => {} - Command::Transmit { config, payload } => { - let tx_config = config - .map(|c| c.resolve(Board::TX_POWER_RANGE)) - .or(state.config); - let Some(cfg) = tx_config else { + let Some(cfg) = machine.context().config else { responses .send(Response::Error(ErrorCode::NotConfigured)) .await; return; }; - if let Err(reason) = cfg.validate(Board::TX_POWER_RANGE) { - warn!("TX config rejected: {}", reason); - responses - .send(Response::Error(ErrorCode::InvalidConfig)) - .await; - return; - } - - let was_receiving = state.mode == Mode::Receiving; - state.mode = Mode::Transmitting; - emit(events, RadioEvent::EnteredTx); - - let tx_result = do_tx(lora, &cfg, &payload).await; - match tx_result { + match start_rx(lora, &cfg).await { Ok(()) => { - emit(events, RadioEvent::PacketTx); - responses.send(Response::TxDone).await; + push(machine, RadioInput::StartRxOk).await; + responses.send(Response::Ok).await; } Err(e) => { - warn!("TX failed: {}", e); - responses.send(Response::Error(ErrorCode::TxTimeout)).await; + warn!("StartRx failed: {}", e); + responses + .send(Response::Error(ErrorCode::InvalidConfig)) + .await; } } + } + Command::StopRx => { + let _ = lora.enter_standby().await; + push(machine, RadioInput::StopRxOk).await; + // try_send: StopRx may be sent internally on USB disconnect + // when the host isn't draining responses. + let _ = responses.try_send(Response::Ok); + } + Command::DisplayOn | Command::DisplayOff | Command::GetMac => {} + Command::Transmit { + config: tx_cfg, + payload, + } => execute_transmit(tx_cfg, &payload, machine, lora, responses).await, + } +} - // Restore previous mode — and announce the transition explicitly - // so downstream consumers don't have to infer it. - if was_receiving { - let rx_cfg = state.config.unwrap_or(cfg); - match start_rx(lora, &rx_cfg).await { - Ok(()) => { - state.mode = Mode::Receiving; - emit(events, RadioEvent::EnteredRx); - } - Err(e) => { - warn!("post-TX RX restart failed: {}", e); - state.mode = Mode::Idle; - emit(events, RadioEvent::Idle); - } - } - } else { - state.mode = Mode::Idle; - emit(events, RadioEvent::Idle); +/// Validate the effective TX config, enter `Transmitting.*`, run +/// `do_tx`, and leave — restoring RX if the machine returned to +/// `Receiving`. Isolated from `handle_cmd` because it's the only +/// command that crosses the statechart in two hops and has a +/// non-trivial post-condition (RX hardware re-arm). +async fn execute_transmit( + tx_cfg: Option, + payload: &[u8], + machine: &mut Radio, + lora: &mut LoRaDriver, + responses: &ResponseChannel, +) { + let eff_cfg = tx_cfg + .map(|c| c.resolve(Board::TX_POWER_RANGE)) + .or(machine.context().config); + let Some(cfg) = eff_cfg else { + responses + .send(Response::Error(ErrorCode::NotConfigured)) + .await; + return; + }; + if let Err(reason) = cfg.validate(Board::TX_POWER_RANGE) { + warn!("TX config rejected: {}", reason); + responses + .send(Response::Error(ErrorCode::InvalidConfig)) + .await; + return; + } + + // Per-state handlers pick TransmittingFromIdle vs TransmittingFromRx. + push(machine, RadioInput::TransmitBegin).await; + + let tx_result = do_tx(lora, &cfg, payload).await; + + // Fire TransmitDone regardless — we must leave Transmitting. The + // exit action announces PacketTx to the display. + push(machine, RadioInput::TransmitDone).await; + + match tx_result { + Ok(()) => responses.send(Response::TxDone).await, + Err(e) => { + warn!("TX failed: {}", e); + responses + .send(Response::Error(ErrorCode::TxTimeout)) + .await; + } + } + + restore_rx_if_needed(machine, lora).await; +} + +async fn handle_rx_result( + rx_result: Result<(u8, PacketStatus), RadioError>, + machine: &mut Radio, + lora: &mut LoRaDriver, + cfg: &RadioConfig, + rx_buf: &mut [u8; MAX_PAYLOAD], + responses: &ResponseChannel, +) { + match rx_result { + Ok((len, pkt_status)) => { + push( + machine, + RadioInput::PacketReceived { + rssi: pkt_status.rssi, + snr: pkt_status.snr, + }, + ) + .await; + + let copy_len = (len as usize).min(MAX_PAYLOAD); + if (len as usize) > copy_len { + warn!("RX payload truncated: {} > {}", len, MAX_PAYLOAD); + } + let mut payload = heapless::Vec::new(); + let _ = payload.extend_from_slice(&rx_buf[..copy_len]); + + if responses + .try_send(Response::RxPacket { + rssi: pkt_status.rssi, + snr: pkt_status.snr, + payload, + }) + .is_err() + { + warn!("RX packet dropped: response channel full"); + } + + if let Err(e) = start_rx(lora, cfg).await { + warn!("restart RX failed: {}", e); + push(machine, RadioInput::RxRestartFailed).await; + let _ = responses.try_send(Response::Error(ErrorCode::RadioBusy)); + } + } + Err(e) => { + warn!("RX error: {}", e); + if start_rx(lora, cfg).await.is_err() { + push(machine, RadioInput::RxRestartFailed).await; + let _ = responses.try_send(Response::Error(ErrorCode::RadioBusy)); } } } @@ -341,13 +521,13 @@ fn to_cr(cr: u8) -> lora_phy::mod_params::CodingRate { } fn modulation_params( - lora: &mut Radio, + lora: &mut LoRaDriver, cfg: &RadioConfig, ) -> Result { lora.create_modulation_params(to_sf(cfg.sf), to_bw(cfg.bw), to_cr(cfg.cr), cfg.freq_hz) } -async fn start_rx(lora: &mut Radio, cfg: &RadioConfig) -> Result<(), RadioError> { +async fn start_rx(lora: &mut LoRaDriver, cfg: &RadioConfig) -> Result<(), RadioError> { let mdltn = modulation_params(lora, cfg)?; let pkt = lora.create_rx_packet_params( cfg.preamble_len, @@ -361,10 +541,10 @@ async fn start_rx(lora: &mut Radio, cfg: &RadioConfig) -> Result<(), RadioError> } async fn rx_once( - lora: &mut Radio, + lora: &mut LoRaDriver, cfg: &RadioConfig, buf: &mut [u8], -) -> Result<(u8, lora_phy::mod_params::PacketStatus), RadioError> { +) -> Result<(u8, PacketStatus), RadioError> { let mdltn = modulation_params(lora, cfg)?; let pkt = lora.create_rx_packet_params( cfg.preamble_len, @@ -379,7 +559,7 @@ async fn rx_once( const CAD_MAX_RETRIES: u8 = 10; -async fn do_tx(lora: &mut Radio, cfg: &RadioConfig, payload: &[u8]) -> Result<(), RadioError> { +async fn do_tx(lora: &mut LoRaDriver, cfg: &RadioConfig, payload: &[u8]) -> Result<(), RadioError> { let mdltn = modulation_params(lora, cfg)?; if cfg.cad_enabled() {