diff --git a/CHANGELOG.md b/CHANGELOG.md index 178c47c..3c0e6fa 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,33 @@ and this project adheres to [Conventional Commits](https://www.conventionalcommi ## [Unreleased] +### Changed + +- Remap AUX strobe switches: AUX7 3-way off → red/green → white, AUX8 momentary → white strobe at 80 +- Add `strobe_split` field to `LedState` for position-light strobe mode + +### Added + +- Temporal dithering module (`src/dither.rs`): adds extra perceived bit depth to WS2812B LEDs by varying quantized output frame-to-frame faster than flicker fusion +- `DitherMode` enum with four modes: `Off`, `ErrorDiffusion` (smooth gradients), `Ordered` (Bayer 4x4, deterministic), `Hybrid` (error diffusion + correlated ordered for low brightness) +- 8.8 fixed-point gamma LUTs (3 x 256 entries, 1536 bytes flash) computed at compile time for high-precision gamma correction in the dithered path +- `SetDitherMode` and `SetDitherFps` BLE commands for runtime control of dithering +- `dither_mode` and `dither_fps` fields in BLE `StateResponse` +- Inner dither loop in LED task: animation renders at `fps` rate, strip refreshes at `dither_fps` rate (100–960 Hz) with different dither patterns between animation frames +- Dither state auto-reset on mode change, strobe activation, and BLE flash sequences +- Unit tests for dither algorithms (error diffusion convergence, ordered determinism, Fix16 gamma roundtrip) +- `DisplayTestPattern` BLE command: temporarily force a color + animation combo for a given duration, overriding FC flight mode patterns +- `CancelTestPattern` BLE command: stop a running test pattern immediately +- `test_active` field in BLE `StateResponse` (true when a test pattern is playing) +- RSSI-based TX link detection via MSP_ANALOG — strobe only activates when RSSI > 0 (replaces unreliable stick-center heuristic) +- `tx_linked` field exposed in BLE `StateResponse` for app display + +### Removed + +- Wi-Fi AP hotspot, HTTP web UI, and DHCP server (BLE is now the sole control interface) +- `embassy-net`, `smoltcp`, `edge-dhcp`, and `embedded-io` dependencies +- `wifi` and `coex` features from `esp-radio` (no longer needed without Wi-Fi) + ### Added - BLE Nordic UART Service (NUS) for app control via JSON protocol diff --git a/Cargo.lock b/Cargo.lock index 3d83215..e64789c 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -347,38 +347,6 @@ dependencies = [ "litrs 1.0.0", ] -[[package]] -name = "edge-dhcp" -version = "0.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2e0b32c831ced877a78378312fe0b6f7cdd5759f3ba272578f582ff9bba5291d" -dependencies = [ - "defmt 1.0.1", - "edge-nal", - "edge-raw", - "embassy-futures", - "embassy-time", - "heapless 0.9.2", - "num_enum", - "rand_core 0.9.5", -] - -[[package]] -name = "edge-nal" -version = "0.6.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f3c7d7163586cb9d457a34561a644aa957ce870226729bf6c9c8beeaead7e0d8" -dependencies = [ - "embassy-time", - "embedded-io-async 0.7.0", -] - -[[package]] -name = "edge-raw" -version = "0.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "466dfce9c2172a4e947b81b556f1f07a86029fbac679e323cfb66c738cc2faea" - [[package]] name = "embassy-embedded-hal" version = "0.5.0" @@ -445,29 +413,6 @@ dependencies = [ "num-traits", ] -[[package]] -name = "embassy-net" -version = "0.8.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "71f0aa32082b7df00164f485322d6edab59122c9718b363b07ec23424c2c06a0" -dependencies = [ - "document-features", - "embassy-net-driver", - "embassy-sync 0.7.2", - "embassy-time", - "embedded-io-async 0.7.0", - "embedded-nal-async", - "heapless 0.8.0", - "managed", - "smoltcp", -] - -[[package]] -name = "embassy-net-driver" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "524eb3c489760508f71360112bca70f6e53173e6fe48fc5f0efd0f5ab217751d" - [[package]] name = "embassy-sync" version = "0.6.2" @@ -505,7 +450,6 @@ checksum = "f4fa65b9284d974dad7a23bb72835c4ec85c0b540d86af7fc4098c88cff51d65" dependencies = [ "cfg-if", "critical-section", - "defmt 1.0.1", "document-features", "embassy-time-driver", "embassy-time-queue-utils", @@ -606,25 +550,6 @@ dependencies = [ "embedded-io 0.7.1", ] -[[package]] -name = "embedded-nal" -version = "0.9.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c56a28be191a992f28f178ec338a0bf02f63d7803244add736d026a471e6ed77" -dependencies = [ - "nb 1.1.0", -] - -[[package]] -name = "embedded-nal-async" -version = "0.9.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "eb5a1bd585135d302f8f6d7de329310938093da6271b37a6c94b8798795c0c6d" -dependencies = [ - "embedded-io-async 0.7.0", - "embedded-nal", -] - [[package]] name = "embedded-storage" version = "0.3.1" @@ -814,7 +739,6 @@ dependencies = [ "cfg-if", "defmt 1.0.1", "document-features", - "embassy-net-driver", "embedded-io 0.6.1", "embedded-io 0.7.1", "embedded-io-async 0.6.1", @@ -1320,27 +1244,6 @@ dependencies = [ "autocfg", ] -[[package]] -name = "num_enum" -version = "0.7.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b1207a7e20ad57b847bbddc6776b968420d38292bbfe2089accff5e19e82454c" -dependencies = [ - "num_enum_derive", - "rustversion", -] - -[[package]] -name = "num_enum_derive" -version = "0.7.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ff32365de1b6743cb203b710788263c44a03de03802daf96092f2da4fe6ba4d7" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.115", -] - [[package]] name = "once_cell" version = "1.21.3" @@ -1997,12 +1900,9 @@ dependencies = [ "bleps", "critical-section", "defmt 0.3.100", - "edge-dhcp", "embassy-executor", - "embassy-net", "embassy-sync 0.7.2", "embassy-time", - "embedded-io 0.7.1", "embedded-io-async 0.7.0", "esp-alloc", "esp-bootloader-esp-idf", @@ -2016,7 +1916,6 @@ dependencies = [ "serde", "serde-json-core", "smart-leds", - "smoltcp", "static_cell", "ws2812-spi", ] diff --git a/Cargo.toml b/Cargo.toml index 56b6bc4..909c770 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -9,13 +9,6 @@ path = "./src/bin/main.rs" [dependencies] defmt = "0.3.10" -embassy-net = { version = "0.8.0", features = [ - "dhcpv4", - "medium-ethernet", - "tcp", - "udp", -] } -embedded-io = "0.7.1" embedded-io-async = "0.7.0" esp-alloc = "0.9.0" esp-hal = { version = "1.0.0", features = [ @@ -25,18 +18,6 @@ esp-hal = { version = "1.0.0", features = [ ] } panic-rtt-target = { version = "0.2.0", features = ["defmt"] } rtt-target = { version = "0.6.1", features = ["defmt"] } -smoltcp = { version = "0.12.0", default-features = false, features = [ - "medium-ethernet", - "multicast", - "proto-dhcpv4", - "proto-dns", - "proto-ipv4", - "socket-dns", - "socket-icmp", - "socket-raw", - "socket-tcp", - "socket-udp", -] } bleps = { git = "https://github.com/bjoernQ/bleps", package = "bleps", rev = "a5148d8ae679e021b78f53fd33afb8bb35d0b62e", features = [ "async", "macros", @@ -56,10 +37,8 @@ esp-rtos = { version = "0.2.0", features = [ ] } esp-radio = { version = "0.17.0", features = [ "ble", - "coex", "defmt", "esp32c3", - "wifi", "unstable", ] } heapless = { version = "0.8.0", default-features = false, features = ["serde"] } @@ -70,7 +49,7 @@ static_cell = { version = "2.1.0", features = ["nightly"] } smart-leds = "0.4.0" ws2812-spi = "0.5.1" esp-bootloader-esp-idf = { version = "0.4.0", features = ["esp32c3"] } -edge-dhcp = { version = "0.7.0", features = ["io", "defmt"] } + [profile.dev] # Rust debug is too slow. diff --git a/FLUTTER_BLE_GUIDE.md b/FLUTTER_BLE_GUIDE.md index da8e728..77847b4 100644 --- a/FLUTTER_BLE_GUIDE.md +++ b/FLUTTER_BLE_GUIDE.md @@ -4,7 +4,7 @@ This guide is for building a Flutter companion app that controls the AirLED ESP3 ## Architecture Overview -The device runs on an ESP32-C3 (XIAO form factor) and exposes a **Nordic UART Service (NUS)** over BLE. Communication is newline-delimited JSON over a serial-like BLE pipe. The device also runs a Wi-Fi AP with HTTP control as a fallback, but BLE is the primary interface for the app. +The device runs on an ESP32-C3 (XIAO form factor) and exposes a **Nordic UART Service (NUS)** over BLE. Communication is newline-delimited JSON over a serial-like BLE pipe. BLE is the primary and only control interface for the app. ## BLE Connection @@ -219,6 +219,32 @@ Per-frame amplitude decay percentage. Only applies in ripple mode. - `value`: `u8` (clamped 90–99), default **97** +### DisplayTestPattern + +Temporarily force a color + animation combo for a given duration, overriding FC flight mode patterns. + +```json +{"DisplayTestPattern":{"color":"rainbow","anim":"ripple","duration_ms":5000}} +``` + +- `color`: string — any valid color mode key (see SetColorMode) +- `anim`: string — any valid animation mode key (see SetAnimMode) +- `duration_ms`: `u16` (1–65535) — how long to display the test pattern in milliseconds + +Returns `ok\n` on success, or `err:unknown_color_mode\n` / `err:unknown_anim_mode\n` on invalid values. + +The test pattern overrides FC flight mode displays but not AUX strobe or BLE flash indicators. When the duration expires, the device reverts to normal behavior and pushes a state update. + +### CancelTestPattern + +Stop a running test pattern immediately and revert to normal behavior. + +```json +{"CancelTestPattern":null} +``` + +Returns `ok\n`. Safe to send even when no test pattern is active. + ## StateResponse (device → app) Full JSON state snapshot. Approximately 250 bytes serialized. @@ -242,7 +268,9 @@ Full JSON state snapshot. Approximately 250 bytes serialized. "ripple_width": 190, "ripple_decay": 97, "fc_connected": false, - "flight_mode": "arming_forbidden" + "flight_mode": "arming_forbidden", + "tx_linked": false, + "test_active": false } ``` @@ -268,6 +296,8 @@ Full JSON state snapshot. Approximately 250 bytes serialized. | `ripple_decay` | int | 90–99 | Ripple decay % | | `fc_connected` | bool | | Flight controller connected | | `flight_mode` | string | see below | Current flight mode | +| `tx_linked` | bool | | RC transmitter link active (RSSI > 0) | +| `test_active` | bool | | A BLE test pattern is currently playing | ### Flight modes diff --git a/src/bin/main.rs b/src/bin/main.rs index 4d9479f..de29ec0 100644 --- a/src/bin/main.rs +++ b/src/bin/main.rs @@ -1,13 +1,7 @@ #![no_std] #![no_main] -use core::net::Ipv4Addr; - use defmt::info; -use edge_dhcp::server::{Server as DhcpServer, ServerOptions as DhcpServerOptions}; -use edge_dhcp::{Options as DhcpOptions, Packet as DhcpPacket}; -use embassy_net::{Ipv4Address, Ipv4Cidr, Runner, Stack, StackResources, StaticConfigV4, tcp::TcpSocket}; -use embassy_net::udp::{PacketMetadata, UdpSocket}; use embassy_time::{Duration, Timer, with_timeout}; use embedded_io_async::{Read as AsyncRead, Write}; use esp_hal::clock::CpuClock; @@ -27,9 +21,6 @@ use bleps::asynch::Ble; use bleps::attribute_server::NotificationData; use bleps::gatt; use esp_radio::ble::controller::BleConnector; -use esp_radio::wifi::{ - AccessPointConfig, ModeConfig, WifiApState, ap_state, -}; use panic_rtt_target as _; use smart_leds::{SmartLedsWrite, RGB8}; use ws2812_spi::prerendered::Ws2812; @@ -39,6 +30,7 @@ use xiao_drone_led_controller::msp::{ use xiao_drone_led_controller::pattern::{ Animation, ColorScheme, Pulse, RippleEffect, StaticAnim, }; +use xiao_drone_led_controller::dither::{self, DitherMode, DitherState}; use xiao_drone_led_controller::postfx::{PostEffect, apply_pipeline}; use xiao_drone_led_controller::ble::{ self as ble_proto, HandleResult, @@ -51,7 +43,6 @@ use static_cell::StaticCell; extern crate alloc; esp_bootloader_esp_idf::esp_app_desc!(); -use alloc::string::ToString; /// Maximum number of WS2812 LEDs supported (compile-time buffer size). const MAX_LEDS: usize = 200; @@ -59,11 +50,8 @@ const MAX_LEDS: usize = 200; /// SPI pre-rendered buffer size for ws2812-spi (4 SPI bytes per 2 data bits × 12 per LED). const SPI_BUF_LEN: usize = MAX_LEDS * 12; -/// Wi-Fi AP SSID. -const WIFI_SSID: &str = "AirLED"; - -/// AP static IP address. -const AP_IP: Ipv4Address = Ipv4Address::new(192, 168, 4, 1); +/// BLE device advertising name. +const BLE_DEVICE_NAME: &str = "AirLED"; /// Maximum number of active LEDs (must match `MAX_LEDS`). const MAX_NUM_LEDS: u16 = MAX_LEDS as u16; @@ -102,42 +90,11 @@ fn main() -> ! { .with_dma(peripherals.DMA_CH0) .with_buffers(dma_rx, dma_tx); - // --- Wi-Fi setup (scheduler is now running) --- + // --- Radio + BLE setup (scheduler is now running) --- static RADIO: StaticCell> = StaticCell::new(); let radio_controller: &'static esp_radio::Controller<'static> = RADIO.init(esp_radio::init().expect("failed to init esp-radio")); - let (mut wifi_controller, interfaces) = - esp_radio::wifi::new(radio_controller, peripherals.WIFI, esp_radio::wifi::Config::default()) - .expect("failed to create wifi"); - - let ap_config = AccessPointConfig::default() - .with_ssid(WIFI_SSID.to_string()) - .with_channel(6); - - wifi_controller - .set_config(&ModeConfig::AccessPoint(ap_config)) - .expect("failed to set wifi config"); - - wifi_controller.start().expect("failed to start wifi"); - - info!("Wi-Fi AP starting..."); - - // --- Network stack --- - let net_config = embassy_net::Config::ipv4_static(StaticConfigV4 { - address: Ipv4Cidr::new(AP_IP, 24), - gateway: Some(AP_IP), - dns_servers: Default::default(), - }); - - static RESOURCES: StaticCell> = StaticCell::new(); - let (stack, runner) = embassy_net::new( - interfaces.ap, - net_config, - RESOURCES.init(StackResources::new()), - 0, // random seed — no true randomness needed for AP - ); - // --- BLE setup --- info!("Setting up BLE..."); let ble_connector = BleConnector::new( @@ -164,29 +121,9 @@ fn main() -> ! { spawner.must_spawn(led_task(spi_bus)); spawner.must_spawn(msp_task(msp_uart)); spawner.must_spawn(ble_task(ble_connector)); - spawner.must_spawn(net_task(runner)); - spawner.must_spawn(web_server(stack)); - spawner.must_spawn(dhcp_server(stack)); - spawner.must_spawn(wifi_keepalive(wifi_controller)); }) } -/// Keeps the Wi-Fi controller alive and logs AP state changes. -#[embassy_executor::task] -async fn wifi_keepalive(wifi_controller: esp_radio::wifi::WifiController<'static>) { - // Wait for AP to start - while ap_state() != WifiApState::Started { - Timer::after(Duration::from_millis(100)).await; - } - info!("Wi-Fi AP started on channel 6"); - - // Keep wifi controller alive (dropping it stops wifi) - let _controller = wifi_controller; - loop { - Timer::after(Duration::from_secs(10)).await; - } -} - /// BLE notification chunk size (BLE default MTU payload). const BLE_CHUNK_SIZE: usize = 20; @@ -223,19 +160,36 @@ static BLE_NOTIFY: embassy_sync::signal::Signal< (), > = embassy_sync::signal::Signal::new(); +/// Compact BLE_TX: shift unsent data to the start of the buffer. +fn compact_tx(tx: &mut BleTxBuf) { + if tx.offset > 0 { + if tx.offset < tx.len { + tx.data.copy_within(tx.offset..tx.len, 0); + tx.len -= tx.offset; + } else { + tx.len = 0; + } + tx.offset = 0; + } +} + /// Process a complete command message from the RX buffer. /// -/// Called from the sync write callback. Writes the response into BLE_TX -/// and signals the notifier. +/// Called from the sync write callback. Appends the response into BLE_TX +/// (after any unsent data) and signals the notifier. fn ble_handle_message(msg: &[u8]) { let Some(cmd) = ble_proto::parse_command(msg) else { - // Write error response + // Append error response after any unsent data critical_section::with(|cs| { let mut tx = BLE_TX.borrow_ref_mut(cs); + compact_tx(&mut tx); let err = b"err:parse\n"; - tx.data[..err.len()].copy_from_slice(err); - tx.len = err.len(); - tx.offset = 0; + let start = tx.len; + let avail = tx.data.len() - start; + if err.len() <= avail { + tx.data[start..start + err.len()].copy_from_slice(err); + tx.len = start + err.len(); + } }); BLE_NOTIFY.signal(()); return; @@ -246,21 +200,33 @@ fn ble_handle_message(msg: &[u8]) { let result = ble_proto::handle_command(&cmd, &mut state); critical_section::with(|cs| { let mut tx = BLE_TX.borrow_ref_mut(cs); - tx.offset = 0; + compact_tx(&mut tx); match result { HandleResult::SendState => { let resp = ble_proto::build_state_response(&state); - tx.len = ble_proto::serialize_state(&resp, &mut tx.data).unwrap_or(0); + let start = tx.len; + let written = ble_proto::serialize_state( + &resp, + &mut tx.data[start..], + ) + .unwrap_or(0); + tx.len = start + written; } HandleResult::Ack => { - tx.data[..3].copy_from_slice(b"ok\n"); - tx.len = 3; + let ack = b"ok\n"; + let start = tx.len; + let avail = tx.data.len() - start; + if ack.len() <= avail { + tx.data[start..start + ack.len()].copy_from_slice(ack); + tx.len = start + ack.len(); + } } HandleResult::Error(e) => { let eb = e.as_bytes(); - let len = eb.len().min(tx.data.len()); - tx.data[..len].copy_from_slice(&eb[..len]); - tx.len = len; + let start = tx.len; + let len = eb.len().min(tx.data.len() - start); + tx.data[start..start + len].copy_from_slice(&eb[..len]); + tx.len = start + len; } } }); @@ -308,7 +274,7 @@ async fn ble_task(mut connector: BleConnector<'static>) { // Set advertising data let adv_data = create_advertising_data(&[ AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED), - AdStructure::CompleteLocalName(WIFI_SSID), + AdStructure::CompleteLocalName(BLE_DEVICE_NAME), ]); match adv_data { Ok(data) => { @@ -333,7 +299,7 @@ async fn ble_task(mut connector: BleConnector<'static>) { continue; } - info!("BLE advertising as \"{}\"", WIFI_SSID); + info!("BLE advertising as \"{}\"", BLE_DEVICE_NAME); // Track whether we've seen a real client (first RX write = client connected) static BLE_CONNECTED: critical_section::Mutex> = @@ -455,9 +421,14 @@ async fn ble_task(mut connector: BleConnector<'static>) { drop(state); critical_section::with(|cs| { let mut tx = BLE_TX.borrow_ref_mut(cs); - tx.len = ble_proto::serialize_state(&resp, &mut tx.data) - .unwrap_or(0); - tx.offset = 0; + compact_tx(&mut tx); + let start = tx.len; + let written = ble_proto::serialize_state( + &resp, + &mut tx.data[start..], + ) + .unwrap_or(0); + tx.len = start + written; }); // Loop back to send chunks } @@ -568,6 +539,7 @@ async fn msp_task(mut uart: Uart<'static, esp_hal::Async>) { let mut prev_aux = [0u16; 12]; // AUX1–AUX12 (channels 5–16) let mut rc_tick: u8 = 0; let mut logged_rc_once = false; + let mut flight_mode = FlightMode::ArmingForbidden; loop { // Retry box map if we never got it (FC wasn't ready at startup) @@ -634,6 +606,7 @@ async fn msp_task(mut uart: Uart<'static, esp_hal::Async>) { let arming_disable = msp::extract_arming_disable_flags(&frame.payload, frame.size) .unwrap_or(0); let mode = msp::resolve_flight_mode(flags, &box_map, arming_disable); + flight_mode = mode; let mut state = STATE.lock().await; if state.flight_mode != mode || !state.fc_connected { info!("MSP: flags=0x{:08x} mode={}", flags, defmt::Debug2Format(&mode)); @@ -642,8 +615,15 @@ async fn msp_task(mut uart: Uart<'static, esp_hal::Async>) { state.fc_connected = true; state.flight_mode = mode; state.debug_flags = flags; + // TX link: FC reports ArmingAllowed or better → valid RX link + let linked = mode != FlightMode::ArmingForbidden; + let link_changed = state.tx_linked != linked; + state.tx_linked = linked; + if !linked { + state.aux_strobe = 0; + } drop(state); - if changed { + if changed || link_changed { STATE_CHANGED.signal(()); } error_count = 0; @@ -658,10 +638,12 @@ async fn msp_task(mut uart: Uart<'static, esp_hal::Async>) { } if error_count >= 10 { + flight_mode = FlightMode::ArmingForbidden; let mut state = STATE.lock().await; state.fc_connected = false; state.flight_mode = FlightMode::ArmingForbidden; state.aux_strobe = 0; + state.tx_linked = false; drop(state); // Reset counter to avoid spamming state writes every tick error_count = 10; @@ -693,26 +675,31 @@ async fn msp_task(mut uart: Uart<'static, esp_hal::Async>) { } // AUX7 (channel 11, index 10) 3-position strobe - // AUX8 (channel 12, index 11) spring switch override → full - // Suppress strobe for first 10s after boot to avoid garbage triggers + // pos 1 (low) → off + // pos 2 (mid) → position-light strobe (red/green) at 80 + // pos 3 (high) → white strobe at 80 + // AUX8 (channel 12, index 11) momentary → white strobe at 80 + // Suppress strobe for first 10s after boot and when FC + // reports ArmingForbidden (no valid RX link). let uptime_ms = embassy_time::Instant::now().as_millis(); - if count >= 12 && uptime_ms > 10_000 { + if count >= 12 && uptime_ms > 10_000 && flight_mode != FlightMode::ArmingForbidden { let aux7 = rc_channels[10]; let aux8 = rc_channels[11]; - let strobe_level: u8 = if aux8 > 1800 { - 255 // AUX8 spring switch → full blast + let (strobe_level, strobe_split): (u8, bool) = if aux8 > 1800 { + (80, false) // AUX8 momentary → mid white } else if aux7 > 1650 { - 255 // AUX7 position 3 → full + (80, false) // AUX7 pos 3 → white } else if aux7 > 1250 { - 80 // AUX7 position 2 → low + (80, true) // AUX7 pos 2 → position-light (red/green) } else { - 0 // off + (0, false) // off }; let mut state = STATE.lock().await; - if state.aux_strobe != strobe_level { - info!("MSP strobe: {}", strobe_level); + if state.aux_strobe != strobe_level || state.strobe_split != strobe_split { + info!("MSP strobe: {} split={}", strobe_level, strobe_split); } state.aux_strobe = strobe_level; + state.strobe_split = strobe_split; } // Log AUX channel changes with deadband (channels 5–16) @@ -787,7 +774,18 @@ async fn led_task(spi_bus: SpiDmaBus<'static, esp_hal::Blocking>) { let mut prev_color_mode = ColorMode::Split; let mut prev_use_hsi = false; + // Dedicated test-pattern animation instances (separate from user ones to preserve phase) + let mut test_pulse = Pulse::new(); + let mut test_ripple = RippleEffect::new(0xBEEF_CAFE); + let mut test_static = StaticAnim; + let mut test_scheme = build_color_scheme(ColorMode::Split, false); + let mut test_prev_color = ColorMode::Split; + let mut buf = [RGB8 { r: 0, g: 0, b: 0 }; MAX_LEDS]; + let mut dither_state = DitherState::new(); + let mut fix16_targets = [[0u16; 3]; MAX_LEDS]; + let mut dither_output = [RGB8 { r: 0, g: 0, b: 0 }; MAX_LEDS]; + let mut prev_dither_mode = DitherMode::Off; let mut write_err_logged = false; let mut frame_counter: u32 = 0; @@ -829,8 +827,20 @@ async fn led_task(spi_bus: SpiDmaBus<'static, esp_hal::Blocking>) { let anim_mode = state.anim_mode; let anim_params = state.anim_params; let aux_strobe = state.aux_strobe; + let strobe_split = state.strobe_split; + let dither_mode = state.dither_mode; + let dither_fps = state.dither_fps; + let test_frames = state.test_pattern_frames; + let test_color = state.test_color; + let test_anim = state.test_anim; drop(state); + // Reset dither accumulators on mode change + if dither_mode != prev_dither_mode { + dither_state.reset(); + prev_dither_mode = dither_mode; + } + // Clear LEDs beyond active count so they don't hold stale colors for led in buf[num_leds..].iter_mut() { *led = RGB8 { r: 0, g: 0, b: 0 }; @@ -838,21 +848,41 @@ async fn led_task(spi_bus: SpiDmaBus<'static, esp_hal::Blocking>) { let active = &mut buf[..num_leds]; // AUX7 strobe override: fast white strobe (~25 Hz) with short attack/decay + // Strobe bypasses dithering entirely — direct u8 writes. if aux_strobe > 0 { + dither_state.reset(); // 4-frame cycle: 2 on, 2 off → 25 Hz at 100 FPS const STROBE_HALF: u32 = 2; const STROBE_PERIOD: u32 = STROBE_HALF * 2; let peak = aux_strobe; let phase = frame_counter % STROBE_PERIOD; - let intensity = if phase < STROBE_HALF { - ((phase + 1) as u16 * peak as u16 / STROBE_HALF as u16) as u8 + // Quadratic envelope: sharper attack, faster tail-off + let linear = if phase < STROBE_HALF { + (phase + 1) * 255 / STROBE_HALF } else { - let off_phase = phase - STROBE_HALF; - ((STROBE_HALF - off_phase) as u16 * peak as u16 / STROBE_HALF as u16) as u8 + let off = phase - STROBE_HALF; + (STROBE_HALF - off) * 255 / STROBE_HALF }; - let color = RGB8 { r: intensity, g: intensity, b: intensity }; - for led in active.iter_mut() { - *led = color; + let intensity = + (linear * linear / 255 * peak as u32 / 255) as u8; + if strobe_split { + // Position-light strobe: red port / green starboard + // Green scaled to 75% to match perceived red brightness + let half = active.len() / 2; + let green_val = (intensity as u16 * 3 / 4) as u8; + let red = RGB8 { r: intensity, g: 0, b: 0 }; + let green = RGB8 { r: 0, g: green_val, b: 0 }; + for led in active[..half].iter_mut() { + *led = green; + } + for led in active[half..].iter_mut() { + *led = red; + } + } else { + let color = RGB8 { r: intensity, g: intensity, b: intensity }; + for led in active.iter_mut() { + *led = color; + } } match ws.write(buf.iter().copied()) { @@ -873,6 +903,7 @@ async fn led_task(spi_bus: SpiDmaBus<'static, esp_hal::Blocking>) { // BLE flash override: solid blue flashes (1× connect, 2× disconnect) if flash_remaining > 0 { + dither_state.reset(); let blue = RGB8 { r: 0, g: 0, b: 255 }; let black = RGB8 { r: 0, g: 0, b: 0 }; @@ -912,11 +943,62 @@ async fn led_task(spi_bus: SpiDmaBus<'static, esp_hal::Blocking>) { continue; } + // BLE test pattern override (between BLE flash and FC flight mode) + if test_frames > 0 { + // Rebuild test color scheme if color changed + if test_color != test_prev_color { + test_scheme = build_color_scheme(test_color, use_hsi); + test_prev_color = test_color; + } + + // Render with test animation + match test_anim { + AnimMode::Static => test_static.render(active, &mut test_scheme), + AnimMode::Pulse => test_pulse.render(active, &mut test_scheme), + AnimMode::Ripple => test_ripple.render(active, &mut test_scheme), + } + + // Decrement remaining frames + { + let mut state = STATE.lock().await; + if state.test_pattern_frames > 0 { + state.test_pattern_frames -= 1; + if state.test_pattern_frames == 0 { + drop(state); + STATE_CHANGED.signal(()); + } + } + } + + let pipeline = [ + PostEffect::Gamma, + PostEffect::ColorBalance { r: bal_r, g: bal_g, b: bal_b }, + PostEffect::Brightness(led_brightness), + PostEffect::CurrentLimit { max_ma }, + ]; + apply_pipeline(active, &pipeline); + + match ws.write(buf.iter().copied()) { + Err(e) if !write_err_logged => { + defmt::warn!("LED write error: {}", defmt::Debug2Format(&e)); + write_err_logged = true; + } + Ok(_) if write_err_logged => { + info!("LED write recovered"); + write_err_logged = false; + } + _ => {} + } + frame_counter = frame_counter.wrapping_add(1); + Timer::after(Duration::from_millis(1000 / fps as u64)).await; + continue; + } + // Flight-mode override logic: // - Armed → rainbow ripple // - Failsafe → sliding red bars // - Disarmed (FC connected) → continuous pulse, red=forbidden / green=allowed - // - No FC → user-selected pattern from web UI + // - No FC → user-selected pattern from BLE app if fc_connected && flight_mode == FlightMode::Armed { armed_ripple.render(active, &mut armed_scheme); } else if fc_connected && flight_mode == FlightMode::Failsafe { @@ -984,475 +1066,78 @@ async fn led_task(spi_bus: SpiDmaBus<'static, esp_hal::Blocking>) { } } - let pipeline = [ - PostEffect::Gamma, - PostEffect::ColorBalance { r: bal_r, g: bal_g, b: bal_b }, - PostEffect::Brightness(led_brightness), - PostEffect::CurrentLimit { max_ma }, - ]; - apply_pipeline(active, &pipeline); - - match ws.write(buf.iter().copied()) { - Err(e) if !write_err_logged => { - defmt::warn!("LED write error: {}", defmt::Debug2Format(&e)); - write_err_logged = true; - } - Ok(_) if write_err_logged => { - info!("LED write recovered"); - write_err_logged = false; - } - _ => {} - } - - frame_counter = frame_counter.wrapping_add(1); - Timer::after(Duration::from_millis(1000 / fps as u64)).await; - } -} - -/// Runs the embassy-net network stack. -#[embassy_executor::task] -async fn net_task(mut runner: Runner<'static, esp_radio::wifi::WifiDevice<'static>>) { - runner.run().await; -} - -/// DHCP server assigning IPs to clients connecting to the AP. -#[embassy_executor::task] -async fn dhcp_server(stack: Stack<'static>) { - // Wait until the stack is configured - while !stack.is_config_up() { - Timer::after(Duration::from_millis(100)).await; - } - - let mut rx_meta = [PacketMetadata::EMPTY; 2]; - let mut rx_buffer = [0u8; 600]; - let mut tx_meta = [PacketMetadata::EMPTY; 2]; - let mut tx_buffer = [0u8; 600]; - - let mut socket = UdpSocket::new(stack, &mut rx_meta, &mut rx_buffer, &mut tx_meta, &mut tx_buffer); - socket.bind(67).expect("failed to bind DHCP server socket"); - - info!("DHCP server running on port 67"); - - let server_ip = Ipv4Addr::new(192, 168, 4, 1); - let mut gw_buf = [Ipv4Addr::UNSPECIFIED; 1]; - let server_options = DhcpServerOptions::new(server_ip, Some(&mut gw_buf)); - - // Up to 8 concurrent leases - let mut server = DhcpServer::<_, 8>::new_with_et(server_ip); - server.range_start = Ipv4Addr::new(192, 168, 4, 50); - server.range_end = Ipv4Addr::new(192, 168, 4, 200); - - let mut buf = [0u8; 600]; - - loop { - let (len, _meta) = match socket.recv_from(&mut buf).await { - Ok(result) => result, - Err(_) => continue, - }; - - let request = match DhcpPacket::decode(&buf[..len]) { - Ok(pkt) => pkt, - Err(e) => { - defmt::warn!("DHCP decode error: {}", defmt::Debug2Format(&e)); - continue; - } - }; - - let mut opt_buf = DhcpOptions::buf(); - - if let Some(reply) = server.handle_request(&mut opt_buf, &server_options, &request) { - match reply.encode(&mut buf) { - Ok(encoded) => { - // DHCP replies go to broadcast 255.255.255.255:68 - let dest = (Ipv4Address::new(255, 255, 255, 255), 68); - if let Err(e) = socket.send_to(encoded, dest).await { - defmt::warn!("DHCP send error: {}", defmt::Debug2Format(&e)); - } - } - Err(e) => { - defmt::warn!("DHCP encode error: {}", defmt::Debug2Format(&e)); - } - } - } - } -} + if dither_mode == DitherMode::Off { + // Non-dithered path: Gamma → Balance → Brightness → CurrentLimit → SPI write + let pipeline = [ + PostEffect::Gamma, + PostEffect::ColorBalance { r: bal_r, g: bal_g, b: bal_b }, + PostEffect::Brightness(led_brightness), + PostEffect::CurrentLimit { max_ma }, + ]; + apply_pipeline(active, &pipeline); -/// Parse query parameters from a request path, updating state values. -/// -/// Expects the query portion after `?`, e.g. `brightness=128&color=split&anim=pulse`. -/// Unknown keys are silently ignored. -fn parse_query_params(query: &str, state: &mut xiao_drone_led_controller::state::LedState) { - // Check for color/anim mode changes first — if present, reset params to defaults - // before applying per-mode overrides in the same request. - for pair in query.split('&') { - if let Some((key, value)) = pair.split_once('=') { - match key { - "color" => { - let new_mode = match value { - "solid_green" => Some(ColorMode::SolidGreen), - "solid_red" => Some(ColorMode::SolidRed), - "split" => Some(ColorMode::Split), - "rainbow" => Some(ColorMode::Rainbow), - _ => None, - }; - if let Some(m) = new_mode { - state.color_mode = m; - } + match ws.write(buf.iter().copied()) { + Err(e) if !write_err_logged => { + defmt::warn!("LED write error: {}", defmt::Debug2Format(&e)); + write_err_logged = true; } - "anim" => { - let new_mode = match value { - "static" => Some(AnimMode::Static), - "pulse" => Some(AnimMode::Pulse), - "ripple" => Some(AnimMode::Ripple), - _ => None, - }; - if let Some(m) = new_mode { - if m != state.anim_mode { - state.anim_mode = m; - state.anim_params = AnimModeParams::default_for(m); - } - } + Ok(_) if write_err_logged => { + info!("LED write recovered"); + write_err_logged = false; } _ => {} } - } - } - for pair in query.split('&') { - if let Some((key, value)) = pair.split_once('=') { - match key { - "brightness" => { - if let Ok(v) = value.parse::() { - state.brightness = v.min(255) as u8; - } - } - "num_leds" => { - if let Ok(v) = value.parse::() { - state.num_leds = v.clamp(1, MAX_NUM_LEDS); - } - } - "fps" => { - if let Ok(v) = value.parse::() { - state.fps = v.clamp(1, 150); - } - } - "max_current_ma" => { - if let Ok(v) = value.parse::() { - state.max_current_ma = v.clamp(100, 2500); - } - } - "color" | "anim" => { /* already handled above */ } - "bal_r" => { - if let Ok(v) = value.parse::() { - state.color_bal_r = v.min(255) as u8; - } - } - "bal_g" => { - if let Ok(v) = value.parse::() { - state.color_bal_g = v.min(255) as u8; - } - } - "bal_b" => { - if let Ok(v) = value.parse::() { - state.color_bal_b = v.min(255) as u8; - } - } - "use_hsi" => { - state.use_hsi = value == "1"; - } - "hue_speed" => { - if let Ok(v) = value.parse::() { - state.color_params.hue_speed = v.clamp(1, 10); - } - } - "pulse_speed" => { - if let Ok(v) = value.parse::() { - if let AnimModeParams::Pulse { speed, .. } = &mut state.anim_params { - *speed = v.clamp(100, 2000); - } - } - } - "min_brightness" => { - if let Ok(v) = value.parse::() { - if let AnimModeParams::Pulse { min_intensity_pct, .. } = &mut state.anim_params { - *min_intensity_pct = v.min(80); - } - } - } - "ripple_speed" => { - if let Ok(v) = value.parse::() { - if let AnimModeParams::Ripple { speed_x10, .. } = &mut state.anim_params { - *speed_x10 = v.clamp(5, 50); - } - } + frame_counter = frame_counter.wrapping_add(1); + Timer::after(Duration::from_millis(1000 / fps as u64)).await; + } else { + // Dithered path: Balance → Brightness → CurrentLimit → Gamma(fix16) → Dither loop + let pre_gamma_pipeline = [ + PostEffect::ColorBalance { r: bal_r, g: bal_g, b: bal_b }, + PostEffect::Brightness(led_brightness), + PostEffect::CurrentLimit { max_ma }, + ]; + apply_pipeline(active, &pre_gamma_pipeline); + + // Convert to 8.8 fixed-point gamma targets (once per animation frame) + dither::gamma_to_fix16(active, &mut fix16_targets[..num_leds]); + + // Inner dither loop: refresh strip at dither_fps rate + let sub_frames = (dither_fps as u32 / fps as u32).max(1); + let sub_frame_ms = 1000u64 / dither_fps as u64; + + for _ in 0..sub_frames { + dither_state.dither_frame( + dither_mode, + &fix16_targets[..num_leds], + &mut dither_output[..num_leds], + ); + + // Copy dithered output into main buffer for SPI write + buf[..num_leds].copy_from_slice(&dither_output[..num_leds]); + // Clear LEDs beyond active count + for led in buf[num_leds..].iter_mut() { + *led = RGB8 { r: 0, g: 0, b: 0 }; } - "ripple_width" => { - if let Ok(v) = value.parse::() { - if let AnimModeParams::Ripple { width_x10, .. } = &mut state.anim_params { - *width_x10 = v.clamp(10, 255); - } + + match ws.write(buf.iter().copied()) { + Err(e) if !write_err_logged => { + defmt::warn!("LED write error: {}", defmt::Debug2Format(&e)); + write_err_logged = true; } - } - "ripple_decay" => { - if let Ok(v) = value.parse::() { - if let AnimModeParams::Ripple { decay_pct, .. } = &mut state.anim_params { - *decay_pct = v.clamp(90, 99); - } + Ok(_) if write_err_logged => { + info!("LED write recovered"); + write_err_logged = false; } + _ => {} } - _ => {} + + frame_counter = frame_counter.wrapping_add(1); + Timer::after(Duration::from_millis(sub_frame_ms)).await; } } } } -/// Map a [`ColorMode`] to its query-string key. -fn color_key(mode: ColorMode) -> &'static str { - match mode { - ColorMode::SolidGreen => "solid_green", - ColorMode::SolidRed => "solid_red", - ColorMode::Split => "split", - ColorMode::Rainbow => "rainbow", - } -} - -/// Map an [`AnimMode`] to its query-string key. -fn anim_key(mode: AnimMode) -> &'static str { - match mode { - AnimMode::Static => "static", - AnimMode::Pulse => "pulse", - AnimMode::Ripple => "ripple", - } -} - -/// Build the HTML control page with current state values injected. -fn build_html_page(state: &xiao_drone_led_controller::state::LedState) -> alloc::string::String { - let brightness = state.brightness; - let num_leds = state.num_leds; - let fps = state.fps; - let max_current_ma = state.max_current_ma; - let color_mode = state.color_mode; - let anim_mode = state.anim_mode; - let anim_params = state.anim_params; - let hue_speed = state.color_params.hue_speed; - let bal_r = state.color_bal_r; - let bal_g = state.color_bal_g; - let bal_b = state.color_bal_b; - let use_hsi = state.use_hsi; - let hsi_checked = if use_hsi { " checked" } else { "" }; - - // Extract param values (use defaults for non-matching variants). - let (pulse_speed, min_brightness) = match anim_params { - AnimModeParams::Pulse { speed, min_intensity_pct } => (speed, min_intensity_pct), - _ => (600, 40), - }; - let (ripple_speed, ripple_width, ripple_decay) = match anim_params { - AnimModeParams::Ripple { speed_x10, width_x10, decay_pct } => (speed_x10, width_x10, decay_pct), - _ => (15, 190, 97), - }; - let csel = |key| if color_key(color_mode) == key { " selected" } else { "" }; - let asel = |key| if anim_key(anim_mode) == key { " selected" } else { "" }; - - let sel_solid_green = csel("solid_green"); - let sel_solid_red = csel("solid_red"); - let sel_split = csel("split"); - let sel_rainbow = csel("rainbow"); - let sel_static = asel("static"); - let sel_pulse = asel("pulse"); - let sel_ripple = asel("ripple"); - - alloc::format!( - r#" - - - -AirLED - - - -
-

AirLED

-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- - -"#, - brightness = brightness, - num_leds = num_leds, - max_leds = MAX_NUM_LEDS, - fps = fps, - max_current_ma = max_current_ma, - sel_solid_green = sel_solid_green, - sel_solid_red = sel_solid_red, - sel_split = sel_split, - sel_rainbow = sel_rainbow, - sel_static = sel_static, - sel_pulse = sel_pulse, - sel_ripple = sel_ripple, - pulse_speed = pulse_speed, - min_brightness = min_brightness, - ripple_speed = ripple_speed, - ripple_width = ripple_width, - ripple_decay = ripple_decay, - bal_r = bal_r, - bal_g = bal_g, - bal_b = bal_b, - hue_speed = hue_speed, - hsi_checked = hsi_checked, - ) -} -/// HTTP server with interactive LED control page. -#[embassy_executor::task] -async fn web_server(stack: Stack<'static>) { - // Wait until the stack is configured - loop { - if stack.is_config_up() { - break; - } - Timer::after(Duration::from_millis(100)).await; - } - info!("Web server listening on 192.168.4.1:80"); - - let mut rx_buffer = [0u8; 1024]; - let mut tx_buffer = [0u8; 4096]; - - loop { - let mut socket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer); - socket.set_timeout(Some(Duration::from_secs(10))); - - if let Err(_e) = socket.accept(80).await { - defmt::warn!("Accept error"); - continue; - } - - // Read HTTP request - let mut buf = [0u8; 512]; - let n = match socket.read(&mut buf).await { - Ok(0) | Err(_) => { - continue; - } - Ok(n) => n, - }; - - // Extract the request path from the first line (e.g. "GET /set?brightness=128 HTTP/1.1") - let request = core::str::from_utf8(&buf[..n]).unwrap_or(""); - let path = request - .split_once(' ') // skip method - .and_then(|(_, rest)| rest.split_once(' ')) // isolate path from HTTP version - .map(|(path, _)| path) - .unwrap_or("/"); - - if path.starts_with("/set") { - // Parse query params and update state - if let Some((_, query)) = path.split_once('?') { - let mut state = STATE.lock().await; - parse_query_params(query, &mut state); - } - - let response = b"HTTP/1.1 204 No Content\r\nConnection: close\r\n\r\n"; - let _ = socket.write_all(response).await; - } else { - // Serve the control page with current values - let state = STATE.lock().await; - let page = build_html_page(&state); - drop(state); - - let header = alloc::format!( - "HTTP/1.1 200 OK\r\nContent-Type: text/html\r\nContent-Length: {}\r\nConnection: close\r\n\r\n", - page.len() - ); - - let _ = socket.write_all(header.as_bytes()).await; - let _ = socket.write_all(page.as_bytes()).await; - } - - let _ = socket.flush().await; - socket.close(); - Timer::after(Duration::from_millis(50)).await; - } -} diff --git a/src/ble.rs b/src/ble.rs index 04693a5..bcf29c7 100644 --- a/src/ble.rs +++ b/src/ble.rs @@ -7,6 +7,7 @@ use heapless::String as HString; use serde::{Deserialize, Serialize}; +use crate::dither::DitherMode; use crate::state::{AnimMode, AnimModeParams, ColorMode, FlightMode, LedState}; /// Maximum number of active LEDs (mirrors `MAX_LEDS` in main). @@ -37,6 +38,14 @@ pub enum Command { SetRippleSpeed { value: u8 }, SetRippleWidth { value: u8 }, SetRippleDecay { value: u8 }, + SetDitherMode { mode: HString<16> }, + SetDitherFps { value: u16 }, + DisplayTestPattern { + color: HString<16>, + anim: HString<16>, + duration_ms: u16, + }, + CancelTestPattern, } // --------------------------------------------------------------------------- @@ -64,6 +73,10 @@ pub struct StateResponse { pub ripple_decay: u8, pub fc_connected: bool, pub flight_mode: &'static str, + pub tx_linked: bool, + pub dither_mode: &'static str, + pub dither_fps: u16, + pub test_active: bool, } // --------------------------------------------------------------------------- @@ -99,6 +112,27 @@ fn flight_mode_str(mode: FlightMode) -> &'static str { } } +/// Map a [`DitherMode`] to its wire-format string key. +pub fn dither_mode_str(mode: DitherMode) -> &'static str { + match mode { + DitherMode::Off => "off", + DitherMode::ErrorDiffusion => "error", + DitherMode::Ordered => "ordered", + DitherMode::Hybrid => "hybrid", + } +} + +/// Parse a dither mode string into a [`DitherMode`]. +fn parse_dither_mode(s: &str) -> Option { + match s { + "off" => Some(DitherMode::Off), + "error" => Some(DitherMode::ErrorDiffusion), + "ordered" => Some(DitherMode::Ordered), + "hybrid" => Some(DitherMode::Hybrid), + _ => None, + } +} + /// Parse a color mode string into a [`ColorMode`]. fn parse_color_mode(s: &str) -> Option { match s { @@ -161,6 +195,10 @@ pub fn build_state_response(state: &LedState) -> StateResponse { ripple_decay, fc_connected: state.fc_connected, flight_mode: flight_mode_str(state.flight_mode), + tx_linked: state.tx_linked, + dither_mode: dither_mode_str(state.dither_mode), + dither_fps: state.dither_fps, + test_active: state.test_pattern_frames > 0, } } @@ -262,6 +300,38 @@ pub fn handle_command(cmd: &Command, state: &mut LedState) -> HandleResult { } HandleResult::Ack } + Command::SetDitherMode { mode } => match parse_dither_mode(mode.as_str()) { + Some(m) => { + state.dither_mode = m; + HandleResult::Ack + } + None => HandleResult::Error("err:unknown_dither_mode\n"), + }, + Command::SetDitherFps { value } => { + state.dither_fps = (*value).clamp(100, 960); + HandleResult::Ack + } + Command::DisplayTestPattern { + color, + anim, + duration_ms, + } => { + let Some(c) = parse_color_mode(color.as_str()) else { + return HandleResult::Error("err:unknown_color_mode\n"); + }; + let Some(a) = parse_anim_mode(anim.as_str()) else { + return HandleResult::Error("err:unknown_anim_mode\n"); + }; + let frames = (state.fps as u32 * *duration_ms as u32) / 1000; + state.test_color = c; + state.test_anim = a; + state.test_pattern_frames = frames.max(1); + HandleResult::Ack + } + Command::CancelTestPattern => { + state.test_pattern_frames = 0; + HandleResult::Ack + } } } diff --git a/src/dither.rs b/src/dither.rs new file mode 100644 index 0000000..ce05f4d --- /dev/null +++ b/src/dither.rs @@ -0,0 +1,489 @@ +//! Temporal dithering for WS2812B LEDs. +//! +//! WS2812B LEDs have 8-bit per-channel resolution. After gamma correction, +//! low-brightness values get crushed (e.g., input 10 maps to output 0–1), +//! creating visible banding. By refreshing the strip faster than perceptual +//! flicker fusion (~60 Hz) and varying the quantized output frame-to-frame, +//! we represent fractional brightness through time-averaging — effectively +//! adding extra bit depth. +//! +//! All types are fixed-size, no-alloc, suitable for `no_std` embedded use. + +use smart_leds::RGB8; + +/// Maximum number of LEDs supported (must match `MAX_LEDS` in main). +const MAX_LEDS: usize = 200; + +/// 8.8 fixed-point: high byte = integer (0–255), low byte = fraction (0–255). +pub type Fix16 = u16; + +// --------------------------------------------------------------------------- +// Compile-time gamma LUTs (8.8 fixed-point output) +// --------------------------------------------------------------------------- + +/// Compute gamma correction for a single input value, returning 8.8 fixed-point. +/// +/// Uses an integer approximation: `(input/255)^gamma * 255` scaled to 16-bit. +/// The `gamma_x10` parameter is gamma × 10 (e.g., 26 for gamma 2.6). +const fn gamma_fix16(input: u8, gamma_x10: u32) -> Fix16 { + if input == 0 { + return 0; + } + if input == 255 { + return 255 << 8; + } + + // We compute (input/255)^gamma * 255 * 256 using integer math. + // Strategy: use repeated squaring on a fixed-point representation. + // Work in 32-bit with 16 fractional bits for intermediate precision. + + // input_norm = input / 255 in 0.16 fixed-point + let input_fp: u64 = (input as u64) << 16; + let norm: u64 = input_fp / 255; // 0.16 fixed-point, range [0, 65536] + + // Compute norm^(gamma_x10/10) using logarithms approximated by iteration. + // For better precision, we use pow by repeated multiplication. + // gamma_x10/10 = integer_part + fractional_part + let int_part = gamma_x10 / 10; + let frac_part = gamma_x10 % 10; // tenths + + // Compute norm^int_part (0.16 fixed-point) + let mut result: u64 = 1 << 16; // 1.0 in 0.16 + let mut i = 0; + while i < int_part { + result = (result * norm) >> 16; + i += 1; + } + + // For the fractional part, approximate norm^0.X by linear interpolation + // between norm^0 (=1) and norm^1 (=norm): norm^frac ≈ 1 + frac*(norm-1) + // This isn't perfectly accurate but is good enough for gamma LUTs. + if frac_part > 0 { + // Compute one more full power for interpolation + let next_power = (result * norm) >> 16; + // Interpolate: result + frac/10 * (next_power - result) + // = result * (10 - frac)/10 + next_power * frac/10 + result = (result * (10 - frac_part as u64) + next_power * frac_part as u64) / 10; + } + + // Scale from 0.16 to the output: result * 255 * 256 / 65536 + // = result * 255 * 256 >> 16 + // = result * 255 >> 8 (since 256 >> 16 = >> 8) + let out = (result * 255) >> 8; + + // Clamp to valid Fix16 range + if out > (255 << 8) { + 255 << 8 + } else { + out as Fix16 + } +} + +/// Build a 256-entry gamma LUT at compile time. +const fn build_gamma_lut(gamma_x10: u32) -> [Fix16; 256] { + let mut lut = [0u16; 256]; + let mut i = 0; + while i < 256 { + lut[i] = gamma_fix16(i as u8, gamma_x10); + i += 1; + } + lut +} + +/// Red channel gamma 2.6 correction LUT (8.8 fixed-point output). +const GAMMA_R_FIX16: [Fix16; 256] = build_gamma_lut(26); + +/// Green channel gamma 2.7 correction LUT (8.8 fixed-point output). +const GAMMA_G_FIX16: [Fix16; 256] = build_gamma_lut(27); + +/// Blue channel gamma 2.5 correction LUT (8.8 fixed-point output). +const GAMMA_B_FIX16: [Fix16; 256] = build_gamma_lut(25); + +// --------------------------------------------------------------------------- +// Dither mode +// --------------------------------------------------------------------------- + +/// Dithering algorithm selection. +#[derive(Clone, Copy, Debug, PartialEq, Eq, defmt::Format)] +pub enum DitherMode { + /// No dithering (bypass, use u8 gamma as before). + Off, + /// Temporal error diffusion (smooth gradients, best for slow fades). + ErrorDiffusion, + /// Ordered Bayer 4x4 (deterministic, good for strobes/fast changes). + Ordered, + /// Hybrid: error diffusion normally, correlated ordered when any channel < 10. + Hybrid, +} + +// --------------------------------------------------------------------------- +// Bayer 4x4 threshold matrix +// --------------------------------------------------------------------------- + +/// 4x4 Bayer ordered dither threshold matrix, scaled to 8.8 fixed-point. +/// +/// Standard Bayer matrix values (0-15) mapped to range [-128, +112] in +/// 8.8 fixed-point (i.e., -0.5 to +0.44 in the integer domain). +const BAYER_4X4: [i16; 16] = { + // Standard Bayer 4x4 normalized positions: 0/16, 8/16, 2/16, 10/16, ... + // Map to [-128, +112] (8.8 fixed-point, covering roughly ±0.5) + let matrix: [u8; 16] = [ + 0, 8, 2, 10, + 12, 4, 14, 6, + 3, 11, 1, 9, + 15, 7, 13, 5, + ]; + let mut result = [0i16; 16]; + let mut i = 0; + while i < 16 { + // Map 0..15 to -128..+112 (step of 16 in 8.8 = step of 1/16 in integer) + result[i] = (matrix[i] as i16) * 16 - 128; + i += 1; + } + result +}; + +// --------------------------------------------------------------------------- +// Dither state +// --------------------------------------------------------------------------- + +/// Per-LED, per-channel error accumulators and frame counter for temporal dithering. +pub struct DitherState { + /// Per-LED per-channel error accumulator (signed 8.8 fixed-point). + error: [[i16; 3]; MAX_LEDS], + /// Frame counter for ordered dithering. + frame: u32, +} + +impl Default for DitherState { + fn default() -> Self { + Self::new() + } +} + +impl DitherState { + /// Create a new dither state with zeroed accumulators. + pub const fn new() -> Self { + Self { + error: [[0i16; 3]; MAX_LEDS], + frame: 0, + } + } + + /// Reset all error accumulators (call on scene/mode change). + pub fn reset(&mut self) { + for e in self.error.iter_mut() { + *e = [0; 3]; + } + } + + /// Apply one dither frame, converting Fix16 targets to quantized RGB8 output. + /// + /// `targets` contains 8.8 fixed-point gamma-corrected values per LED per channel. + /// `output` receives the quantized u8 values for this frame. + pub fn dither_frame( + &mut self, + mode: DitherMode, + targets: &[[Fix16; 3]], + output: &mut [RGB8], + ) { + let len = targets.len().min(output.len()).min(MAX_LEDS); + + match mode { + DitherMode::Off => { + // Simple rounding, no dithering + for i in 0..len { + output[i] = RGB8 { + r: ((targets[i][0] + 128) >> 8).min(255) as u8, + g: ((targets[i][1] + 128) >> 8).min(255) as u8, + b: ((targets[i][2] + 128) >> 8).min(255) as u8, + }; + } + } + DitherMode::ErrorDiffusion => { + self.dither_error_diffusion(targets, output, len); + } + DitherMode::Ordered => { + self.dither_ordered(targets, output, len, false); + } + DitherMode::Hybrid => { + self.dither_hybrid(targets, output, len); + } + } + + self.frame = self.frame.wrapping_add(1); + } + + /// Error diffusion dithering: accumulates quantization error per LED per channel. + fn dither_error_diffusion( + &mut self, + targets: &[[Fix16; 3]], + output: &mut [RGB8], + len: usize, + ) { + for i in 0..len { + let mut rgb = [0u8; 3]; + for ch in 0..3 { + let target = targets[i][ch] as i16; + let corrected = target.saturating_add(self.error[i][ch]); + // Round to nearest u8 (add 0.5 in 8.8 = 128, then shift) + let quantized = ((corrected.max(0) + 128) >> 8).min(255) as u8; + // New error = corrected - quantized (in 8.8) + let new_error = corrected - ((quantized as i16) << 8); + // Clamp error to prevent runaway accumulation + self.error[i][ch] = new_error.clamp(-256, 256); + rgb[ch] = quantized; + } + output[i] = RGB8 { r: rgb[0], g: rgb[1], b: rgb[2] }; + } + } + + /// Ordered Bayer 4x4 dithering using spatial LED index + temporal frame index. + /// + /// When `correlated` is true, the same threshold is used for all 3 channels + /// (preserves hue at very low brightness). Otherwise each channel uses a + /// different column offset for less visible patterning. + fn dither_ordered( + &mut self, + targets: &[[Fix16; 3]], + output: &mut [RGB8], + len: usize, + correlated: bool, + ) { + let frame_row = (self.frame as usize) & 3; // row = frame mod 4 + for i in 0..len { + let col = i & 3; // column = LED index mod 4 + let idx_base = frame_row * 4 + col; + let threshold = BAYER_4X4[idx_base & 15]; + + let mut rgb = [0u8; 3]; + for ch in 0..3 { + let t = if correlated { + threshold + } else { + // Offset each channel by a different amount to decorrelate + BAYER_4X4[(idx_base + ch * 5) & 15] + }; + let target = targets[i][ch] as i16; + let dithered = target + t; + rgb[ch] = ((dithered.max(0)) >> 8).min(255) as u8; + } + output[i] = RGB8 { r: rgb[0], g: rgb[1], b: rgb[2] }; + } + } + + /// Hybrid dithering: error diffusion for normal brightness, correlated ordered + /// when any channel target is below threshold (integer part < 10). + fn dither_hybrid( + &mut self, + targets: &[[Fix16; 3]], + output: &mut [RGB8], + len: usize, + ) { + let low_threshold: u16 = 10 << 8; + let frame_row = (self.frame as usize) & 3; + + for i in 0..len { + let any_low = targets[i][0] < low_threshold + || targets[i][1] < low_threshold + || targets[i][2] < low_threshold; + + if any_low { + // Use correlated ordered dithering for low-brightness LEDs + let col = i & 3; + let idx_base = frame_row * 4 + col; + let threshold = BAYER_4X4[idx_base & 15]; + + let mut rgb = [0u8; 3]; + for ch in 0..3 { + let target = targets[i][ch] as i16; + let dithered = target + threshold; + rgb[ch] = ((dithered.max(0)) >> 8).min(255) as u8; + } + // Reset error accumulator since we're not using error diffusion + self.error[i] = [0; 3]; + output[i] = RGB8 { r: rgb[0], g: rgb[1], b: rgb[2] }; + } else { + // Use error diffusion for normal brightness + let mut rgb = [0u8; 3]; + for ch in 0..3 { + let target = targets[i][ch] as i16; + let corrected = target.saturating_add(self.error[i][ch]); + let quantized = ((corrected.max(0) + 128) >> 8).min(255) as u8; + let new_error = corrected - ((quantized as i16) << 8); + self.error[i][ch] = new_error.clamp(-256, 256); + rgb[ch] = quantized; + } + output[i] = RGB8 { r: rgb[0], g: rgb[1], b: rgb[2] }; + } + } + + self.frame = self.frame.wrapping_add(1); + } +} + +// --------------------------------------------------------------------------- +// Gamma conversion (RGB8 → Fix16 targets) +// --------------------------------------------------------------------------- + +/// Apply gamma correction to an RGB8 buffer, producing 8.8 fixed-point targets. +/// +/// This runs once per animation frame. The dither loop then repeatedly +/// quantizes these targets to produce slightly different u8 outputs each sub-frame. +pub fn gamma_to_fix16(src: &[RGB8], dst: &mut [[Fix16; 3]]) { + let len = src.len().min(dst.len()); + for i in 0..len { + dst[i] = [ + GAMMA_R_FIX16[src[i].r as usize], + GAMMA_G_FIX16[src[i].g as usize], + GAMMA_B_FIX16[src[i].b as usize], + ]; + } +} + +// --------------------------------------------------------------------------- +// Unit tests +// --------------------------------------------------------------------------- + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn fix16_gamma_endpoints() { + // Input 0 → output 0 for all channels + assert_eq!(GAMMA_R_FIX16[0], 0); + assert_eq!(GAMMA_G_FIX16[0], 0); + assert_eq!(GAMMA_B_FIX16[0], 0); + // Input 255 → output 255.0 (= 255 << 8 = 65280) + assert_eq!(GAMMA_R_FIX16[255], 255 << 8); + assert_eq!(GAMMA_G_FIX16[255], 255 << 8); + assert_eq!(GAMMA_B_FIX16[255], 255 << 8); + } + + #[test] + fn fix16_gamma_monotonic() { + // All LUTs should be monotonically non-decreasing + for lut in [&GAMMA_R_FIX16, &GAMMA_G_FIX16, &GAMMA_B_FIX16] { + for i in 1..256 { + assert!( + lut[i] >= lut[i - 1], + "LUT not monotonic at index {}: {} < {}", + i, + lut[i], + lut[i - 1] + ); + } + } + } + + #[test] + fn fix16_gamma_green_steeper_than_red() { + // Green gamma (2.7) is steeper than red (2.6), so mid-range green < red + assert!(GAMMA_G_FIX16[128] < GAMMA_R_FIX16[128]); + } + + #[test] + fn error_diffusion_converges() { + // A constant target of 0.5 (Fix16 = 128) should produce alternating 0/1 + // and the error should stay bounded. + let mut state = DitherState::new(); + let targets = [[128u16; 3]; 1]; // 0.5 in 8.8 for all channels + let mut output = [RGB8 { r: 0, g: 0, b: 0 }; 1]; + + let mut sum = [0u32; 3]; + let n = 100; + for _ in 0..n { + state.dither_frame(DitherMode::ErrorDiffusion, &targets, &mut output); + sum[0] += output[0].r as u32; + sum[1] += output[0].g as u32; + sum[2] += output[0].b as u32; + } + + // Average should be close to 0.5 (either 0 or 1 each frame) + // With error diffusion, target 0.5 should produce ~50% ones + for ch in 0..3 { + let avg_x100 = sum[ch] * 100 / n; + assert!( + avg_x100 >= 30 && avg_x100 <= 70, + "channel {} average {}/100 outside expected range", + ch, + avg_x100, + ); + } + + // Error should be bounded + for ch in 0..3 { + assert!( + state.error[0][ch].unsigned_abs() <= 256, + "error[0][{}] = {} exceeds bounds", + ch, + state.error[0][ch], + ); + } + } + + #[test] + fn ordered_dither_deterministic() { + // Same inputs + same frame counter → same outputs + let targets = [[512u16; 3]; 4]; // 2.0 in 8.8 + let mut output_a = [RGB8 { r: 0, g: 0, b: 0 }; 4]; + let mut output_b = [RGB8 { r: 0, g: 0, b: 0 }; 4]; + + let mut state_a = DitherState::new(); + let mut state_b = DitherState::new(); + + state_a.dither_frame(DitherMode::Ordered, &targets, &mut output_a); + state_b.dither_frame(DitherMode::Ordered, &targets, &mut output_b); + + for i in 0..4 { + assert_eq!(output_a[i], output_b[i], "mismatch at LED {}", i); + } + } + + #[test] + fn gamma_to_fix16_roundtrip() { + // Full white should map to [255<<8, 255<<8, 255<<8] + let src = [RGB8 { r: 255, g: 255, b: 255 }]; + let mut dst = [[0u16; 3]; 1]; + gamma_to_fix16(&src, &mut dst); + assert_eq!(dst[0], [255 << 8, 255 << 8, 255 << 8]); + + // Black should map to [0, 0, 0] + let src = [RGB8 { r: 0, g: 0, b: 0 }]; + gamma_to_fix16(&src, &mut dst); + assert_eq!(dst[0], [0, 0, 0]); + } + + #[test] + fn dither_off_rounds_correctly() { + let mut state = DitherState::new(); + // Target: 2.6 in 8.8 = (2 << 8) + 153 = 665 + let targets = [[665u16; 3]; 1]; + let mut output = [RGB8 { r: 0, g: 0, b: 0 }; 1]; + state.dither_frame(DitherMode::Off, &targets, &mut output); + // (665 + 128) >> 8 = 793 >> 8 = 3 (rounds 2.6 to 3) + assert_eq!(output[0].r, 3); + } + + #[test] + fn hybrid_uses_ordered_for_low_brightness() { + // Targets below threshold (< 10 << 8 = 2560) should use ordered dithering + let mut state = DitherState::new(); + let targets = [[256u16; 3]; 4]; // 1.0 in 8.8 — below threshold + let mut output = [RGB8 { r: 0, g: 0, b: 0 }; 4]; + state.dither_frame(DitherMode::Hybrid, &targets, &mut output); + // Should produce valid output without panicking + for px in &output[..4] { + assert!(px.r <= 2 && px.g <= 2 && px.b <= 2); + } + } + + #[test] + fn reset_clears_error() { + let mut state = DitherState::new(); + state.error[0] = [100, -200, 50]; + state.reset(); + assert_eq!(state.error[0], [0, 0, 0]); + } +} diff --git a/src/lib.rs b/src/lib.rs index 436d61a..b60080b 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,6 +1,7 @@ #![no_std] pub mod ble; +pub mod dither; pub mod msp; pub mod pattern; pub mod postfx; diff --git a/src/msp.rs b/src/msp.rs index 00e9e44..3d473b2 100644 --- a/src/msp.rs +++ b/src/msp.rs @@ -16,6 +16,9 @@ pub const MSP_BOXNAMES: u8 = 116; /// MSP command: RC channel values (16 × u16 LE, 1000–2000 µs). pub const MSP_RC: u8 = 105; +/// MSP command: analog values (vbat, mAh drawn, RSSI, amps). +pub const MSP_ANALOG: u8 = 110; + /// MSP command: box IDs (permanent numeric IDs, one byte each). pub const MSP_BOXIDS: u8 = 119; @@ -310,6 +313,22 @@ pub fn parse_rc_channels(payload: &[u8], size: u8, out: &mut [u16; MAX_RC_CHANNE count } +// --------------------------------------------------------------------------- +// Analog / RSSI parser +// --------------------------------------------------------------------------- + +/// Extract the RSSI value from an MSP_ANALOG response payload. +/// +/// MSP_ANALOG payload: `[vbat: u8, mah_drawn: u16 LE, rssi: u16 LE, amps: i16 LE]`. +/// RSSI is at bytes 3–4 as a u16 LE value (0–1023 in Betaflight). +/// Returns `None` if the payload is too short. +pub fn extract_rssi(payload: &[u8], size: u8) -> Option { + if size < 5 { + return None; + } + Some(u16::from_le_bytes([payload[3], payload[4]])) +} + // --------------------------------------------------------------------------- // Flight mode resolution // --------------------------------------------------------------------------- @@ -566,4 +585,5 @@ mod tests { let mode = resolve_flight_mode(0, &box_map, 0x0004); assert_eq!(mode, FlightMode::ArmingForbidden); } + } diff --git a/src/state.rs b/src/state.rs index 87b4cdf..0fa16fd 100644 --- a/src/state.rs +++ b/src/state.rs @@ -7,6 +7,8 @@ use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex; use embassy_sync::mutex::Mutex; use embassy_sync::signal::Signal; +use crate::dither::DitherMode; + /// Active color scheme. #[derive(Clone, Copy, Debug, PartialEq, Eq, defmt::Format)] pub enum ColorMode { @@ -135,8 +137,24 @@ pub struct LedState { pub debug_arm_box: u8, /// Index of the FAILSAFE box in the BOXNAMES map (255 = not found). pub debug_failsafe_box: u8, - /// AUX7 strobe intensity (0 = off, nonzero = peak brightness). + /// AUX strobe intensity (0 = off, nonzero = peak brightness). pub aux_strobe: u8, + /// When true, strobe uses position-light colours (red port / green starboard). + pub strobe_split: bool, + /// Whether the RC transmitter has an active link (RSSI > 0). + pub tx_linked: bool, + /// Temporal dithering method (Off, ErrorDiffusion, Ordered, Hybrid). + pub dither_mode: DitherMode, + /// Dither refresh rate in Hz (100–960). Only used when dither_mode != Off. + /// Animation updates still happen at `fps` rate; the strip is refreshed + /// at this rate with dithered sub-frames between animation updates. + pub dither_fps: u16, + /// Remaining frames for a BLE test pattern (0 = inactive). + pub test_pattern_frames: u32, + /// Color mode for the active test pattern. + pub test_color: ColorMode, + /// Animation mode for the active test pattern. + pub test_anim: AnimMode, } impl Default for LedState { @@ -160,6 +178,13 @@ impl Default for LedState { debug_arm_box: 255, debug_failsafe_box: 255, aux_strobe: 0, + strobe_split: false, + tx_linked: false, + dither_mode: DitherMode::Off, + dither_fps: 300, + test_pattern_frames: 0, + test_color: ColorMode::Split, + test_anim: AnimMode::Static, } } } @@ -200,4 +225,11 @@ pub static STATE: Mutex = Mutex::new(LedState debug_arm_box: 255, debug_failsafe_box: 255, aux_strobe: 0, + strobe_split: false, + tx_linked: false, + dither_mode: DitherMode::Off, + dither_fps: 300, + test_pattern_frames: 0, + test_color: ColorMode::Split, + test_anim: AnimMode::Static, });