diff --git a/.gitignore b/.gitignore index 7cadc23..805279c 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,6 @@ target Cargo.lock .DS_Store + +#agent files +GEMINI.md diff --git a/Cargo.toml b/Cargo.toml index 962cd55..8b2ba0f 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "ld2410s" -version = "0.1.2" +version = "0.2.0" authors = ["Max van der Schee <26795741+mvdschee@users.noreply.github.com>"] description = "HLK-LD2410S driver with backend-agnostic UART ownership for serialport or esp-idf-hal" edition = "2024" @@ -18,8 +18,8 @@ debug = true opt-level = "z" [[example]] -name = "desktop" -path = "examples/desktop.rs" +name = "serial" +path = "examples/serial.rs" required-features = ["serial"] [[example]] @@ -27,13 +27,26 @@ name = "esp" path = "examples/esp.rs" required-features = ["embedded"] +[[example]] +name = "async_serial" +path = "examples/async_serial.rs" +required-features = ["async"] + +[[example]] +name = "async_esp" +path = "examples/async_esp.rs" +required-features = ["embedded", "async"] + [dependencies] heapless = "0.9.2" +embedded-io-async = { version = "0.7.0", optional = true } +embassy-time = { version = "0.5.0", optional = true } [features] default = [] serial = ["serialport"] embedded = ["esp-idf-hal"] +async = ["embedded-io-async", "embassy-time"] [dependencies.serialport] version = "4.8.1" @@ -42,3 +55,11 @@ optional = true [dependencies.esp-idf-hal] version = "0.45.2" optional = true + + +[dev-dependencies] +tokio = { version = "1.49.0", features = ["full"] } +tokio-serial = "5.4.5" +embedded-io-adapters = { version = "0.7.0", features = ["tokio-1"] } +anyhow = "1.0.100" +embassy-time = { version = "0.5.0", features = ["std", "generic-queue-8"] } diff --git a/README.md b/README.md index 0ff1574..c0d6926 100644 --- a/README.md +++ b/README.md @@ -1,28 +1,27 @@ # LD2410S Rust Driver -A Rust library for reading and controlling the **HLK-LD2410S** 24GHz radar presence sensor over UART. -Supports both **desktop** (via [serialport](https://crates.io/crates/serialport)) and **embedded** (via [esp-idf-hal](https://crates.io/crates/esp-idf-hal)) targets. +A **no_std**, **zero-heap** Rust library for reading and controlling the **HLK-LD2410S** 24GHz radar presence sensor over UART. ## ✨ Features -- Works with **desktop** and **ESP-IDF** environments using the same API. -- **Unified UART interface** so your application doesn’t need to handle serial quirks. -- Built-in **frame parsing** for: +- **Portability**: Core driver is `no_std` and uses zero heap allocations (via `heapless`). +- **Flexible IO**: Supports **Blocking** (Sync) and **Async** (via `embedded-io-async`). +- **Backend Agnostic**: Works on desktop (via `serialport`) and embedded (via `esp-idf-hal`) through a simple trait abstraction. +- **Unified Parsing**: Built-in frame parsing for: - **Minimal Packets** (Presence state, Distance) - **Standard/Engineering Packets** (Distance, Signal Energy arrays) - **Firmware Version** & **Serial Number** -- **Configuration Support**: - - Switch between Minimal and Standard/Engineering modes. - - Configure reporting frequency (e.g., 8Hz). - - Configure response speed. -- Automatic caching of last reading if no fresh data is available. +- **Full Configuration**: + - Switch between Minimal and Standard modes. + - Set reporting frequencies, response speeds, and gate thresholds. + - Support for Automatic Calibration. ## πŸ“š Documentation This library implements the protocol defined in the official HLK-LD2410S manuals: -- [HLK-LD2410S Serial Communication Protocol V1.00](./docs/HLK-LD2410S_serial_communication_protocol-V1.00.pdf) (26 Nov 2024) -- [HLK-LD2410S User Manual V1.2](./docs/HLK-LD2410S_User_manual-V1.2.pdf) (20 Apr 2024) +- [HLK-LD2410S Serial Communication Protocol V1.00](./docs/HLK-LD2410S_serial_communication_protocol-V1.00.pdf) +- [HLK-LD2410S User Manual V1.2](./docs/HLK-LD2410S_User_manual-V1.2.pdf) ## πŸ“¦ Installation @@ -30,15 +29,19 @@ Add to your `Cargo.toml`: ```toml [dependencies] -ld2410s = { git = "https://github.com/mvdschee/ld2410s", tag = "v0.1.2", features = ["serial"] } # desktop serialport -# ld2410s = { git = "https://github.com/mvdschee/ld2410s", tag = "v0.1.2", features = ["embedded"] } # embedded ESP-IDF +# Desktop Serial (Sync) +ld2410s = { version = "0.2.0", features = ["serial"] } +# Async Support +# ld2410s = { version = "0.2.0", features = ["async"] } +# Embedded ESP-IDF +# ld2410s = { version = "0.2.0", features = ["embedded"] } ``` ## πŸš€ Examples -### Desktop (via USB-to-UART adapter) +### Desktop (Sync) - cargo run --example desktop --features serial + cargo run --example serial --features serial ```rs let port = serialport::new("/dev/tty.usbserial-123", BAUD_RATE) @@ -46,17 +49,13 @@ ld2410s = { git = "https://github.com/mvdschee/ld2410s", tag = "v0.1.2", feature .open()?; // 1. Initialize sensor (Standard Mode = Engineering Data) - let mut sensor = LD2410S::new(SerialPortWrapper(port), OutputMode::Standard); + // LD2410S::new(UART, TIMER, MODE) + let mut sensor = LD2410S::new(SerialPortWrapper(port), StdTimer::default(), OutputMode::Standard); sensor.init()?; - // 2. Configure for faster updates (8Hz) - sensor.set_distance_frequency(8.0)?; - sensor.set_status_frequency(8.0)?; - sensor.set_response_speed(10)?; - loop { - if let Some(reading) = sensor.read_latest()? { - match reading.data { + if let Ok(Some(packet)) = sensor.next_packet() { + match packet { ld2410s::Packet::Standard(s) => println!("Dist: {} Energy: {:?}", s.distance_cm, s.energy), _ => {} } @@ -64,78 +63,61 @@ ld2410s = { git = "https://github.com/mvdschee/ld2410s", tag = "v0.1.2", feature } ``` -### ESP32 (via `esp-idf-hal`) +### Desktop (Async via Tokio) + + cargo run --example async_serial --features async + +```rs + let port = tokio_serial::new("/dev/tty.usbserial-123", BAUD_RATE).open_native_async()?; + let mut sensor = LD2410SAsync::new(FromTokio::new(port), OutputMode::Standard); + sensor.init().await?; + + loop { + if let Ok(Some(packet)) = sensor.next_packet().await { + // handle packet... + } + } +``` + +### ESP32 (Sync via `esp-idf-hal`) cargo run --example esp --features embedded ```rs - let peripherals = Peripherals::take().unwrap(); - let pins = peripherals.pins; - let cfg = Config::default().baudrate(BAUD_RATE.Hz()); - let uart = UartDriver::new( - peripherals.uart1, - pins.gpio4, - pins.gpio5, - None, None, - &cfg, - )?; - - let mut sensor = LD2410S::new(EspUartWrapper(uart), OutputMode::Standard); + let uart = UartDriver::new(peripherals.uart1, pins.gpio4, pins.gpio5, None, None, &cfg)?; + let mut sensor = LD2410S::new(EspUartWrapper(uart), EspTimer, OutputMode::Standard); sensor.init()?; - sensor.set_distance_frequency(8.0)?; loop { - if let Some(reading) = sensor.read_latest()? { - match reading.data { - ld2410s::Packet::Standard(s) => println!("Dist: {} Energy: {:?}", s.distance_cm, s.energy), - _ => {} - } + if let Ok(Some(packet)) = sensor.next_packet() { + // handle packet... } } ``` -## βš™οΈ Advanced Configuration - -### Manual Thresholds - -You can manually set the sensitivity (energy threshold) for each of the 16 distance gates. - -- **Trigger Threshold**: Energy required to switch from Unoccupied -> Occupied. -- **Hold Threshold**: Energy required to maintain Occupied state. - -```rs -// Set gates 0-15. Lower value = Higher sensitivity. -// Example: High sensitivity for close range (gates 0-2), lower for far (3-15). -let triggers: [u16; 16] = [ - 15, 15, 20, 30, 40, 50, 60, 60, - 60, 60, 60, 60, 60, 60, 60, 60 -]; -sensor.set_trigger_thresholds(&triggers)?; - -// Hold thresholds are usually slightly lower than trigger to prevent flickering -let holds: [u16; 16] = [ - 10, 10, 15, 25, 35, 45, 55, 55, - 55, 55, 55, 55, 55, 55, 55, 55 -]; -sensor.set_hold_thresholds(&holds)?; -``` - -### Automatic Calibration +### ESP32 (Async via `esp-idf-hal`) -Use this **once** during installation with an **empty room**. The sensor will measure background noise and set thresholds automatically. + cargo run --example async_esp --features embedded,async ```rs -// 1. Trigger Factor (added to noise floor for Trigger) -// 2. Retention Factor (added to noise floor for Hold) -// 3. Scanning Time (seconds) -// Example: factor=2, retention=1, scan=120s -sensor.set_auto_threshold(2, 1, 120)?; + let uart = AsyncUartDriver::new(peripherals.uart1, pins.gpio4, pins.gpio5, None, None, &cfg)?; + let mut sensor = LD2410SAsync::new(uart, OutputMode::Standard); + + block_on(async { + sensor.init().await.unwrap(); + loop { + if let Ok(Some(packet)) = sensor.next_packet().await { + // handle packet... + } + } + }) ``` ## βš™οΈ Feature Flags -- `serial` β†’ Use [serialport](https://crates.io/crates/serialport) (desktop/hosted) -- `embedded` β†’ Use [esp-idf-hal](https://crates.io/crates/esp-idf-hal) (ESP32) +- `serial` β†’ Use [serialport](https://crates.io/crates/serialport) (desktop/hosted). +- `embedded` β†’ Use [esp-idf-hal](https://crates.io/crates/esp-idf-hal) (ESP32). +- `async` β†’ Use [embedded-io-async](https://crates.io/crates/embedded-io-async) and [embassy-time](https://crates.io/crates/embassy-time). ## πŸ“ License diff --git a/examples/async_esp.rs b/examples/async_esp.rs new file mode 100644 index 0000000..2f34775 --- /dev/null +++ b/examples/async_esp.rs @@ -0,0 +1,52 @@ +use esp_idf_hal::prelude::*; +use esp_idf_hal::task::block_on; +use esp_idf_hal::uart::{AsyncUartDriver, config::Config}; +use ld2410s::asynchronous::LD2410SAsync; +use ld2410s::{BAUD_RATE, OutputMode, Packet}; + +fn main() -> anyhow::Result<()> { + esp_idf_svc::sys::link_patches(); + + let peripherals = Peripherals::take().unwrap(); + let pins = peripherals.pins; + let tx = pins.gpio4; + let rx = pins.gpio5; + let cfg = Config::default().baudrate(BAUD_RATE.Hz()); + + // Initialize Async UART + let uart = AsyncUartDriver::new( + peripherals.uart1, + tx, + rx, + Option::::None, + Option::::None, + &cfg, + )?; + + // Create Async Driver + // AsyncUartDriver implements embedded_io_async::Read and Write + let mut sensor = LD2410SAsync::new(uart, OutputMode::Standard); + + // Run the async task + block_on(async { + println!("Initializing sensor..."); + sensor.init().await.expect("Failed to init"); + + println!("Configuring sensor..."); + sensor.set_distance_frequency(8.0).await.expect("Failed to config"); + sensor.set_status_frequency(8.0).await.expect("Failed to config"); + sensor.set_response_speed(10).await.expect("Failed to config"); + + loop { + match sensor.next_packet().await { + Ok(Some(packet)) => match packet { + Packet::Minimal(m) => println!("Minimal: {:?}", m), + Packet::Standard(s) => println!("Standard: {:?}", s), + _ => {} + }, + Ok(None) => {} + Err(e) => println!("Error: {:?}", e), + } + } + }) +} diff --git a/examples/async_serial.rs b/examples/async_serial.rs new file mode 100644 index 0000000..f6eb5ea --- /dev/null +++ b/examples/async_serial.rs @@ -0,0 +1,57 @@ +use embedded_io_adapters::tokio_1::FromTokio; +use ld2410s::asynchronous::LD2410SAsync; +use ld2410s::{BAUD_RATE, OutputMode, Packet}; +use std::time::Duration; +use tokio_serial::SerialPortBuilderExt; + +#[tokio::main] +async fn main() -> anyhow::Result<()> { + let port_name = "/dev/tty.usbserial-130"; // Adjust to your port + + let mut port = tokio_serial::new(port_name, BAUD_RATE).open_native_async()?; + + #[cfg(unix)] + port.set_exclusive(false)?; + + // Wrap the tokio serial port in an adapter that implements embedded-io-async traits + let embedded_port = FromTokio::new(port); + + // Create the async driver + let mut sensor = LD2410SAsync::new(embedded_port, OutputMode::Standard); + + println!("Initializing sensor..."); + sensor.init().await.expect("Failed to initialize sensor"); + + println!("Configuring sensor..."); + sensor.set_distance_frequency(8.0).await.map_err(|e| anyhow::anyhow!("{:?}", e))?; + sensor.set_status_frequency(8.0).await.map_err(|e| anyhow::anyhow!("{:?}", e))?; + sensor.set_response_speed(10).await.map_err(|e| anyhow::anyhow!("{:?}", e))?; + + println!("Reading firmware version..."); + match sensor.read_firmware_version().await { + Ok(v) => println!( + "Firmware: Type={:08X} VerType={:04X} v{}.{}.{}", + v.equipment_type, v.version_type, v.major, v.minor, v.patch + ), + Err(e) => eprintln!("Error reading firmware: {:?}", e), + } + + println!("Starting poll loop..."); + loop { + // Read packets asynchronously + match sensor.next_packet().await { + Ok(Some(packet)) => match packet { + Packet::Minimal(m) => println!("Minimal: {:?}", m), + Packet::Standard(s) => println!("Standard: {:?}", s), + Packet::Ack(ack) => println!("ACK: {:?}", ack), + Packet::Firmware(v) => println!("Firmware: {:?}", v), + Packet::SerialNumber(sn) => println!("Serial Number: {:?}", sn), + }, + Ok(None) => {} + Err(e) => eprintln!("UART Error: {:?}", e), + } + + // Yield to let other tasks run (not strictly needed with await, but good practice in loops) + tokio::time::sleep(Duration::from_millis(10)).await; + } +} diff --git a/examples/desktop.rs b/examples/desktop.rs deleted file mode 100644 index 0193265..0000000 --- a/examples/desktop.rs +++ /dev/null @@ -1,70 +0,0 @@ -use ld2410s::{BAUD_RATE, LD2410S, OutputMode, uart::SerialPortWrapper}; -use std::time::Duration; - -fn main() { - let port = serialport::new("/dev/tty.usbserial-123", BAUD_RATE) - .timeout(Duration::from_millis(50)) - .open() - .unwrap(); - - let mut sensor = LD2410S::new(SerialPortWrapper(port), OutputMode::Standard); - - // 1. Initialize the sensor. - // This sends the "Switch to Standard Output" command (0x007A) if Standard mode is requested. - println!("Initializing sensor..."); - let _ = sensor.init(); - - // 2. (Optional) Configure sensor parameters for faster updates. - // The sensor defaults to ~1Hz. We set it to 8Hz here for smoother tracking. - // Response speed 10 = Fast (default is 5 = Normal). - println!("Configuring sensor..."); - let _ = sensor.set_distance_frequency(8.0); - let _ = sensor.set_status_frequency(8.0); - let _ = sensor.set_response_speed(10); - - // 3. Read diagnostic info (Firmware Version & Serial Number). - println!("Reading firmware version..."); - match sensor.read_firmware_version() { - Ok(Some(v)) => println!( - "Firmware: Type={:08X} VerType={:04X} v{}.{}.{}", - v.equipment_type, v.version_type, v.major, v.minor, v.patch - ), - Ok(None) => println!("Firmware version: timeout"), - Err(e) => eprintln!("Error reading firmware: {:?}", e), - } - - println!("Reading serial number..."); - match sensor.read_serial_number() { - Ok(Some(sn)) => { - let sn_str = String::from_utf8_lossy(&sn); - println!("Serial Number: {}", sn_str); - } - Ok(None) => println!("Serial Number: timeout"), - Err(e) => eprintln!("Error reading serial number: {:?}", e), - } - - loop { - // 4. Main Loop: Read latest data packets. - if let Some(reading) = sensor.read_latest().unwrap() { - let prefix = if reading.fresh { - "[FRESH]" - } else { - "[CACHED]" - }; - match &reading.data { - ld2410s::Packet::Minimal(m) => println!("{} Minimal: {:?}", prefix, m), - ld2410s::Packet::Standard(s) => println!("{} Standard: {:?}", prefix, s), - ld2410s::Packet::Ack(ack) => println!("{} ACK: {:?}", prefix, ack), - ld2410s::Packet::Firmware(v) => { - println!( - "{} Firmware: Type={:08X} v{}.{}.{}", - prefix, v.equipment_type, v.major, v.minor, v.patch - ) - } - ld2410s::Packet::SerialNumber(sn) => println!("{} Serial Number: {:?}", prefix, sn), - } - } - - std::thread::sleep(Duration::from_millis(100)); - } -} diff --git a/examples/esp.rs b/examples/esp.rs index 47d0be8..328c84c 100644 --- a/examples/esp.rs +++ b/examples/esp.rs @@ -1,10 +1,13 @@ +use core::time::Duration; use esp_idf_hal::prelude::*; use esp_idf_hal::uart::{UartDriver, config::Config}; use ld2410s::OutputMode; -use ld2410s::{BAUD_RATE, LD2410S, uart::EspUartWrapper}; -use std::time::Duration; +use ld2410s::{ + BAUD_RATE, LD2410S, + uart::{EspTimer, EspUartWrapper}, +}; -fn main() { +fn main() -> anyhow::Result<()> { // esp_idf_svc::log::EspLogger::initialize_default(); // optional logging let peripherals = Peripherals::take().unwrap(); @@ -14,7 +17,7 @@ fn main() { let tx = pins.gpio4; // ESP TX -> LD2410S RX let rx = pins.gpio5; // ESP RX -> LD2410S TX - let cfg = Config::default().baudrate(BAUD_RATE); + let cfg = Config::default().baudrate(BAUD_RATE.Hz()); // RTS/CTS unused: let uart = UartDriver::new( @@ -26,40 +29,38 @@ fn main() { &cfg, )?; - // Optional: set a short RX timeout so read() returns quickly - // (API exists on recent esp-idf-hal; if not in your version, you can skip) - // uart.set_rx_timeout(Duration::from_millis(20))?; - - let mut sensor = LD2410S::new(EspUartWrapper(uart), OutputMode::Standard); + // LD2410S now requires a Timer implementation (EspTimer for ESP32) + let mut sensor = LD2410S::new(EspUartWrapper(uart), EspTimer, OutputMode::Standard); // 1. Initialize the sensor (switches to the requested OutputMode). - // This might take a second as it sends commands and waits for ACKs. + println!("Initializing sensor..."); if let Err(e) = sensor.init() { println!("Failed to init sensor: {:?}", e); } // 2. (Optional) Configure sensor parameters. - // By default, the sensor might report at 1Hz. Here we set it to 8Hz for faster updates. - // We also set the response speed to 'Fast' (10) to make it more reactive. println!("Configuring sensor parameters..."); let _ = sensor.set_distance_frequency(8.0); let _ = sensor.set_status_frequency(8.0); let _ = sensor.set_response_speed(10); loop { - // 3. Read the latest data. - // read_latest() tries to fetch a fresh packet. If no fresh packet arrives within poll_timeout, - // it returns the last cached packet (fresh=false). - if let Some(r) = sensor.read_latest().unwrap() { - if let Some(m) = r.data.as_minimal() { - println!("Minimal (Fresh: {}): {:?}", r.fresh, m); - } + // 3. Read packets. + match sensor.next_packet() { + Ok(Some(packet)) => { + if let Some(m) = packet.as_minimal() { + println!("Minimal: {:?}", m); + } - if let Some(s) = r.data.as_standard() { - println!("Standard (Fresh: {}): {:?}", r.fresh, s); + if let Some(s) = packet.as_standard() { + println!("Standard: {:?}", s); + } } + Ok(None) => {} + Err(e) => println!("UART Error: {:?}", e), } - // small delay to keep loop polite; adjust to your needs - std::thread::sleep(Duration::from_millis(100)); + + // small delay to keep loop polite + esp_idf_hal::delay::FreeRtos::delay_ms(10); } } diff --git a/examples/serial.rs b/examples/serial.rs new file mode 100644 index 0000000..2e227c2 --- /dev/null +++ b/examples/serial.rs @@ -0,0 +1,70 @@ +use ld2410s::{ + BAUD_RATE, LD2410S, OutputMode, + uart::{SerialPortWrapper, StdTimer}, +}; +use std::time::Duration; + +fn main() { + let port = serialport::new("/dev/tty.usbserial-130", BAUD_RATE) + .timeout(Duration::from_millis(50)) + .open() + .expect("Failed to open serial port"); + + // Now requires a Timer implementation (StdTimer for desktop) + let mut sensor = + LD2410S::new(SerialPortWrapper(port), StdTimer::default(), OutputMode::Standard); + + // 1. Initialize the sensor. + println!("Initializing sensor..."); + sensor.init().expect("Failed to initialize sensor"); + + // 2. Configure sensor parameters for faster updates. + println!("Configuring sensor..."); + let _ = sensor.set_distance_frequency(8.0); + let _ = sensor.set_status_frequency(8.0); + let _ = sensor.set_response_speed(10); + + // 3. Read diagnostic info (Firmware Version & Serial Number). + println!("Reading firmware version..."); + match sensor.read_firmware_version() { + Ok(v) => println!( + "Firmware: Type={:08X} VerType={:04X} v{}.{}.{}", + v.equipment_type, v.version_type, v.major, v.minor, v.patch + ), + Err(e) => eprintln!("Error reading firmware: {:?}", e), + } + + println!("Reading serial number..."); + match sensor.read_serial_number() { + Ok(sn) => { + let sn_str = String::from_utf8_lossy(&sn); + println!("Serial Number: {}", sn_str); + } + Err(e) => eprintln!("Error reading serial number: {:?}", e), + } + + loop { + // 4. Main Loop: Read packets. + // next_packet() is now a non-blocking poll of the UART. + match sensor.next_packet() { + Ok(Some(packet)) => match packet { + ld2410s::Packet::Minimal(m) => println!("Minimal: {:?}", m), + ld2410s::Packet::Standard(s) => println!("Standard: {:?}", s), + ld2410s::Packet::Ack(ack) => println!("ACK: {:?}", ack), + ld2410s::Packet::Firmware(v) => { + println!( + "Firmware: Type={:08X} v{}.{}.{}", + v.equipment_type, v.major, v.minor, v.patch + ) + } + ld2410s::Packet::SerialNumber(sn) => println!("Serial Number: {:?}", sn), + }, + Ok(None) => { + // No packet ready yet + } + Err(e) => eprintln!("UART Error: {:?}", e), + } + + std::thread::sleep(Duration::from_millis(10)); + } +} diff --git a/src/asynchronous.rs b/src/asynchronous.rs new file mode 100644 index 0000000..3897b4d --- /dev/null +++ b/src/asynchronous.rs @@ -0,0 +1,200 @@ +use crate::OutputMode; +use crate::commands; +use crate::device::Config; +use crate::error::Error; +use crate::protocol::Protocol; +use crate::types::{FirmwareVersion, Packet}; +use core::time::Duration as CoreDuration; +use embassy_time::{Duration, TimeoutError, Timer, with_timeout}; +use embedded_io_async::{Read, Write}; +use heapless::Vec; + +/// Asynchronous driver for LD2410S +pub struct LD2410SAsync { + uart: U, + protocol: Protocol, + output_mode: OutputMode, + config: Config, +} + +impl LD2410SAsync +where + U: Read + Write, +{ + pub fn new(uart: U, output_mode: OutputMode) -> Self { + Self { + uart, + protocol: Protocol::new(), + output_mode, + config: Config::default(), + } + } + + pub fn set_config(&mut self, config: Config) { + self.config = config; + } + + fn to_embassy_duration(d: CoreDuration) -> Duration { + Duration::from_micros(d.as_micros() as u64) + } + + pub async fn init(&mut self) -> Result<(), Error> { + let cmd = commands::switch_output_frame(self.output_mode); + self.run_config_transaction(&cmd).await + } + + async fn run_config_transaction(&mut self, cmd: &[u8]) -> Result<(), Error> { + self.send_and_wait_ack(&commands::enter_config_frame()).await?; + Timer::after(Self::to_embassy_duration(self.config.config_mode_delay)).await; + + self.send_and_wait_ack(cmd).await?; + Timer::after(Self::to_embassy_duration(self.config.config_mode_delay)).await; + + self.send_and_wait_ack(&commands::exit_config_frame()).await?; + Timer::after(Self::to_embassy_duration(self.config.config_mode_delay)).await; + Ok(()) + } + + async fn send_and_wait_ack(&mut self, cmd: &[u8]) -> Result<(), Error> { + self.uart.write_all(cmd).await.map_err(Error::Uart)?; + self.uart.flush().await.map_err(Error::Uart)?; + + let res = with_timeout(Self::to_embassy_duration(self.config.command_timeout), async { + loop { + match self.next_packet().await? { + Some(Packet::Ack(ack)) => { + return if ack.success { + Ok(()) + } else { + Err(Error::CommandFailed) + }; + } + Some(_) => { + // Received a non-ACK packet (e.g. data). + // Process the next packet immediately; don't sleep. + continue; + } + None => { + // No packet available yet. Yield/sleep briefly. + Timer::after(Duration::from_millis(5)).await; + } + } + } + }) + .await; + + match res { + Ok(inner) => inner, + Err(TimeoutError) => Err(Error::Timeout), + } + } + + pub async fn next_packet(&mut self) -> Result, Error> { + if let Some(p) = self.protocol.next_packet() { + return Ok(Some(p)); + } + + let mut buf = [0u8; 128]; + let n = self.uart.read(&mut buf).await.map_err(Error::Uart)?; + if n > 0 { + self.protocol.process_bytes(&buf[..n]); + } + + Ok(self.protocol.next_packet()) + } + + pub async fn read_firmware_version(&mut self) -> Result> { + self.send_and_wait_ack(&commands::enter_config_frame()).await?; + Timer::after(Self::to_embassy_duration(self.config.config_mode_delay)).await; + + let frame = commands::read_firmware_frame(); + self.uart.write_all(&frame).await.map_err(Error::Uart)?; + self.uart.flush().await.map_err(Error::Uart)?; + + let res = with_timeout(Self::to_embassy_duration(self.config.command_timeout), async { + loop { + match self.next_packet().await? { + Some(Packet::Firmware(v)) => return Ok::>(v), + Some(_) => continue, + None => Timer::after(Duration::from_millis(5)).await, + } + } + }) + .await; + + let result = match res { + Ok(inner) => inner?, + Err(TimeoutError) => return Err(Error::Timeout), + }; + + self.send_and_wait_ack(&commands::exit_config_frame()).await?; + Timer::after(Self::to_embassy_duration(self.config.config_mode_delay)).await; + + Ok(result) + } + + pub async fn set_distance_frequency(&mut self, hz: f32) -> Result<(), Error> { + let val = (hz * 10.0) as u16; + let cmd = commands::set_distance_freq_x10_frame(val); + self.run_config_transaction(&cmd).await + } + + pub async fn set_status_frequency(&mut self, hz: f32) -> Result<(), Error> { + let val = (hz * 10.0) as u16; + let cmd = commands::set_status_freq_x10_frame(val); + self.run_config_transaction(&cmd).await + } + + pub async fn set_response_speed(&mut self, speed: u16) -> Result<(), Error> { + let cmd = commands::set_response_speed_frame(speed); + self.run_config_transaction(&cmd).await + } + + pub async fn set_trigger_thresholds( + &mut self, + thresholds: &[u16; 16], + ) -> Result<(), Error> { + let cmd = commands::write_trigger_threshold_frame(thresholds); + self.run_config_transaction(&cmd).await + } + + pub async fn set_hold_thresholds( + &mut self, + thresholds: &[u16; 16], + ) -> Result<(), Error> { + let cmd = commands::write_hold_threshold_frame(thresholds); + self.run_config_transaction(&cmd).await + } + + pub async fn read_serial_number(&mut self) -> Result, Error> { + self.send_and_wait_ack(&commands::enter_config_frame()).await?; + Timer::after(Self::to_embassy_duration(self.config.config_mode_delay)).await; + + let frame = commands::read_serial_frame(); + self.uart.write_all(&frame).await.map_err(Error::Uart)?; + self.uart.flush().await.map_err(Error::Uart)?; + + let res = with_timeout(Self::to_embassy_duration(self.config.command_timeout), async { + loop { + match self.next_packet().await? { + Some(Packet::SerialNumber(sn)) => { + return Ok::, Error>(sn); + } + Some(_) => continue, + None => Timer::after(Duration::from_millis(5)).await, + } + } + }) + .await; + + let result = match res { + Ok(inner) => inner?, + Err(TimeoutError) => return Err(Error::Timeout), + }; + + self.send_and_wait_ack(&commands::exit_config_frame()).await?; + Timer::after(Self::to_embassy_duration(self.config.config_mode_delay)).await; + + Ok(result) + } +} diff --git a/src/commands.rs b/src/commands.rs index 7efc070..d5bc4d8 100644 --- a/src/commands.rs +++ b/src/commands.rs @@ -1,7 +1,11 @@ #![allow(dead_code)] use crate::types::{CMD_HEAD, CMD_TAIL, OutputMode}; -use std::vec::Vec; +use heapless::Vec; + +// Max size for a command frame. Most are < 20 bytes. +// 256 is generous to allow for batch parameter writes if needed. +pub const MAX_CMD_SIZE: usize = 256; // ---- Command words (from the PDF) ---- const CMD_READ_FIRMWARE: u16 = 0x0000; @@ -17,76 +21,78 @@ const CMD_WRITE_TRIGGER_THRESHOLD: u16 = 0x0072; const CMD_READ_TRIGGER_THRESHOLD: u16 = 0x0073; const CMD_WRITE_HOLD_THRESHOLD: u16 = 0x0076; const CMD_READ_HOLD_THRESHOLD: u16 = 0x0077; - const CMD_ENABLE_ENG_MODE: u16 = 0x0062; /// Build: HEAD | len(u16 LE) | cmd(u16 LE) | params... | TAIL -fn build_cmd(cmd: u16, params: &[u8]) -> Vec { +fn build_cmd(cmd: u16, params: &[u8]) -> Vec { let data_len = 2 + params.len(); // cmd(2) + params - let mut out = Vec::with_capacity(4 + 2 + data_len + 4); + let mut out = Vec::::new(); + + // We use expect here because MAX_CMD_SIZE is sized to fit any valid protocol command. + // If this panics, it's a developer error in sizing the buffer. // HEAD - out.extend_from_slice(&CMD_HEAD); + out.extend_from_slice(&CMD_HEAD).expect("Cmd buffer overflow"); // LEN - out.push((data_len & 0xFF) as u8); - out.push(((data_len >> 8) & 0xFF) as u8); + out.push((data_len & 0xFF) as u8).expect("Cmd buffer overflow"); + out.push(((data_len >> 8) & 0xFF) as u8).expect("Cmd buffer overflow"); // CMD (LE) - out.push((cmd & 0xFF) as u8); - out.push(((cmd >> 8) & 0xFF) as u8); + out.push((cmd & 0xFF) as u8).expect("Cmd buffer overflow"); + out.push(((cmd >> 8) & 0xFF) as u8).expect("Cmd buffer overflow"); // PARAMS - out.extend_from_slice(params); + out.extend_from_slice(params).expect("Cmd buffer overflow"); // TAIL - out.extend_from_slice(&CMD_TAIL); + out.extend_from_slice(&CMD_TAIL).expect("Cmd buffer overflow"); out } // ---- High-level helpers built on build_cmd ---- -pub fn read_firmware_frame() -> Vec { +pub fn read_firmware_frame() -> Vec { build_cmd(CMD_READ_FIRMWARE, &[]) } -pub fn enter_config_frame() -> Vec { +pub fn enter_config_frame() -> Vec { // 0x00FF with param 0x0001 build_cmd(CMD_ENABLE_CONFIG, &[0x01, 0x00]) } -pub fn exit_config_frame() -> Vec { +pub fn exit_config_frame() -> Vec { build_cmd(CMD_END_CONFIG, &[]) } -pub fn read_serial_frame() -> Vec { +pub fn read_serial_frame() -> Vec { build_cmd(CMD_READ_SERIAL, &[]) } -pub fn switch_output_frame(mode: OutputMode) -> Vec { +pub fn switch_output_frame(mode: OutputMode) -> Vec { build_cmd(CMD_SWITCH_OUTPUT, &mode.as_six_bytes()) } /// Write generic/common parameters (pairs of (word, value_u32 LE)) /// Example: set distance report freq (word 0x000C) to 8.0 Hz -> value = 80 (Hz*10) -pub fn write_generic_params_frame(pairs: &[(u16, u32)]) -> Vec { - let mut params: Vec = Vec::new(); +pub fn write_generic_params_frame(pairs: &[(u16, u32)]) -> Vec { + let mut params: Vec = Vec::new(); for &(word, value) in pairs { // word (u16 LE) - params.push((word & 0xFF) as u8); - params.push((word >> 8) as u8); + params.push((word & 0xFF) as u8).unwrap(); + params.push((word >> 8) as u8).unwrap(); // value (u32 LE) - params.push((value & 0xFF) as u8); - params.push(((value >> 8) & 0xFF) as u8); - params.push(((value >> 16) & 0xFF) as u8); - params.push(((value >> 24) & 0xFF) as u8); + params.push((value & 0xFF) as u8).unwrap(); + params.push(((value >> 8) & 0xFF) as u8).unwrap(); + params.push(((value >> 16) & 0xFF) as u8).unwrap(); + params.push(((value >> 24) & 0xFF) as u8).unwrap(); } build_cmd(CMD_WRITE_GENERIC, ¶ms) } /// Read generic/common parameters for a list of words -pub fn read_generic_params_frame(words: &[u16]) -> Vec { - let mut params: Vec = Vec::new(); +pub fn read_generic_params_frame(words: &[u16]) -> Vec { + let mut params: Vec = Vec::new(); for &w in words { - params.push((w & 0xFF) as u8); - params.push((w >> 8) as u8); + params.push((w & 0xFF) as u8).unwrap(); + params.push((w >> 8) as u8).unwrap(); } build_cmd(CMD_READ_GENERIC, ¶ms) } @@ -95,59 +101,61 @@ pub fn auto_threshold_frame( trigger_factor: u16, retention_factor: u16, scanning_time: u16, -) -> Vec { - let mut params = Vec::new(); - params.extend_from_slice(&[0x02, 0x00]); // Trigger factor word - params.push((trigger_factor & 0xFF) as u8); - params.push((trigger_factor >> 8) as u8); +) -> Vec { + let mut params: Vec = Vec::new(); + params.extend_from_slice(&[0x02, 0x00]).unwrap(); // Trigger factor word + params.push((trigger_factor & 0xFF) as u8).unwrap(); + params.push((trigger_factor >> 8) as u8).unwrap(); - params.extend_from_slice(&[0x01, 0x00]); // Retention factor word - params.push((retention_factor & 0xFF) as u8); - params.push((retention_factor >> 8) as u8); + params.extend_from_slice(&[0x01, 0x00]).unwrap(); // Retention factor word + params.push((retention_factor & 0xFF) as u8).unwrap(); + params.push((retention_factor >> 8) as u8).unwrap(); - params.extend_from_slice(&[0x78, 0x00]); // Scanning time word - params.push((scanning_time & 0xFF) as u8); - params.push((scanning_time >> 8) as u8); + params.extend_from_slice(&[0x78, 0x00]).unwrap(); // Scanning time word + params.push((scanning_time & 0xFF) as u8).unwrap(); + params.push((scanning_time >> 8) as u8).unwrap(); build_cmd(CMD_AUTO_THRESHOLD, ¶ms) } -fn build_threshold_params(thresholds: &[u16; 16]) -> Vec { - let mut params = Vec::with_capacity(16 * 6); +fn build_threshold_params(thresholds: &[u16; 16]) -> Vec { + let mut params: Vec = Vec::new(); for (i, &val) in thresholds.iter().enumerate() { let word = i as u16; - params.push((word & 0xFF) as u8); - params.push((word >> 8) as u8); - params.push((val & 0xFF) as u8); - params.push(((val >> 8) & 0xFF) as u8); - params.push(0); - params.push(0); + params.push((word & 0xFF) as u8).unwrap(); + params.push((word >> 8) as u8).unwrap(); + params.push((val & 0xFF) as u8).unwrap(); + params.push(((val >> 8) & 0xFF) as u8).unwrap(); + params.push(0).unwrap(); + params.push(0).unwrap(); } + params } -pub fn write_trigger_threshold_frame(thresholds: &[u16; 16]) -> Vec { +pub fn write_trigger_threshold_frame(thresholds: &[u16; 16]) -> Vec { build_cmd(CMD_WRITE_TRIGGER_THRESHOLD, &build_threshold_params(thresholds)) } -pub fn read_trigger_threshold_frame() -> Vec { - let mut params = Vec::new(); +pub fn read_trigger_threshold_frame() -> Vec { + let mut params: Vec = Vec::new(); for i in 0..16u16 { - params.push((i & 0xFF) as u8); - params.push((i >> 8) as u8); + params.push((i & 0xFF) as u8).unwrap(); + params.push((i >> 8) as u8).unwrap(); } + build_cmd(CMD_READ_TRIGGER_THRESHOLD, ¶ms) } -pub fn write_hold_threshold_frame(thresholds: &[u16; 16]) -> Vec { +pub fn write_hold_threshold_frame(thresholds: &[u16; 16]) -> Vec { build_cmd(CMD_WRITE_HOLD_THRESHOLD, &build_threshold_params(thresholds)) } -pub fn read_hold_threshold_frame() -> Vec { - let mut params = Vec::new(); +pub fn read_hold_threshold_frame() -> Vec { + let mut params: Vec = Vec::new(); for i in 0..16u16 { - params.push((i & 0xFF) as u8); - params.push((i >> 8) as u8); + params.push((i & 0xFF) as u8).unwrap(); + params.push((i >> 8) as u8).unwrap(); } build_cmd(CMD_READ_HOLD_THRESHOLD, ¶ms) } @@ -156,17 +164,17 @@ pub fn read_hold_threshold_frame() -> Vec { /// Build a frame to set the Standard "distance reporting" frequency (word 0x000C). /// Pass Hz*10, e.g. 80 = 8.0 Hz. Use inside ENTER/EXIT CONFIG. -pub fn set_distance_freq_x10_frame(x10_hz: u16) -> Vec { +pub fn set_distance_freq_x10_frame(x10_hz: u16) -> Vec { write_generic_params_frame(&[(0x000C, x10_hz as u32)]) } /// Set status reporting frequency (word 0x0002). -pub fn set_status_freq_x10_frame(x10_hz: u16) -> Vec { +pub fn set_status_freq_x10_frame(x10_hz: u16) -> Vec { write_generic_params_frame(&[(0x0002, x10_hz as u32)]) } /// Set response speed (word 0x000B). /// 5 = Normal, 10 = Fast. -pub fn set_response_speed_frame(speed: u16) -> Vec { +pub fn set_response_speed_frame(speed: u16) -> Vec { write_generic_params_frame(&[(0x000B, speed as u32)]) } diff --git a/src/device.rs b/src/device.rs index 64fbd4c..6b532fb 100644 --- a/src/device.rs +++ b/src/device.rs @@ -1,17 +1,14 @@ -use crate::parser::{parse_command_responses, parse_standard_frames}; -use crate::types::{CMD_HEAD, FirmwareVersion, MINIMAL_HEAD, Packet, STANDARD_HEAD}; +use crate::commands; +use crate::error::Error; +use crate::protocol::Protocol; +use crate::types::{FirmwareVersion, Packet}; use crate::uart::UartInterface; -use crate::{OutputMode, parser::parse_minimal_frames}; -use crate::{Reading, commands}; -use heapless::{Deque, Vec}; -use std::thread::sleep; -use std::time::{Duration, Instant}; - -const STREAM_BUF_CAP: usize = 1024; // ring buffer size +use crate::{OutputMode, Timer}; +use core::time::Duration; +use heapless::Vec; #[derive(Debug, Clone, Copy)] pub struct Config { - pub poll_timeout: Duration, pub command_timeout: Duration, pub config_mode_delay: Duration, } @@ -19,32 +16,32 @@ pub struct Config { impl Default for Config { fn default() -> Self { Self { - poll_timeout: Duration::from_millis(100), command_timeout: Duration::from_millis(1000), config_mode_delay: Duration::from_millis(250), } } } -/// Driver owning a UART interface -pub struct LD2410S { +/// Driver owning a UART interface and a Timer +pub struct LD2410S { uart: U, - stream_buf: Deque, - last: Option, + timer: T, + protocol: Protocol, output_mode: OutputMode, config: Config, } -impl LD2410S +impl LD2410S where U: UartInterface, + T: Timer, { /// Create a new LD2410S driver. - pub fn new(uart: U, output_mode: OutputMode) -> Self { + pub fn new(uart: U, timer: T, output_mode: OutputMode) -> Self { Self { uart, - stream_buf: Deque::new(), - last: None, + timer, + protocol: Protocol::new(), output_mode, config: Config::default(), } @@ -55,38 +52,40 @@ where } /// Initialize the device by switching to the desired output mode. - /// This does NOT set frequencies or speeds; use the setters for that. - pub fn init(&mut self) -> Result<(), U::Error> { + pub fn init(&mut self) -> Result<(), Error> { let cmd = commands::switch_output_frame(self.output_mode); self.run_config_transaction(&cmd) } - pub fn set_distance_frequency(&mut self, hz: f32) -> Result<(), U::Error> { + pub fn set_distance_frequency(&mut self, hz: f32) -> Result<(), Error> { let val = (hz * 10.0) as u16; let cmd = commands::set_distance_freq_x10_frame(val); self.run_config_transaction(&cmd) } - pub fn set_status_frequency(&mut self, hz: f32) -> Result<(), U::Error> { + pub fn set_status_frequency(&mut self, hz: f32) -> Result<(), Error> { let val = (hz * 10.0) as u16; let cmd = commands::set_status_freq_x10_frame(val); self.run_config_transaction(&cmd) } /// Set response speed: 5 (Normal) or 10 (Fast) - pub fn set_response_speed(&mut self, speed: u16) -> Result<(), U::Error> { + pub fn set_response_speed(&mut self, speed: u16) -> Result<(), Error> { let cmd = commands::set_response_speed_frame(speed); self.run_config_transaction(&cmd) } /// Set trigger thresholds for all 16 gates. - pub fn set_trigger_thresholds(&mut self, thresholds: &[u16; 16]) -> Result<(), U::Error> { + pub fn set_trigger_thresholds( + &mut self, + thresholds: &[u16; 16], + ) -> Result<(), Error> { let cmd = commands::write_trigger_threshold_frame(thresholds); self.run_config_transaction(&cmd) } /// Set hold (maintenance) thresholds for all 16 gates. - pub fn set_hold_thresholds(&mut self, thresholds: &[u16; 16]) -> Result<(), U::Error> { + pub fn set_hold_thresholds(&mut self, thresholds: &[u16; 16]) -> Result<(), Error> { let cmd = commands::write_hold_threshold_frame(thresholds); self.run_config_transaction(&cmd) } @@ -97,273 +96,107 @@ where trigger_factor: u16, retention_factor: u16, scanning_time: u16, - ) -> Result<(), U::Error> { + ) -> Result<(), Error> { let cmd = commands::auto_threshold_frame(trigger_factor, retention_factor, scanning_time); self.run_config_transaction(&cmd) } /// Helper to wrap a command in Enter/Exit config mode frames - fn run_config_transaction(&mut self, cmd: &[u8]) -> Result<(), U::Error> { + fn run_config_transaction(&mut self, cmd: &[u8]) -> Result<(), Error> { self.send_and_wait_ack(&commands::enter_config_frame())?; - sleep(self.config.config_mode_delay); + self.timer.sleep(self.config.config_mode_delay); self.send_and_wait_ack(cmd)?; - sleep(self.config.config_mode_delay); + self.timer.sleep(self.config.config_mode_delay); self.send_and_wait_ack(&commands::exit_config_frame())?; - sleep(self.config.config_mode_delay); + self.timer.sleep(self.config.config_mode_delay); Ok(()) } - fn send_and_wait_ack(&mut self, cmd: &[u8]) -> Result { - self.uart.write_all(cmd)?; - let start = Instant::now(); - while start.elapsed() < self.config.command_timeout { - let packets = self.poll(256)?; - for p in packets { + fn send_and_wait_ack(&mut self, cmd: &[u8]) -> Result<(), Error> { + self.uart.write_all(cmd).map_err(Error::Uart)?; + let start = self.timer.now(); + while self.timer.now() - start < self.config.command_timeout { + if let Some(p) = self.next_packet()? { if let Packet::Ack(ack) = p { - // We could check if ack.command matches cmd, but cmd parsing is tricky here - // without decoding the input cmd. For init sequence, any ACK is likely good enough - // or we assume it's for the last command. - return Ok(ack.success); - } - } - sleep(Duration::from_millis(10)); - } - Ok(false) // Timeout - } - - /// Non-blocking poll: reads whatever is available now, parses frames, updates cache. - pub fn poll(&mut self, read_chunk: usize) -> Result, U::Error> { - let mut read_buffer = [0u8; 256]; - let bytes_to_read = read_buffer.len().min(read_chunk); - let bytes_read = self.uart.read(&mut read_buffer[..bytes_to_read])?; - - for &byte in &read_buffer[..bytes_read] { - if self.stream_buf.push_back(byte).is_err() { - let _ = self.stream_buf.pop_front(); - let _ = self.stream_buf.push_back(byte); - } - } - - let mut packets: Vec = Vec::new(); - - // Parse from the stream buffer continuously until we can't anymore - loop { - if self.stream_buf.len() < 4 { - break; - } - - // Snapshot the beginning of the buffer to check headers - let mut head = [0u8; 4]; - let len = self.stream_buf.len(); - - for (i, byte) in self.stream_buf.iter().take(4.min(len)).enumerate() { - head[i] = *byte; - } - - let mut parsed_packets = Vec::::new(); // small temporary batch - - // 1. Check for Command/ACK Header - let consumed = if head == CMD_HEAD { - // Copy relevant part to contiguous slice for parser - let snapshot_len = len.min(256); // Limit to reasonable frame size check - let mut snapshot = [0u8; 256]; - - for (i, byte) in self.stream_buf.iter().take(snapshot_len).enumerate() { - snapshot[i] = *byte; - } - - let (p, c) = parse_command_responses(&snapshot[..snapshot_len]); - - if c > 0 { - for pkt in p { - let _ = parsed_packets.push(pkt); - } - - c - } else { - // Header matches but incomplete frame, wait for more data - break; - } - } - // 2. Check for Standard Header - else if head == STANDARD_HEAD { - let snapshot_len = len.min(256); - let mut snapshot = [0u8; 256]; - - for (i, byte) in self.stream_buf.iter().take(snapshot_len).enumerate() { - snapshot[i] = *byte; - } - - let (p, c) = parse_standard_frames(&snapshot[..snapshot_len]); - - if c > 0 { - for pkt in p { - let _ = parsed_packets.push(pkt); - } - - c - } else { - break; - } - } - // 3. Check for Minimal Header (1 byte 0x6E) - else if head[0] == MINIMAL_HEAD { - let snapshot_len = len.min(16); // Minimal is only 5 bytes - let mut snapshot = [0u8; 16]; - - for (i, byte) in self.stream_buf.iter().take(snapshot_len).enumerate() { - snapshot[i] = *byte; - } - - let (p, c) = parse_minimal_frames(&snapshot[..snapshot_len]); - - if c > 0 { - for pkt in p { - let _ = parsed_packets.push(pkt); - } - - c - } else { - // Might be incomplete minimal frame - if len < 5 { - break; + return if ack.success { + Ok(()) } else { - // Malformed? parse_minimal_frames usually consumes if it finds valid frame. - // If it returned 0 consumed but we have >5 bytes, it's not a valid frame starting here. - 1 // Skip the 0x6E and try again - } - } - } - // 4. Unknown byte - else { - 1 - }; - - // Apply consumption - for _ in 0..consumed { - let _ = self.stream_buf.pop_front(); - } - - // Add parsed packets to result - for pkt in parsed_packets { - let _ = packets.push(pkt); - } - - // If we didn't consume anything, break to avoid infinite loop on partial data - if consumed == 0 { - break; - } - } - - // Update last known state if data packet - if let Some(last_packet) = packets.last().cloned() { - match last_packet { - Packet::Minimal(_) | Packet::Standard(_) => { - self.last = Some(last_packet); + Err(Error::CommandFailed) + }; } - _ => {} } + self.timer.sleep(Duration::from_millis(10)); } - - Ok(packets) + Err(Error::Timeout) } - /// Wait up to the configured poll_timeout for a fresh frame; - /// if none arrives, return cached snapshot if available. - pub fn read_latest(&mut self) -> Result, U::Error> { - let start = Instant::now(); - loop { - let frames = self.poll(256)?; - // Search for the latest DATA packet - let latest_data = - frames.iter().rev().find(|p| matches!(p, Packet::Minimal(_) | Packet::Standard(_))); - - if let Some(data) = latest_data { - self.last = Some(data.clone()); - return Ok(Some(Reading { - data: data.clone(), - fresh: true, - })); - } - if start.elapsed() >= self.config.poll_timeout { - return Ok(self.snapshot().map(|data| Reading { - data, - fresh: false, - })); - } - std::thread::sleep(Duration::from_millis(2)); + /// Attempt to read the next packet from UART. + /// This is non-blocking regarding the UART if the UART implementation is non-blocking. + pub fn next_packet(&mut self) -> Result, Error> { + let mut read_buffer = [0u8; 128]; + match self.uart.read(&mut read_buffer) { + Ok(0) => {} + Ok(n) => self.protocol.process_bytes(&read_buffer[..n]), + Err(e) => return Err(Error::Uart(e)), } - } - pub fn snapshot(&self) -> Option { - self.last.clone() + Ok(self.protocol.next_packet()) } /// Send a raw command to the sensor - pub fn send_command(&mut self, cmd: &[u8]) -> Result<(), U::Error> { - self.uart.write_all(cmd) - } - - pub fn set_output_mode(&mut self, mode: OutputMode) -> Result<(), U::Error> { - let frame = commands::switch_output_frame(mode); - self.uart.write_all(&frame) + pub fn send_command(&mut self, cmd: &[u8]) -> Result<(), Error> { + self.uart.write_all(cmd).map_err(Error::Uart) } - pub fn read_firmware_version(&mut self) -> Result, U::Error> { - self.uart.write_all(&commands::enter_config_frame())?; - sleep(self.config.config_mode_delay); + pub fn read_firmware_version(&mut self) -> Result> { + self.send_and_wait_ack(&commands::enter_config_frame())?; + self.timer.sleep(self.config.config_mode_delay); let frame = commands::read_firmware_frame(); - self.uart.write_all(&frame)?; + self.uart.write_all(&frame).map_err(Error::Uart)?; let mut result = None; - let start = Instant::now(); - while start.elapsed() < self.config.command_timeout { - let packets = self.poll(256)?; - for p in packets { + let start = self.timer.now(); + while self.timer.now() - start < self.config.command_timeout { + if let Some(p) = self.next_packet()? { if let Packet::Firmware(v) = p { result = Some(v); + break; } } - if result.is_some() { - break; - } - sleep(Duration::from_millis(10)); + self.timer.sleep(Duration::from_millis(10)); } - self.uart.write_all(&commands::exit_config_frame())?; - sleep(self.config.config_mode_delay); + self.send_and_wait_ack(&commands::exit_config_frame())?; + self.timer.sleep(self.config.config_mode_delay); - Ok(result) + result.ok_or(Error::Timeout) } - pub fn read_serial_number(&mut self) -> Result>, U::Error> { - self.uart.write_all(&commands::enter_config_frame())?; - sleep(self.config.config_mode_delay); + pub fn read_serial_number(&mut self) -> Result, Error> { + self.send_and_wait_ack(&commands::enter_config_frame())?; + self.timer.sleep(self.config.config_mode_delay); let frame = commands::read_serial_frame(); - self.uart.write_all(&frame)?; + self.uart.write_all(&frame).map_err(Error::Uart)?; let mut result = None; - let start = Instant::now(); - while start.elapsed() < self.config.command_timeout { - let packets = self.poll(256)?; - for p in packets { + let start = self.timer.now(); + while self.timer.now() - start < self.config.command_timeout { + if let Some(p) = self.next_packet()? { if let Packet::SerialNumber(sn) = p { result = Some(sn); + break; } } - if result.is_some() { - break; - } - sleep(Duration::from_millis(10)); + self.timer.sleep(Duration::from_millis(10)); } - self.uart.write_all(&commands::exit_config_frame())?; - sleep(self.config.config_mode_delay); + self.send_and_wait_ack(&commands::exit_config_frame())?; + self.timer.sleep(self.config.config_mode_delay); - Ok(result) + result.ok_or(Error::Timeout) } } diff --git a/src/error.rs b/src/error.rs new file mode 100644 index 0000000..01ba464 --- /dev/null +++ b/src/error.rs @@ -0,0 +1,16 @@ +use core::fmt::Debug; + +#[derive(Debug)] +pub enum Error { + Uart(E), + Timeout, + BufferOverflow, + InvalidFrame, + CommandFailed, +} + +impl From for Error { + fn from(e: E) -> Self { + Error::Uart(e) + } +} diff --git a/src/lib.rs b/src/lib.rs index e171771..b560325 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,8 +1,29 @@ +#![no_std] + +#[cfg(feature = "serial")] +extern crate std; + +#[cfg(feature = "embedded")] +extern crate alloc; + pub use crate::device::LD2410S; +pub use crate::error::Error; pub use crate::types::*; mod commands; mod device; +mod error; mod parser; +mod protocol; mod types; pub mod uart; + +#[cfg(feature = "async")] +pub mod asynchronous; + +use core::time::Duration; + +pub trait Timer { + fn sleep(&mut self, duration: Duration); + fn now(&self) -> Duration; // Monotonic time since some epoch +} diff --git a/src/protocol.rs b/src/protocol.rs new file mode 100644 index 0000000..dbae04b --- /dev/null +++ b/src/protocol.rs @@ -0,0 +1,140 @@ +use crate::parser::{parse_command_responses, parse_minimal_frames, parse_standard_frames}; +use crate::types::{CMD_HEAD, MINIMAL_HEAD, Packet, STANDARD_HEAD}; +use heapless::Deque; + +pub const STREAM_BUF_CAP: usize = 1024; + +/// Protocol handles the stateful parsing of bytes into packets. +/// It is "Sans-IO", meaning it doesn't know about UART or threads. +pub struct Protocol { + stream_buf: Deque, +} + +impl Default for Protocol { + fn default() -> Self { + Self::new() + } +} + +impl Protocol { + pub fn new() -> Self { + Self { + stream_buf: Deque::new(), + } + } + + /// Feed a single byte into the parser. + pub fn process_byte(&mut self, byte: u8) { + if self.stream_buf.push_back(byte).is_err() { + let _ = self.stream_buf.pop_front(); + let _ = self.stream_buf.push_back(byte); + } + } + + /// Feed a slice of bytes into the parser. + pub fn process_bytes(&mut self, bytes: &[u8]) { + for &b in bytes { + self.process_byte(b); + } + } + + /// Attempt to parse the next packet from the internal buffer. + /// Returns None if no complete packet is available. + pub fn next_packet(&mut self) -> Option { + loop { + if self.stream_buf.is_empty() { + return None; + } + + if self.stream_buf.len() < 4 { + // We need at least 4 bytes to check headers (except for minimal, which is 1 byte head) + // but even minimal needs 5 bytes total. + let head = *self.stream_buf.front().unwrap(); + if head != MINIMAL_HEAD && head != STANDARD_HEAD[0] && head != CMD_HEAD[0] { + let _ = self.stream_buf.pop_front(); + continue; + } + return None; + } + + // Peek at the beginning of the buffer to check headers + let mut head = [0u8; 4]; + for (i, byte) in self.stream_buf.iter().take(4).enumerate() { + head[i] = *byte; + } + + let consumed; + let mut packet = None; + + // 1. Check for Command/ACK Header + if head == CMD_HEAD { + let (p, c) = self.parse_with::<_, 4>(|b| parse_command_responses(b), 256); + if c > 0 { + consumed = c; + packet = p.first().cloned(); + } else { + return None; // Incomplete + } + } + // 2. Check for Standard Header + else if head == STANDARD_HEAD { + let (p, c) = self.parse_with::<_, 16>(|b| parse_standard_frames(b), 256); + if c > 0 { + consumed = c; + packet = p.first().cloned(); + } else { + return None; // Incomplete + } + } + // 3. Check for Minimal Header + else if head[0] == MINIMAL_HEAD { + let (p, c) = self.parse_with::<_, 16>(|b| parse_minimal_frames(b), 16); + if c > 0 { + consumed = c; + packet = p.first().cloned(); + } else { + // Might be incomplete minimal frame (needs 5 bytes) + if self.stream_buf.len() < 5 { + return None; + } else { + // Malformed? skip the 0x6E + consumed = 1; + } + } + } + // 4. Unknown byte + else { + consumed = 1; + } + + // Apply consumption + for _ in 0..consumed { + let _ = self.stream_buf.pop_front(); + } + + if packet.is_some() { + return packet; + } + } + } + + /// Helper to copy buffer to a contiguous slice and run a parser function + fn parse_with( + &self, + parser: F, + max_len: usize, + ) -> (heapless::Vec, usize) + where + F: FnOnce(&[u8]) -> (heapless::Vec, usize), + { + let snapshot_len = self.stream_buf.len().min(max_len); + let mut snapshot = [0u8; 256]; // Should be large enough for any frame + let final_len = snapshot_len.min(256); + + for (i, byte) in self.stream_buf.iter().take(final_len).enumerate() { + snapshot[i] = *byte; + } + + parser(&snapshot[..final_len]) + } +} diff --git a/src/types.rs b/src/types.rs index e86086e..153d887 100644 --- a/src/types.rs +++ b/src/types.rs @@ -104,13 +104,6 @@ impl Packet { } } -/// Returned by read_latest(): either fresh data or cached -#[derive(Clone, Debug)] -pub struct Reading { - pub data: Packet, - pub fresh: bool, -} - impl From for Packet { fn from(m: MinimalPacket) -> Self { Packet::Minimal(m) diff --git a/src/uart.rs b/src/uart.rs index 569d211..1825a66 100644 --- a/src/uart.rs +++ b/src/uart.rs @@ -1,4 +1,4 @@ -use std::fmt::Debug; +use core::fmt::Debug; // Trait that abstracts UART operations for LD2410S pub trait UartInterface { @@ -11,14 +11,17 @@ pub trait UartInterface { // ─── SERIALPORT (Desktop) IMPLEMENTATION ──────────────────────────────── #[cfg(feature = "serial")] -pub use serial_impl::SerialPortWrapper; +pub use serial_impl::{SerialPortWrapper, StdTimer}; #[cfg(feature = "serial")] mod serial_impl { use super::UartInterface; + use crate::Timer; use serialport::SerialPort; + use std::boxed::Box; use std::fmt; use std::io::{Read, Write}; + use std::time::{Duration, Instant}; /// Wrapper type for `serialport` UART on desktop pub struct SerialPortWrapper(pub Box); @@ -49,18 +52,43 @@ mod serial_impl { } } } + + pub struct StdTimer { + start: Instant, + } + + impl Default for StdTimer { + fn default() -> Self { + Self { + start: Instant::now(), + } + } + } + + impl Timer for StdTimer { + fn sleep(&mut self, duration: Duration) { + std::thread::sleep(duration); + } + + fn now(&self) -> Duration { + self.start.elapsed() + } + } } // ─── ESP-IDF-HAL (embedded) IMPLEMENTATION ──────────────────────────────── #[cfg(feature = "embedded")] -pub use embedded_impl::EspUartWrapper; +pub use embedded_impl::{EspTimer, EspUartWrapper}; #[cfg(feature = "embedded")] mod embedded_impl { use super::UartInterface; + use crate::Timer; use core::fmt; + use core::time::Duration; use esp_idf_hal::{ + delay::FreeRtos, sys::{ESP_ERR_TIMEOUT, EspError}, uart::UartDriver, }; @@ -78,13 +106,10 @@ mod embedded_impl { type Error = EspError; fn write_all(&mut self, data: &[u8]) -> Result<(), Self::Error> { - // esp-idf-hal write is already blocking for the slice self.0.write(data).map(|_| ()) } fn read(&mut self, buf: &mut [u8]) -> Result { - // Treat UART timeout as "no data" (Ok(0)), pass through other errors. - // timeout=0 (non-blocking) ensures we don't block the polling loop. match self.0.read(buf, 0) { Ok(n) => Ok(n), Err(e) if e.code() == ESP_ERR_TIMEOUT => Ok(0), @@ -92,4 +117,19 @@ mod embedded_impl { } } } + + pub struct EspTimer; + + impl Timer for EspTimer { + fn sleep(&mut self, duration: Duration) { + FreeRtos::delay_ms(duration.as_millis() as u32); + } + + fn now(&self) -> Duration { + // Use u64 to prevent overflow (ticks * 1000 can easily exceed u32::MAX) + let ticks = unsafe { esp_idf_hal::sys::xTaskGetTickCount() } as u64; + let hz = esp_idf_hal::sys::configTICK_RATE_HZ as u64; + Duration::from_millis((ticks * 1000) / hz) + } + } }