From 1ee6fb7605a78817247d1d8044519892902cf1b1 Mon Sep 17 00:00:00 2001 From: Moritz Riede Date: Thu, 19 Feb 2026 03:02:34 +0100 Subject: [PATCH 1/5] feat: gate AUX strobe on TX link, remove Wi-Fi stack MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add RC-based TX link detection: when all 4 stick channels (AETR) read exactly 1500 µs, no transmitter is bound and strobe is suppressed. Prevents false strobe triggers from unassigned AUX channels defaulting to 1500 µs center position. Remove Wi-Fi AP, HTTP web UI, DHCP server, and related dependencies (embassy-net, smoltcp, edge-dhcp, embedded-io). BLE is now the sole control interface. Expose tx_linked field in BLE StateResponse for app display. --- CHANGELOG.md | 11 + Cargo.lock | 101 -------- Cargo.toml | 23 +- FLUTTER_BLE_GUIDE.md | 6 +- src/bin/main.rs | 550 +++---------------------------------------- src/ble.rs | 2 + src/msp.rs | 1 + src/state.rs | 4 + 8 files changed, 55 insertions(+), 643 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 178c47c..bdceb1f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,17 @@ and this project adheres to [Conventional Commits](https://www.conventionalcommi ### Added +- TX link detection via RC stick channels — if all 4 sticks (AETR) read exactly 1500 µs, no TX is bound; strobe only activates when TX is linked +- `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 - `ble` module (`src/ble.rs`): Command/Response types with serde, command handler, state snapshot builder, JSON serialization helpers — all `no_std`, no heap, unit-testable - `ble_task`: async task advertising as "AirLED", serving NUS GATT service with chunked notifications 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..416168d 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 @@ -242,7 +242,8 @@ 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 } ``` @@ -268,6 +269,7 @@ 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) | ### Flight modes diff --git a/src/bin/main.rs b/src/bin/main.rs index 4d9479f..266ef24 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; @@ -51,7 +42,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 +49,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 +89,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 +120,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; @@ -308,7 +244,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 +269,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> = @@ -561,13 +497,14 @@ async fn msp_task(mut uart: Uart<'static, esp_hal::Async>) { } info!("MSP: entering poll loop"); - // --- Phase 2: poll MSP_STATUS + MSP_RC every tick (~20 Hz) --- + // --- Phase 2: poll MSP_STATUS + MSP_RC + MSP_ANALOG every tick (~20 Hz) --- let mut error_count: u8 = 0; let mut logged_raw_status = false; let mut rc_channels = [0u16; msp::MAX_RC_CHANNELS]; 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 tx_linked = false; loop { // Retry box map if we never got it (FC wasn't ready at startup) @@ -662,6 +599,8 @@ async fn msp_task(mut uart: Uart<'static, esp_hal::Async>) { state.fc_connected = false; state.flight_mode = FlightMode::ArmingForbidden; state.aux_strobe = 0; + state.tx_linked = false; + tx_linked = false; drop(state); // Reset counter to avoid spamming state writes every tick error_count = 10; @@ -692,11 +631,31 @@ async fn msp_task(mut uart: Uart<'static, esp_hal::Async>) { ); } + // TX link detection: when no TX is bound all 4 stick + // channels (AETR) sit at exactly 1500 µs. + if count >= 4 { + let linked = !(rc_channels[0] == 1500 + && rc_channels[1] == 1500 + && rc_channels[2] == 1500 + && rc_channels[3] == 1500); + if linked != tx_linked { + info!("MSP: TX link {}", if linked { "up" } else { "down" }); + tx_linked = linked; + let mut state = STATE.lock().await; + state.tx_linked = linked; + if !linked { + state.aux_strobe = 0; + } + drop(state); + STATE_CHANGED.signal(()); + } + } + // 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 + // Suppress strobe for first 10s after boot and when TX is not linked let uptime_ms = embassy_time::Instant::now().as_millis(); - if count >= 12 && uptime_ms > 10_000 { + if count >= 12 && uptime_ms > 10_000 && tx_linked { let aux7 = rc_channels[10]; let aux8 = rc_channels[11]; let strobe_level: u8 = if aux8 > 1800 { @@ -916,7 +875,7 @@ async fn led_task(spi_bus: SpiDmaBus<'static, esp_hal::Blocking>) { // - 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 { @@ -1009,450 +968,5 @@ async fn led_task(spi_bus: SpiDmaBus<'static, esp_hal::Blocking>) { } } -/// 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)); - } - } - } - } -} - -/// 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; - } - } - "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); - } - } - } - _ => {} - } - } - } - - 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); - } - } - } - "ripple_width" => { - if let Ok(v) = value.parse::() { - if let AnimModeParams::Ripple { width_x10, .. } = &mut state.anim_params { - *width_x10 = v.clamp(10, 255); - } - } - } - "ripple_decay" => { - if let Ok(v) = value.parse::() { - if let AnimModeParams::Ripple { decay_pct, .. } = &mut state.anim_params { - *decay_pct = v.clamp(90, 99); - } - } - } - _ => {} - } - } - } -} - -/// 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..05dccfe 100644 --- a/src/ble.rs +++ b/src/ble.rs @@ -64,6 +64,7 @@ pub struct StateResponse { pub ripple_decay: u8, pub fc_connected: bool, pub flight_mode: &'static str, + pub tx_linked: bool, } // --------------------------------------------------------------------------- @@ -161,6 +162,7 @@ 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, } } diff --git a/src/msp.rs b/src/msp.rs index 00e9e44..9aa446a 100644 --- a/src/msp.rs +++ b/src/msp.rs @@ -566,4 +566,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..571b121 100644 --- a/src/state.rs +++ b/src/state.rs @@ -137,6 +137,8 @@ pub struct LedState { pub debug_failsafe_box: u8, /// AUX7 strobe intensity (0 = off, nonzero = peak brightness). pub aux_strobe: u8, + /// Whether the RC transmitter has an active link (RSSI > 0). + pub tx_linked: bool, } impl Default for LedState { @@ -160,6 +162,7 @@ impl Default for LedState { debug_arm_box: 255, debug_failsafe_box: 255, aux_strobe: 0, + tx_linked: false, } } } @@ -200,4 +203,5 @@ pub static STATE: Mutex = Mutex::new(LedState debug_arm_box: 255, debug_failsafe_box: 255, aux_strobe: 0, + tx_linked: false, }); From 10abeacd3d5f8f591a5940df3447f77e9d232f1d Mon Sep 17 00:00:00 2001 From: Moritz Riede Date: Fri, 20 Feb 2026 23:50:14 +0100 Subject: [PATCH 2/5] feat: temporal dithering, gate strobe on flight mode MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add temporal dithering module (src/dither.rs) with three algorithms: error diffusion, ordered Bayer 4x4, and hybrid (default). 8.8 fixed-point gamma LUTs computed at compile time provide sub-u8 precision. The LED task now runs an inner dither loop at dither_fps rate (100-960 Hz) between animation frames. Gate AUX strobe on flight_mode != ArmingForbidden instead of RSSI-based tx_linked heuristic — the FC's own state machine already validates RX link, eliminating false strobe triggers from noisy RSSI when no TX is connected. BLE commands: SetDitherMode, SetDitherFps, DisplayTestPattern, CancelTestPattern. Default dither mode is Hybrid at 300 Hz. --- CHANGELOG.md | 13 +- FLUTTER_BLE_GUIDE.md | 30 ++- src/bin/main.rs | 204 ++++++++++++++---- src/ble.rs | 68 ++++++ src/dither.rs | 489 +++++++++++++++++++++++++++++++++++++++++++ src/lib.rs | 1 + src/msp.rs | 19 ++ src/state.rs | 24 +++ 8 files changed, 801 insertions(+), 47 deletions(-) create mode 100644 src/dither.rs diff --git a/CHANGELOG.md b/CHANGELOG.md index bdceb1f..5972f85 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,7 +9,18 @@ and this project adheres to [Conventional Commits](https://www.conventionalcommi ### Added -- TX link detection via RC stick channels — if all 4 sticks (AETR) read exactly 1500 µs, no TX is bound; strobe only activates when TX is linked +- 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 diff --git a/FLUTTER_BLE_GUIDE.md b/FLUTTER_BLE_GUIDE.md index 416168d..77847b4 100644 --- a/FLUTTER_BLE_GUIDE.md +++ b/FLUTTER_BLE_GUIDE.md @@ -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. @@ -243,7 +269,8 @@ Full JSON state snapshot. Approximately 250 bytes serialized. "ripple_decay": 97, "fc_connected": false, "flight_mode": "arming_forbidden", - "tx_linked": false + "tx_linked": false, + "test_active": false } ``` @@ -270,6 +297,7 @@ Full JSON state snapshot. Approximately 250 bytes serialized. | `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 266ef24..199d2d8 100644 --- a/src/bin/main.rs +++ b/src/bin/main.rs @@ -30,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, @@ -497,14 +498,14 @@ async fn msp_task(mut uart: Uart<'static, esp_hal::Async>) { } info!("MSP: entering poll loop"); - // --- Phase 2: poll MSP_STATUS + MSP_RC + MSP_ANALOG every tick (~20 Hz) --- + // --- Phase 2: poll MSP_STATUS + MSP_RC every tick (~20 Hz) --- let mut error_count: u8 = 0; let mut logged_raw_status = false; let mut rc_channels = [0u16; msp::MAX_RC_CHANNELS]; 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 tx_linked = false; + let mut flight_mode = FlightMode::ArmingForbidden; loop { // Retry box map if we never got it (FC wasn't ready at startup) @@ -571,6 +572,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)); @@ -579,8 +581,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; @@ -595,12 +604,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; - tx_linked = false; drop(state); // Reset counter to avoid spamming state writes every tick error_count = 10; @@ -631,31 +640,12 @@ async fn msp_task(mut uart: Uart<'static, esp_hal::Async>) { ); } - // TX link detection: when no TX is bound all 4 stick - // channels (AETR) sit at exactly 1500 µs. - if count >= 4 { - let linked = !(rc_channels[0] == 1500 - && rc_channels[1] == 1500 - && rc_channels[2] == 1500 - && rc_channels[3] == 1500); - if linked != tx_linked { - info!("MSP: TX link {}", if linked { "up" } else { "down" }); - tx_linked = linked; - let mut state = STATE.lock().await; - state.tx_linked = linked; - if !linked { - state.aux_strobe = 0; - } - drop(state); - STATE_CHANGED.signal(()); - } - } - // AUX7 (channel 11, index 10) 3-position strobe // AUX8 (channel 12, index 11) spring switch override → full - // Suppress strobe for first 10s after boot and when TX is not linked + // 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 && tx_linked { + 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 { @@ -746,7 +736,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; @@ -788,8 +789,19 @@ 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 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 }; @@ -797,7 +809,9 @@ 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; @@ -832,6 +846,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 }; @@ -871,6 +886,57 @@ 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 @@ -943,28 +1009,76 @@ 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; + 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); + + 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; + } + _ => {} } - 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; + } 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 }; + } + + 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(sub_frame_ms)).await; } - _ => {} } - - frame_counter = frame_counter.wrapping_add(1); - Timer::after(Duration::from_millis(1000 / fps as u64)).await; } } diff --git a/src/ble.rs b/src/ble.rs index 05dccfe..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, } // --------------------------------------------------------------------------- @@ -65,6 +74,9 @@ pub struct StateResponse { 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, } // --------------------------------------------------------------------------- @@ -100,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 { @@ -163,6 +196,9 @@ pub fn build_state_response(state: &LedState) -> StateResponse { 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, } } @@ -264,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 9aa446a..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 // --------------------------------------------------------------------------- diff --git a/src/state.rs b/src/state.rs index 571b121..0497d37 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 { @@ -139,6 +141,18 @@ pub struct LedState { pub aux_strobe: u8, /// 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 { @@ -163,6 +177,11 @@ impl Default for LedState { debug_failsafe_box: 255, aux_strobe: 0, tx_linked: false, + dither_mode: DitherMode::Hybrid, + dither_fps: 300, + test_pattern_frames: 0, + test_color: ColorMode::Split, + test_anim: AnimMode::Static, } } } @@ -204,4 +223,9 @@ pub static STATE: Mutex = Mutex::new(LedState debug_failsafe_box: 255, aux_strobe: 0, tx_linked: false, + dither_mode: DitherMode::Hybrid, + dither_fps: 300, + test_pattern_frames: 0, + test_color: ColorMode::Split, + test_anim: AnimMode::Static, }); From 0222a7fa7c75ab72aabec17df021205bedd8fa81 Mon Sep 17 00:00:00 2001 From: Moritz Riede Date: Sat, 21 Feb 2026 04:12:14 +0100 Subject: [PATCH 3/5] fix: disable temporal dithering by default --- src/state.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/state.rs b/src/state.rs index 0497d37..aad4b91 100644 --- a/src/state.rs +++ b/src/state.rs @@ -177,7 +177,7 @@ impl Default for LedState { debug_failsafe_box: 255, aux_strobe: 0, tx_linked: false, - dither_mode: DitherMode::Hybrid, + dither_mode: DitherMode::Off, dither_fps: 300, test_pattern_frames: 0, test_color: ColorMode::Split, @@ -223,7 +223,7 @@ pub static STATE: Mutex = Mutex::new(LedState debug_failsafe_box: 255, aux_strobe: 0, tx_linked: false, - dither_mode: DitherMode::Hybrid, + dither_mode: DitherMode::Off, dither_fps: 300, test_pattern_frames: 0, test_color: ColorMode::Split, From 577e0cba5325bb35bf38f1ace4f4db24bef6067b Mon Sep 17 00:00:00 2001 From: Moritz Riede Date: Sun, 22 Feb 2026 05:00:30 +0100 Subject: [PATCH 4/5] feat: position-light strobe mode and BLE TX append buffering Remap AUX strobe switches: AUX8 momentary and AUX7 mid both trigger white strobe at 80, AUX7 high triggers red/green position-light strobe. Add strobe_split field to LedState for split colour rendering. BLE TX responses are now appended after unsent data instead of overwriting, with a compact step to reclaim consumed buffer space. --- CHANGELOG.md | 5 +++ src/bin/main.rs | 106 ++++++++++++++++++++++++++++++++++++------------ src/state.rs | 6 ++- 3 files changed, 89 insertions(+), 28 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5972f85..9022122 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,11 @@ and this project adheres to [Conventional Commits](https://www.conventionalcommi ## [Unreleased] +### Changed + +- Remap AUX strobe switches: AUX8 momentary → white strobe at 80, AUX7 mid → white strobe at 80, AUX7 high → position-light strobe (red port / green starboard) 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 diff --git a/src/bin/main.rs b/src/bin/main.rs index 199d2d8..f1a21bb 100644 --- a/src/bin/main.rs +++ b/src/bin/main.rs @@ -160,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; @@ -183,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; } } }); @@ -392,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 } @@ -641,27 +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 + // pos 1 (low) → off + // pos 2 (mid) → white strobe at 80 + // pos 3 (high) → position-light strobe (red/green) 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 && 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, true) // AUX7 pos 3 → position-light (red/green) } else if aux7 > 1250 { - 80 // AUX7 position 2 → low + (80, false) // AUX7 pos 2 → mid white } 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) @@ -789,6 +827,7 @@ 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; @@ -823,9 +862,22 @@ async fn led_task(spi_bus: SpiDmaBus<'static, esp_hal::Blocking>) { let off_phase = phase - STROBE_HALF; ((STROBE_HALF - off_phase) as u16 * peak as u16 / STROBE_HALF as u16) as u8 }; - let color = RGB8 { r: intensity, g: intensity, b: intensity }; - for led in active.iter_mut() { - *led = color; + if strobe_split { + // Position-light strobe: red port / green starboard + let half = active.len() / 2; + let red = RGB8 { r: intensity, g: 0, b: 0 }; + let green = RGB8 { r: 0, g: intensity, b: 0 }; + for led in active[..half].iter_mut() { + *led = red; + } + for led in active[half..].iter_mut() { + *led = green; + } + } else { + let color = RGB8 { r: intensity, g: intensity, b: intensity }; + for led in active.iter_mut() { + *led = color; + } } match ws.write(buf.iter().copied()) { diff --git a/src/state.rs b/src/state.rs index aad4b91..0fa16fd 100644 --- a/src/state.rs +++ b/src/state.rs @@ -137,8 +137,10 @@ 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). @@ -176,6 +178,7 @@ 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, @@ -222,6 +225,7 @@ 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, From 93fe30c8bd0c2c90db843a50c80da6615151ed7a Mon Sep 17 00:00:00 2001 From: Moritz Riede Date: Sun, 22 Feb 2026 05:11:56 +0100 Subject: [PATCH 5/5] fix: swap AUX7 strobe order, quadratic curve, and color corrections MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Reorder AUX7 three-way: off → red/green position-light → white. Use quadratic envelope for sharper strobe flash. Scale green to 75% to match perceived red brightness. Fix port/starboard color swap. --- CHANGELOG.md | 2 +- src/bin/main.rs | 27 ++++++++++++++++----------- 2 files changed, 17 insertions(+), 12 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9022122..3c0e6fa 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,7 +9,7 @@ and this project adheres to [Conventional Commits](https://www.conventionalcommi ### Changed -- Remap AUX strobe switches: AUX8 momentary → white strobe at 80, AUX7 mid → white strobe at 80, AUX7 high → position-light strobe (red port / green starboard) at 80 +- 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 diff --git a/src/bin/main.rs b/src/bin/main.rs index f1a21bb..de29ec0 100644 --- a/src/bin/main.rs +++ b/src/bin/main.rs @@ -676,8 +676,8 @@ async fn msp_task(mut uart: Uart<'static, esp_hal::Async>) { // AUX7 (channel 11, index 10) 3-position strobe // pos 1 (low) → off - // pos 2 (mid) → white strobe at 80 - // pos 3 (high) → position-light strobe (red/green) at 80 + // 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). @@ -688,9 +688,9 @@ async fn msp_task(mut uart: Uart<'static, esp_hal::Async>) { let (strobe_level, strobe_split): (u8, bool) = if aux8 > 1800 { (80, false) // AUX8 momentary → mid white } else if aux7 > 1650 { - (80, true) // AUX7 pos 3 → position-light (red/green) + (80, false) // AUX7 pos 3 → white } else if aux7 > 1250 { - (80, false) // AUX7 pos 2 → mid white + (80, true) // AUX7 pos 2 → position-light (red/green) } else { (0, false) // off }; @@ -856,22 +856,27 @@ async fn led_task(spi_bus: SpiDmaBus<'static, esp_hal::Blocking>) { 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 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: intensity, b: 0 }; + let green = RGB8 { r: 0, g: green_val, b: 0 }; for led in active[..half].iter_mut() { - *led = red; + *led = green; } for led in active[half..].iter_mut() { - *led = green; + *led = red; } } else { let color = RGB8 { r: intensity, g: intensity, b: intensity };