From 85dfa8c8b61f77650c87c1cf3098548fc56bb9b1 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 10 Feb 2026 19:22:02 -0900 Subject: [PATCH] claudes first try --- CLAUDE.md | 467 +++++++ boards/ark/fpv/default.px4board | 1 + .../px4/stm/stm32_common/dshot/CMakeLists.txt | 8 +- .../stm/stm32_common/dshot/dshot_bitbang.c | 1120 +++++++++++++++++ 4 files changed, 1595 insertions(+), 1 deletion(-) create mode 100644 CLAUDE.md create mode 100644 platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot_bitbang.c diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000000..29ba14d619 --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,467 @@ +# Bitbang DShot Prototype - Design Specification + +## Goal + +Implement bitbang DShot (including bidirectional) as an alternative to the existing DMA burst/capture approach in the STM32H7 platform layer. The implementation is generic - it works on any STM32H7 board by dynamically discovering port/pin groupings from `timer_io_channels[]` at init time. The **ARK FPV** board is the test target. + +Bitbang DShot uses a pacer timer to trigger DMA transfers to/from GPIO registers (BSRR for output, IDR for input), instead of using timer PWM channels with DMA burst to CCR registers. This decouples motor signal generation from timer channel hardware and enables simultaneous capture of all motors on a GPIO port (eliminating round-robin). + +## Key Advantage Over Current Approach + +The current BDShot implementation captures **one motor per timer per cycle** (round-robin) because each capture channel needs its own DMA stream. At 200Hz update rate, each motor gets RPM updates at only 50Hz (200/4). + +Bitbang capture reads the entire GPIO IDR register on every sample, capturing **all motors on a port simultaneously**. Every cycle produces RPM data for every motor - a 4x improvement in per-motor RPM update rate. + +## Architecture Overview + +### Current (DMA Burst/Capture) +``` +Timer (PWM mode) ──► CCR1-CCR4 (via DMA burst to DMAR) ──► Pin AF output +Timer (Capture mode) ──► CCR edge timestamps (via per-channel DMA) ──► one channel at a time +``` + +### Bitbang +``` +Timer (counter mode) ──► Update event triggers DMA ──► writes BSRR words to GPIO port +Timer (counter mode) ──► Update event triggers DMA ──► reads IDR from GPIO port (all pins) +``` + +## Port Group Discovery + +At init time, the implementation iterates `timer_io_channels[]` and groups channels by GPIO port. The port and pin are extracted from the existing `gpio_out` field using NuttX bit encoding: + +```c +#include // GPIO_PORT_MASK, GPIO_PORT_SHIFT, GPIO_PIN_MASK, GPIO_PIN_SHIFT + +// Extract port index and pin number from timer_io_channels[ch].gpio_out +uint8_t port_index = (gpio_out & GPIO_PORT_MASK) >> GPIO_PORT_SHIFT; // 0=A, 1=B, ... 7=H, 8=I +uint8_t pin_number = (gpio_out & GPIO_PIN_MASK) >> GPIO_PIN_SHIFT; // 0-15 + +// Get GPIO peripheral base address from NuttX lookup table +uint32_t gpio_base = g_gpiobase[port_index]; // e.g., STM32_GPIOH_BASE = 0x58021C00 +``` + +### Port Group Init Algorithm + +```c +// Pseudocode for up_dshot_init() +for (uint8_t ch = 0; ch < MAX_TIMER_IO_CHANNELS; ch++) { + if (!(channel_mask & (1 << ch))) continue; + + uint32_t gpio_out = timer_io_channels[ch].gpio_out; + uint8_t port_index = (gpio_out & GPIO_PORT_MASK) >> GPIO_PORT_SHIFT; + uint8_t pin_num = (gpio_out & GPIO_PIN_MASK) >> GPIO_PIN_SHIFT; + uint8_t timer_index = timer_io_channels[ch].timer_index; + + // Find or create port group for this GPIO port + port_group_t *pg = find_or_create_port_group(port_index); + pg->gpio_base = g_gpiobase[port_index]; + pg->pin_mask |= (1 << pin_num); + pg->moder_mask |= (3 << (pin_num * 2)); + pg->moder_output |= (1 << (pin_num * 2)); // 01 = output mode + pg->pin_numbers[pg->pin_count] = pin_num; + pg->output_channels[pg->pin_count] = ch; + pg->pin_count++; + + // Assign a pacer timer - use the first timer associated with any motor on this port. + // On H7 with DMAMUX the timer just needs a TIMx_UP DMA mapping; any timer works. + if (pg->timer_index == UINT8_MAX) { + pg->timer_index = timer_index; + } +} +``` + +This works on any board. Motors on the same GPIO port share a port group regardless of which timer they were originally assigned to. + +### ARK FPV Example (test target) + +On the ARK FPV, the above algorithm produces two port groups: + +| Motor | GPIO | Pin | Port Group | Pacer Timer | +|---------|-------|-----|------------|-------------| +| FMU_CH1 | PI0 | 0 | Port I | TIM5 (first timer seen on Port I) | +| FMU_CH2 | PH12 | 12 | Port H | TIM5 | +| FMU_CH3 | PH11 | 11 | Port H | TIM5 | +| FMU_CH4 | PH10 | 10 | Port H | TIM5 | +| FMU_CH5 | PI5 | 5 | Port I | (already assigned) | +| FMU_CH6 | PI6 | 6 | Port I | (already assigned) | +| FMU_CH7 | PI7 | 7 | Port I | (already assigned) | +| FMU_CH8 | PI2 | 2 | Port I | (already assigned) | + +- **Port H group**: 3 motors, pin_mask=0x1C00, pacer=TIM5 +- **Port I group**: 5 motors, pin_mask=0xE5, pacer=TIM5 (first timer found; could also be TIM8) + +Note: The pacer timer just needs a valid `dma_map_up` entry. The first timer encountered for a port group is used. Since DMAMUX on H7 can route any timer update event to any DMA stream, any timer with DMA configured works as a pacer. + +## GPIO Registers (STM32H7) + +``` +Offset 0x00: MODER - Mode register (2 bits/pin: 00=input, 01=output, 10=AF, 11=analog) +Offset 0x10: IDR - Input data register (read-only, bits [15:0] = pin state) +Offset 0x14: ODR - Output data register +Offset 0x18: BSRR - Bit set/reset register (write-only, atomic) + Bits [15:0] = SET (writing 1 sets pin HIGH) + Bits [31:16] = RESET (writing 1 sets pin LOW) + Writing 0 = no change +``` + +GPIO base addresses are looked up at runtime via `g_gpiobase[port_index]` (NuttX global array). + +### MODER Manipulation + +Pin masks and MODER masks are computed dynamically during port group init from the discovered pin numbers: + +```c +// For each pin in the port group: +pg->pin_mask |= (1 << pin_num); // for BSRR set/reset +pg->moder_mask |= (3 << (pin_num * 2)); // which MODER bits to touch +pg->moder_output |= (1 << (pin_num * 2)); // 01 = general purpose output +// moder_input is always 0 (clear the bits = input mode) +``` + +To switch to output: `modifyreg32(gpio_base + STM32_GPIO_MODER_OFFSET, pg->moder_mask, pg->moder_output)` +To switch to input: `modifyreg32(gpio_base + STM32_GPIO_MODER_OFFSET, pg->moder_mask, 0)` + +## DMA Configuration + +The DMA request source (DMAMUX) stays the same as current - TIMx_UP triggers each DMA transfer. The only change is the **peripheral address** and **transfer direction**. + +### Output DMA (Memory → GPIO BSRR) +```c +// Peripheral address is GPIO BSRR instead of TIM DMAR +px4_stm32_dmasetup(pg->dma_handle, + pg->gpio_base + STM32_GPIO_BSRR_OFFSET, // e.g., 0x58021C18 for Port H + (uint32_t)bsrr_output_buffer[pg_index], + SUBPERIODS_PER_BIT * DSHOT_FRAME_BITS, // 20 * 16 = 320 transfers (+ 1 trailing zero) + DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | + DMA_SCR_MINC | DMA_SCR_DIR_M2P | DMA_SCR_TCIE); +``` + +### Input DMA (GPIO IDR → Memory) +```c +px4_stm32_dmasetup(pg->dma_handle, + pg->gpio_base + STM32_GPIO_IDR_OFFSET, // e.g., 0x58021C10 for Port H + (uint32_t)idr_capture_buffer[pg_index], + CAPTURE_SAMPLE_COUNT, // enough samples for GCR frame (~512) + DMA_SCR_PRIHI | DMA_SCR_MSIZE_16BITS | DMA_SCR_PSIZE_16BITS | + DMA_SCR_MINC | DMA_SCR_DIR_P2M | DMA_SCR_TCIE); +``` + +### DMAMUX Routing (unchanged) + +Each port group uses its pacer timer's `dma_map_up` for DMAMUX routing: +```c +pg->dma_handle = stm32_dmachannel(io_timers[pg->timer_index].dshot.dma_map_up); +``` + +The DMAMUX determines **when** the DMA fires (on timer update events). The DMA stream registers determine **what** gets transferred (BSRR/IDR addresses). These are independent - the DMA request source and transfer target don't need to be the same peripheral. + +## Timer Configuration + +Each port group's pacer timer is configured as a simple upcounter. No PWM output, no capture-compare - just generate periodic update events. + +```c +// Same prescaler as current DShot implementation +rPSC(timer) = ((timer_clock_freq / dshot_pwm_freq) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1; +rARR(timer) = DSHOT_MOTOR_PWM_BIT_WIDTH - 1; // 19 (20 counts per sub-period) +// Enable update DMA request +rDIER(timer) |= ATIM_DIER_UDE; +// Enable counter +rCR1(timer) |= GTIM_CR1_CEN; +``` + +With `DSHOT_MOTOR_PWM_BIT_WIDTH = 20`, each timer update fires DMA once per sub-period. One DShot bit = 20 sub-periods. + +## Output Buffer Layout (BSRR Words) + +Each DShot frame is 16 bits. Each bit has 20 sub-periods. The buffer contains one 32-bit BSRR word per sub-period. + +**DShot bit waveform (sub-periods 0-19):** +- Bit '1' (70% duty): SET at sub-period 0, RESET at sub-period 14, zeros elsewhere +- Bit '0' (35% duty): SET at sub-period 0, RESET at sub-period 7, zeros elsewhere + +**Buffer structure per port:** +``` +[bit0_subperiod0] [bit0_subperiod1] ... [bit0_subperiod19] +[bit1_subperiod0] [bit1_subperiod1] ... [bit1_subperiod19] +... +[bit15_subperiod0] [bit15_subperiod1] ... [bit15_subperiod19] +[trailing_zero] ← ensures all pins return LOW at frame end +``` + +Total: (16 * 20) + 1 = 321 BSRR words = 1284 bytes per port. + +**Encoding example** (3 motors on pins 10, 11, 12 of some port, sending bits 1, 0, 1): +``` +Sub-period 0: BSRR = (1<<10) | (1<<11) | (1<<12) = 0x00001C00 (SET all three) +Sub-period 7: BSRR = (1<<(11+16)) = 0x00080000 (RESET pin11 only - it's '0') +Sub-period 14: BSRR = (1<<(10+16)) | (1<<(12+16)) = 0x14000000 (RESET pin10, pin12 - they're '1') +All other sub-periods: BSRR = 0x00000000 (no change) +``` + +### Motor Data Set Function + +The port group index and pin number for each output channel are determined at init time and stored in a lookup table. No per-frame discovery needed. + +```c +// Populated during up_dshot_init() from timer_io_channels[ch].gpio_out +static uint8_t _channel_to_port_group[MAX_TIMER_IO_CHANNELS]; +static uint8_t _channel_to_pin[MAX_TIMER_IO_CHANNELS]; + +void dshot_motor_data_set(uint8_t output_channel, uint16_t packet) +{ + uint8_t pg_index = _channel_to_port_group[output_channel]; + uint8_t pin_num = _channel_to_pin[output_channel]; + uint32_t set_mask = (1 << pin_num); + uint32_t reset_mask = (1 << (pin_num + 16)); + + uint32_t *buffer = bsrr_output_buffer[pg_index]; + + for (int bit = 0; bit < 16; bit++) { + int base = bit * SUBPERIODS_PER_BIT; + bool is_one = (packet >> (15 - bit)) & 1; // MSB first + int reset_point = is_one ? MOTOR_PWM_BIT_1 : MOTOR_PWM_BIT_0; + + buffer[base + 0] |= set_mask; // SET at start of bit + buffer[base + reset_point] |= reset_mask; // RESET at duty cycle end + } +} +``` + +Note: Buffer must be zeroed before building each frame, then all motors OR their masks into the shared per-port buffer. + +## Capture Buffer Layout (IDR Samples) + +After transmitting, GPIO pins switch to input mode. DMA samples the IDR register at the same timer rate (20 sub-periods per DShot bit period). + +**GCR response timing:** +- GCR bit rate = DShot bit rate * 5/4 (750kHz for DShot600) +- GCR bit period = 1.333us +- Sample period = 83.3ns (at DShot600 with 20 sub-periods) +- Samples per GCR bit = ~16 (heavy oversampling, good for edge detection) +- GCR frame = 21 bits = ~336 samples minimum +- Use 400-512 samples to be safe + +**Buffer:** Array of uint16_t, one per IDR sample. +``` +[idr_sample_0] [idr_sample_1] ... [idr_sample_N] +``` + +Each sample contains all 16 pins of the port. Extract per-motor bitstream by masking: +```c +bool pin_state = (idr_sample >> pin_num) & 1; +``` + +### Capture Processing + +Convert IDR samples to edge intervals, then reuse existing GCR decoder: + +```c +void process_bitbang_capture(uint8_t port_group, uint8_t pin_num, uint8_t output_channel) +{ + uint16_t *buffer = idr_capture_buffer[port_group]; + uint32_t intervals[32]; + unsigned interval_count = 0; + + // Extract edges from IDR samples + bool prev_state = (buffer[0] >> pin_num) & 1; + unsigned last_edge = 0; + + for (unsigned i = 1; i < CAPTURE_SAMPLE_COUNT; i++) { + bool state = (buffer[i] >> pin_num) & 1; + if (state != prev_state) { + // Edge detected + if (last_edge > 0 && interval_count < 32) { + intervals[interval_count++] = i - last_edge; + } + last_edge = i; + prev_state = state; + } + } + + // Now decode using same GCR logic as existing code + // intervals[] contains sample counts between edges + // (equivalent to CCR timestamp differences in current impl) +} +``` + +The existing `convert_edge_intervals_to_bitstream()` logic (adaptive base interval, bit counting, RLL/GCR decode, CRC check) can be reused with minor adaptation since the interval units are now sample counts instead of timer ticks. The ratio math is identical. + +## Sequence of Operations + +### Per-cycle flow (called from `up_dshot_trigger()`): + +``` +1. TRANSMIT PHASE + a. Zero the BSRR output buffers for all port groups + b. Build BSRR buffers (call dshot_motor_data_set for each motor) + - Each motor ORs its pin masks into its port group's shared buffer + c. Configure GPIO pins as output (MODER = 01) for each port group + d. Flush dcache on output buffers + e. For each port group: + - Allocate DMA via stm32_dmachannel(io_timers[pg->timer_index].dshot.dma_map_up) + - Setup DMA: memory→BSRR, 32-bit, 321 transfers + - Enable timer UDE (update DMA request) + - Start DMA with burst_finished_callback + - Enable timer counter + f. ~27us later (16 bits × 20 sub-periods × 83.3ns), DMA complete fires + +2. TRANSITION PHASE (in burst_finished_callback, per port group) + a. Stop DMA, free DMA handle + b. Disable timer + c. Switch GPIO pins to input mode (MODER = 00) + d. Schedule capture start via hrt_call_after(30us) + +3. CAPTURE PHASE (in capture_start_callback, per port group) + a. Zero capture buffer for this port group + b. Flush dcache + c. Allocate DMA via stm32_dmachannel(dma_map_up) [reuse same DMAMUX route] + d. Setup DMA: IDR→memory, 16-bit, 512 transfers + e. Enable timer UDE, start DMA, enable timer + f. Schedule capture_complete via hrt_call_after(frame_time + margin) + +4. PROCESSING PHASE (in capture_complete_callback, per port group) + a. Stop DMA, free DMA handle + b. Disable timer + c. Invalidate dcache on capture buffer + d. For EACH motor on the port (not round-robin!): + - Extract per-pin bitstream from IDR samples using pin_numbers[] + - Detect edges, compute intervals + - Decode GCR → eRPM/EDT + e. Switch GPIO back to output mode (MODER = 01) + f. Mark port group cycle complete + +``` + +## File Structure + +### New file: `platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot_bitbang.c` + +Implements the same `up_dshot_*` API as `dshot.c`: +- `up_dshot_init()` - Initialize port groups, allocate DMA, configure timers +- `up_dshot_trigger()` - Start the TX→wait→RX→process cycle +- `dshot_motor_data_set()` - Build BSRR words into port group buffers +- `up_dshot_arm()` - Enable/disable GPIO output +- `up_bdshot_get_erpm()` - Return captured eRPM (unchanged) +- `up_bdshot_channel_online()` - Return online status (unchanged) +- `up_bdshot_status()` - Print diagnostics + +### New/modified: `platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt` + +Add build option to select `dshot.c` vs `dshot_bitbang.c`. + +### Board-level config + +No board-specific changes needed. The bitbang implementation reads the same `io_timers[]` and `timer_io_channels[]` structures that every board already defines. It extracts GPIO port/pin from `timer_io_channels[].gpio_out` (which `initIOTimerChannel()` already populates with `getGPIOPort(pin.port) | getGPIOPin(pin.pin)`) and the DMA mapping from `io_timers[].dshot.dma_map_up`. + +A board opts into bitbang mode via a build flag or Kconfig option that selects `dshot_bitbang.c` instead of `dshot.c`. + +### What stays the same +- `drv_dshot.h` API - no changes +- `DShot.cpp` / `DShot.h` (the driver module) - no changes +- `DShotTelemetry.cpp` / `DShotTelemetry.h` - no changes +- GCR decoding logic - copied/adapted from dshot.c but the core algorithm is identical +- All higher-level consumers of eRPM data + +## Data Structures + +```c +#define MAX_PORT_GROUPS 4 // Max distinct GPIO ports with motor pins (typically 1-3) +#define MAX_PINS_PER_GROUP 8 + +// Port group: one per GPIO port that has motor pins. Built dynamically at init. +typedef struct port_group_t { + uint32_t gpio_base; // GPIO port base address (from g_gpiobase[]) + uint8_t port_index; // NuttX port index (0=A, 7=H, 8=I, etc.) + uint32_t pin_mask; // Bitmask of motor pins on this port + uint32_t moder_mask; // MODER register mask for mode switching + uint32_t moder_output; // MODER value for output mode (01 per pin) + uint8_t pin_count; // Number of motor pins on this port + uint8_t pin_numbers[MAX_PINS_PER_GROUP]; // Pin numbers (0-15) + uint8_t output_channels[MAX_PINS_PER_GROUP]; // Corresponding output channel indices + uint8_t timer_index; // Pacer timer index (into io_timers[]) + DMA_HANDLE dma_handle; // DMA stream handle (reused between TX/RX) + bool bidirectional; // BDShot enabled for this group + bool cycle_complete; // Ready for next trigger +} port_group_t; + +static port_group_t _port_groups[MAX_PORT_GROUPS] = {}; +static uint8_t _num_port_groups = 0; + +// Per-channel lookup tables (populated at init from timer_io_channels[].gpio_out) +static uint8_t _channel_to_port_group[MAX_TIMER_IO_CHANNELS]; +static uint8_t _channel_to_pin[MAX_TIMER_IO_CHANNELS]; +``` + +## DMA Resource Usage Comparison + +### Current (DMA burst + capture) +- **Output**: 1 DMA stream per timer = N_timers streams +- **Capture**: 1 DMA stream per captured channel (round-robin, 1 at a time per timer) +- DMA streams are freed/reallocated between burst and capture phases + +### Bitbang +- **Output**: 1 DMA stream per port group = N_ports streams +- **Capture**: 1 DMA stream per port group (same streams, reconfigured) +- DMA streams freed/reallocated between phases (same pattern as current) + +DMA resource usage is equivalent (typically N_timers == N_ports or close). The DMAMUX routing uses the same timer update event mappings. + +## Memory Usage + +### Current +- Output: ~272 bytes per timer (4 channels * 17 words * 4 bytes, cache-aligned) +- Capture: ~256 bytes per timer (4 channels * 32 samples * 2 bytes) + +### Bitbang +- Output: ~1284 bytes per port group (321 words * 4 bytes, cache-aligned) +- Capture: ~1024 bytes per port group (512 samples * 2 bytes, cache-aligned) +- Scales with number of port groups, not number of motors +- Higher per-group cost, but acceptable for H7 (1MB+ SRAM) + +## Open Questions / Risks + +1. **GPIO output speed**: BSRR writes through AHB4 bus on H7. At 83.3ns per DMA transfer (12MHz), is there enough bus bandwidth? Should be fine - AHB4 runs at 240MHz, and BSRR writes are single-cycle. + +2. **DMA priority vs other peripherals**: SPI1 (IMU) is on DMA1. Timer DMA is already high priority. No change expected. + +3. **GPIO input threshold**: When reading IDR for bidir, the input threshold depends on GPIO input configuration (Schmitt trigger, pull-up/down). The current capture approach uses timer input capture which has different input characteristics. May need to configure GPIO pull-up to match idle-high behavior. + +4. **Signal integrity**: Timer PWM output has precise edge timing locked to the timer clock. BSRR writes via DMA may have slight jitter due to bus arbitration. This is likely negligible (< 10ns on H7) but should be verified. + +5. **OTYPER configuration**: For bidirectional DShot, the pin needs to be open-drain (or at least not actively driven when in input mode). The MODER switch to input (00) handles this, but verify that switching MODER during/after a frame doesn't cause glitches. + +6. **Cache coherency**: BSRR output buffers need dcache clean before DMA. IDR capture buffers need dcache invalidate after DMA. Same pattern as current code. + +## Implementation Order + +1. **Port group initialization**: Parse `timer_io_channels[]` to build `port_group_t` structures grouped by GPIO port instead of timer +2. **Output-only DShot**: Implement BSRR buffer building and DMA output. Verify with logic analyzer that DShot frames are correct +3. **Bidirectional capture**: Implement IDR sampling and per-pin edge extraction. Verify GCR decode produces valid eRPM +4. **Integration**: Wire up to the existing `up_dshot_*` API so `DShot.cpp` works unchanged +5. **Testing**: Compare eRPM data quality (CRC error rate, update rate) vs current implementation + +## Reference: Betaflight Bitbang Implementation + +- [Betaflight dshot_bitbang.c](https://github.com/betaflight/betaflight/blob/master/src/platform/STM32/dshot_bitbang.c) +- [Betaflight dshot_bitbang_impl.h](https://github.com/betaflight/betaflight/blob/master/src/main/drivers/dshot_bitbang_impl.h) +- [Original GPIO banging PR #7446](https://github.com/betaflight/betaflight/pull/7446) +- [Bidirectional bitbang PR #8779](https://github.com/betaflight/betaflight/pull/8779) + +## Reference: Current PX4 Implementation Files + +- DShot platform layer: `platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c` +- IO timer layer: `platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c` +- IO timer struct defs: `platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h` +- GPIO/pin helpers: `platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/hw_description.h` +- H7 timer/DMA descriptions: `platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h` +- H7 GPIO register defs: NuttX `arch/arm/src/stm32h7/stm32_gpio.h` (GPIO_PORT_MASK, GPIO_PIN_MASK, g_gpiobase[]) +- H7 GPIO HW registers: NuttX `arch/arm/src/stm32h7/hardware/stm32h7x3xx_gpio.h` (STM32_GPIO_MODER_OFFSET, etc.) +- DShot API header: `src/drivers/drv_dshot.h` +- DShot driver module: `src/drivers/dshot/DShot.cpp` + +## Reference: Test Target (ARK FPV) + +- Timer config: `boards/ark/fpv/src/timer_config.cpp` +- DMA map: `boards/ark/fpv/nuttx-config/include/board_dma_map.h` +- Board config: `boards/ark/fpv/src/board_config.h` diff --git a/boards/ark/fpv/default.px4board b/boards/ark/fpv/default.px4board index 75f88802ac..c98dc784ac 100644 --- a/boards/ark/fpv/default.px4board +++ b/boards/ark/fpv/default.px4board @@ -14,6 +14,7 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_DSHOT_BITBANG=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt index 5221ae896a..9574923f65 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt @@ -31,7 +31,13 @@ # ############################################################################ +if(CONFIG_DRIVERS_DSHOT_BITBANG) + set(DSHOT_SRC dshot_bitbang.c) +else() + set(DSHOT_SRC dshot.c) +endif() + px4_add_library(arch_dshot - dshot.c + ${DSHOT_SRC} ) target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot_bitbang.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot_bitbang.c new file mode 100644 index 0000000000..19bcce5270 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot_bitbang.c @@ -0,0 +1,1120 @@ +/**************************************************************************** + * + * Copyright (C) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Bitbang DShot implementation. + * + * Uses a pacer timer to trigger DMA to GPIO BSRR/IDR registers instead of + * timer DMA burst to CCR registers. This captures all motors on a GPIO port + * simultaneously for bidirectional DShot, giving every motor RPM data every + * cycle instead of round-robin. + */ + +#if (CONFIG_STM32_HAVE_IP_DMA_V1) +// Do nothing. IP DMA V1 MCUs are not supported. +#else + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// DShot protocol definitions +#define ONE_MOTOR_DATA_SIZE 16u +#define MOTOR_PWM_BIT_1 14u +#define MOTOR_PWM_BIT_0 7u +#define DSHOT_THROTTLE_POSITION 5u +#define DSHOT_TELEMETRY_POSITION 4u +#define NIBBLES_SIZE 4u +#define DSHOT_NUMBER_OF_NIBBLES 3u +#define SUBPERIODS_PER_BIT DSHOT_MOTOR_PWM_BIT_WIDTH // 20 + +// Bitbang-specific +#define MAX_PORT_GROUPS 4u +#define MAX_PINS_PER_GROUP 8u +#define BSRR_BUFFER_SIZE (ONE_MOTOR_DATA_SIZE * SUBPERIODS_PER_BIT + 1) // 321 words +#define CAPTURE_SAMPLE_COUNT 512u + +// DMA stream configuration +#define DSHOT_DMA_SCR_TX (DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | \ + DMA_SCR_MINC | DMA_SCR_DIR_M2P | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) + +#define DSHOT_DMA_SCR_RX (DMA_SCR_PRIHI | DMA_SCR_MSIZE_16BITS | DMA_SCR_PSIZE_16BITS | \ + DMA_SCR_MINC | DMA_SCR_DIR_P2M | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) + +#if defined(CONFIG_ARMV7M_DCACHE) +# define DMA_BUFFER_MASK (ARMV7M_DCACHE_LINESIZE - 1) +# define DMA_ALIGN_UP(n) (((n) + DMA_BUFFER_MASK) & ~DMA_BUFFER_MASK) +#else +# define DMA_ALIGN_UP(n) (n) +#endif + +// Timer register macros (from io_timer.c, local to this file) +#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg) +#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) +#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET) +#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET) +#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET) +#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET) +#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET) +#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET) +#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET) + +// Port group: one per GPIO port that has motor pins +typedef struct port_group_t { + uint32_t gpio_base; // GPIO port base address + uint8_t port_index; // NuttX port index (0=A, 7=H, 8=I) + uint32_t pin_mask; // Bitmask of motor pins on this port + uint32_t moder_mask; // MODER register mask + uint32_t moder_output; // MODER value for output mode (01 per pin) + uint8_t pin_count; // Number of motor pins on this port + uint8_t pin_numbers[MAX_PINS_PER_GROUP]; // Pin numbers (0-15) + uint8_t output_channels[MAX_PINS_PER_GROUP]; // Corresponding output channel indices + uint8_t timer_index; // Pacer timer index (into io_timers[]) + DMA_HANDLE dma_handle; + bool bidirectional; + bool cycle_complete; + uint8_t pg_index; // Self-index for callback arg +} port_group_t; + +static port_group_t _port_groups[MAX_PORT_GROUPS] = {}; +static uint8_t _num_port_groups = 0; + +// Per-channel lookup tables +static uint8_t _channel_to_port_group[MAX_TIMER_IO_CHANNELS]; +static uint8_t _channel_to_pin[MAX_TIMER_IO_CHANNELS]; +static bool _channel_initialized[MAX_TIMER_IO_CHANNELS] = {}; + +// Stored DShot packets (set by dshot_motor_data_set, consumed by up_dshot_trigger) +static uint16_t _channel_packet[MAX_TIMER_IO_CHANNELS] = {}; + +// DMA buffers (cache-aligned) +static uint32_t _bsrr_buffer[MAX_PORT_GROUPS][BSRR_BUFFER_SIZE] + px4_cache_aligned_data() = {}; +static uint16_t _idr_buffer[MAX_PORT_GROUPS][CAPTURE_SAMPLE_COUNT] + px4_cache_aligned_data() = {}; + +// GCR decode table (same as dshot.c) +static const uint32_t gcr_decode[32] = { + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x9, 0xA, 0xB, 0x0, 0xD, 0xE, 0xF, + 0x0, 0x0, 0x2, 0x3, 0x0, 0x5, 0x6, 0x7, + 0x0, 0x0, 0x8, 0x1, 0x0, 0x4, 0xC, 0x0 +}; + +// Global state +static uint32_t _bdshot_channel_mask = 0xFFFFFFFF; +static uint32_t _dshot_frequency = 0; +static bool _edt_enabled = false; +static bool _armed = false; + +// ESC boot delay +static const uint64_t ESC_BOOT_DELAY_US = 3000000; + +// Online flags +#define BDSHOT_OFFLINE_COUNT 200 +static bool _bdshot_online[MAX_TIMER_IO_CHANNELS] = {}; +static bool _bdshot_capture_supported[MAX_TIMER_IO_CHANNELS] = {}; +static int _consecutive_failures[MAX_TIMER_IO_CHANNELS] = {}; +static int _consecutive_successes[MAX_TIMER_IO_CHANNELS] = {}; + +typedef struct { + int32_t erpm; + bool ready; + float rate_hz; + uint64_t last_timestamp; +} erpm_data_t; + +typedef struct { + uint8_t value; + bool ready; + float rate_hz; + uint64_t last_timestamp; +} edt_data_t; + +// Adaptive base interval per channel (samples per GCR bit) +// At 20 sub-periods per DShot bit and GCR rate = DShot * 5/4, we get 16 samples per GCR bit +static float _base_interval[MAX_TIMER_IO_CHANNELS] = { [0 ...(MAX_TIMER_IO_CHANNELS - 1)] = 16.0f }; + +static erpm_data_t _erpms[MAX_TIMER_IO_CHANNELS] = {}; +static edt_data_t _edt_temp[MAX_TIMER_IO_CHANNELS] = {}; +static edt_data_t _edt_volt[MAX_TIMER_IO_CHANNELS] = {}; +static edt_data_t _edt_curr[MAX_TIMER_IO_CHANNELS] = {}; + +// hrt callback handles for capture scheduling, one per port group +static struct hrt_call _capture_calls[MAX_PORT_GROUPS]; + +// Decoding status for each channel +static uint32_t read_ok[MAX_TIMER_IO_CHANNELS] = {}; +static uint32_t read_fail_crc[MAX_TIMER_IO_CHANNELS] = {}; + +// Forward declarations +static void tx_dma_complete_callback(DMA_HANDLE handle, uint8_t status, void *arg); +static void capture_start_callback(void *arg); +static void capture_complete_callback(void *arg); +static void process_capture_results(uint8_t pg_index); +static uint32_t convert_idr_to_bitstream(uint8_t pg_index, uint8_t pin_num, uint8_t output_channel); +static void decode_dshot_telemetry(uint32_t payload, struct BDShotTelemetry *packet); +static float calculate_rate_hz(uint64_t last_timestamp, float last_rate_hz, uint64_t timestamp); + +static void gpio_set_mode_output(port_group_t *pg) +{ + modifyreg32(pg->gpio_base + STM32_GPIO_MODER_OFFSET, pg->moder_mask, pg->moder_output); +} + +static void gpio_set_mode_input(port_group_t *pg) +{ + modifyreg32(pg->gpio_base + STM32_GPIO_MODER_OFFSET, pg->moder_mask, 0); +} + +static void gpio_set_idle_state(port_group_t *pg) +{ + if (pg->bidirectional) { + // BDShot idles HIGH - set all motor pins + putreg32(pg->pin_mask, pg->gpio_base + STM32_GPIO_BSRR_OFFSET); + + } else { + // Normal DShot idles LOW - reset all motor pins + putreg32(pg->pin_mask << 16, pg->gpio_base + STM32_GPIO_BSRR_OFFSET); + } +} + +static void timer_configure_pacer(uint8_t timer_index) +{ + // Enable timer clock + modifyreg32(io_timers[timer_index].clock_register, 0, io_timers[timer_index].clock_bit); + + // Disable timer before configuring + rCR1(timer_index) = 0; + + // Configure prescaler: timer clock / (dshot_freq * SUBPERIODS_PER_BIT) + uint32_t timer_freq = io_timers[timer_index].clock_freq; + uint32_t target_freq = _dshot_frequency * SUBPERIODS_PER_BIT; + rPSC(timer_index) = (timer_freq / target_freq) - 1; + + // Auto-reload: one DMA transfer per timer update + rARR(timer_index) = SUBPERIODS_PER_BIT - 1; + + test + + // Generate update event to load prescaler + rEGR(timer_index) = GTIM_EGR_UG; + + // Clear any pending update interrupt/DMA flags + rSR(timer_index) = 0; +} + +static void timer_enable(uint8_t timer_index) +{ + rCR1(timer_index) |= GTIM_CR1_CEN; +} + +static void timer_disable(uint8_t timer_index) +{ + rCR1(timer_index) &= ~GTIM_CR1_CEN; + rCNT(timer_index) = 0; +} + +static void timer_set_ude(uint8_t timer_index, bool enable) +{ + if (enable) { + rDIER(timer_index) |= ATIM_DIER_UDE; + + } else { + rDIER(timer_index) &= ~ATIM_DIER_UDE; + } +} + +static port_group_t *find_port_group(uint8_t port_index) +{ + for (uint8_t i = 0; i < _num_port_groups; i++) { + if (_port_groups[i].port_index == port_index) { + return &_port_groups[i]; + } + } + + return NULL; +} + +static port_group_t *find_or_create_port_group(uint8_t port_index) +{ + port_group_t *pg = find_port_group(port_index); + + if (pg) { + return pg; + } + + if (_num_port_groups >= MAX_PORT_GROUPS) { + return NULL; + } + + pg = &_port_groups[_num_port_groups]; + memset(pg, 0, sizeof(*pg)); + pg->port_index = port_index; + pg->gpio_base = g_gpiobase[port_index]; + pg->timer_index = UINT8_MAX; + pg->cycle_complete = true; + pg->pg_index = _num_port_groups; + _num_port_groups++; + + return pg; +} + +int up_dshot_init(uint32_t channel_mask, uint32_t bdshot_channel_mask, unsigned dshot_pwm_freq, bool edt_enable) +{ + _dshot_frequency = dshot_pwm_freq; + _bdshot_channel_mask = bdshot_channel_mask; + _edt_enabled = edt_enable; + _num_port_groups = 0; + + if (bdshot_channel_mask) { + PX4_INFO("BDShot bitbang enabled"); + } + + int32_t channels_init_mask = 0; + + // Phase 1: Discover port groups from timer_io_channels[] + // Collect available timer indices per port group for assignment + uint8_t pg_available_timers[MAX_PORT_GROUPS][MAX_IO_TIMERS]; + uint8_t pg_available_timer_count[MAX_PORT_GROUPS] = {}; + + for (uint8_t ch = 0; ch < MAX_TIMER_IO_CHANNELS; ch++) { + if (!(channel_mask & (1 << ch))) { + continue; + } + + uint32_t gpio_out = timer_io_channels[ch].gpio_out; + + if (gpio_out == 0) { + continue; + } + + uint8_t timer_index = timer_io_channels[ch].timer_index; + + if (timer_index >= MAX_IO_TIMERS || io_timers[timer_index].dshot.dma_map_up == 0) { + // No DMA UP mapping for this timer - skip + continue; + } + + uint8_t port_index = (gpio_out & GPIO_PORT_MASK) >> GPIO_PORT_SHIFT; + uint8_t pin_num = (gpio_out & GPIO_PIN_MASK) >> GPIO_PIN_SHIFT; + + port_group_t *pg = find_or_create_port_group(port_index); + + if (!pg) { + PX4_WARN("Too many port groups, skipping channel %u", ch); + continue; + } + + if (pg->pin_count >= MAX_PINS_PER_GROUP) { + PX4_WARN("Too many pins on port group %u, skipping channel %u", pg->pg_index, ch); + continue; + } + + // Add pin to port group + pg->pin_mask |= (1u << pin_num); + pg->moder_mask |= (3u << (pin_num * 2)); + pg->moder_output |= (1u << (pin_num * 2)); // 01 = output mode + pg->pin_numbers[pg->pin_count] = pin_num; + pg->output_channels[pg->pin_count] = ch; + pg->pin_count++; + + // Track channel-to-port-group mapping + _channel_to_port_group[ch] = pg->pg_index; + _channel_to_pin[ch] = pin_num; + + // Mark BDShot + if (bdshot_channel_mask & (1 << ch)) { + pg->bidirectional = true; + } + + // Track available timers for this port group (deduplicated) + bool timer_already_listed = false; + + for (uint8_t t = 0; t < pg_available_timer_count[pg->pg_index]; t++) { + if (pg_available_timers[pg->pg_index][t] == timer_index) { + timer_already_listed = true; + break; + } + } + + if (!timer_already_listed && pg_available_timer_count[pg->pg_index] < MAX_IO_TIMERS) { + pg_available_timers[pg->pg_index][pg_available_timer_count[pg->pg_index]] = timer_index; + pg_available_timer_count[pg->pg_index]++; + } + } + + // Phase 2: Assign pacer timers (greedy, fewest-alternatives-first) + // Sort port groups by number of available timers (ascending) - simple selection sort + uint8_t pg_order[MAX_PORT_GROUPS]; + + for (uint8_t i = 0; i < _num_port_groups; i++) { + pg_order[i] = i; + } + + for (uint8_t i = 0; i < _num_port_groups; i++) { + for (uint8_t j = i + 1; j < _num_port_groups; j++) { + if (pg_available_timer_count[pg_order[j]] < pg_available_timer_count[pg_order[i]]) { + uint8_t tmp = pg_order[i]; + pg_order[i] = pg_order[j]; + pg_order[j] = tmp; + } + } + } + + bool timer_assigned[MAX_IO_TIMERS] = {}; + + for (uint8_t i = 0; i < _num_port_groups; i++) { + uint8_t pg_idx = pg_order[i]; + port_group_t *pg = &_port_groups[pg_idx]; + + for (uint8_t t = 0; t < pg_available_timer_count[pg_idx]; t++) { + uint8_t ti = pg_available_timers[pg_idx][t]; + + if (!timer_assigned[ti]) { + pg->timer_index = ti; + timer_assigned[ti] = true; + break; + } + } + + if (pg->timer_index == UINT8_MAX) { + PX4_ERR("No timer available for port group %u (port %c)", pg_idx, 'A' + pg->port_index); + continue; + } + } + + // Phase 3: Initialize GPIO pins and pacer timers + for (uint8_t pg_idx = 0; pg_idx < _num_port_groups; pg_idx++) { + port_group_t *pg = &_port_groups[pg_idx]; + + if (pg->timer_index == UINT8_MAX) { + continue; + } + + io_timer_channel_mode_t mode = pg->bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot; + + // Initialize each channel through io_timer to claim resources + for (uint8_t p = 0; p < pg->pin_count; p++) { + uint8_t ch = pg->output_channels[p]; + int ret = io_timer_channel_init(ch, mode, NULL, NULL); + + if (ret != OK) { + PX4_WARN("io_timer_channel_init %u failed (%d)", ch, ret); + continue; + } + + _channel_initialized[ch] = true; + channels_init_mask |= (1 << ch); + + // Mark BDShot capture supported (bitbang captures all simultaneously) + if (bdshot_channel_mask & (1 << ch)) { + _bdshot_capture_supported[ch] = true; + } + } + + // Override GPIO MODER from AF (10) to GP output (01) + gpio_set_mode_output(pg); + gpio_set_idle_state(pg); + + // Configure pacer timer + timer_configure_pacer(pg->timer_index); + + PX4_INFO("Port group %u: GPIO port %c, %u pins, pin_mask=0x%04lx, pacer=TIM%u (idx %u)", + pg_idx, 'A' + pg->port_index, pg->pin_count, + (unsigned long)pg->pin_mask, pg->timer_index + 1, pg->timer_index); + } + + return channels_init_mask; +} + +void dshot_motor_data_set(uint8_t channel, uint16_t data, bool telemetry) +{ + if (channel >= MAX_TIMER_IO_CHANNELS || !_channel_initialized[channel]) { + return; + } + + // Build the 16-bit packet and store for later BSRR buffer construction in trigger() + uint16_t packet = 0; + uint16_t checksum = 0; + + packet |= data << DSHOT_THROTTLE_POSITION; + packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION; + + uint16_t csum_data = packet; + csum_data >>= NIBBLES_SIZE; + + for (uint8_t i = 0; i < DSHOT_NUMBER_OF_NIBBLES; i++) { + checksum ^= (csum_data & 0x0F); + csum_data >>= NIBBLES_SIZE; + } + + if (_bdshot_channel_mask & (1 << channel)) { + packet |= ((~checksum) & 0x0F); + + } else { + packet |= ((checksum) & 0x0F); + } + + _channel_packet[channel] = packet; +} + +// Build BSRR words for a single motor into the port group's shared buffer +static void build_bsrr_for_channel(uint8_t channel) +{ + uint8_t pg_index = _channel_to_port_group[channel]; + uint8_t pin_num = _channel_to_pin[channel]; + port_group_t *pg = &_port_groups[pg_index]; + uint32_t *buffer = _bsrr_buffer[pg_index]; + uint16_t packet = _channel_packet[channel]; + + uint32_t set_mask, reset_mask; + + if (pg->bidirectional) { + // BDShot (inverted): idle HIGH, pulse LOW + set_mask = (1u << (pin_num + 16)); // BSRR reset (drive LOW) + reset_mask = (1u << pin_num); // BSRR set (drive HIGH) + + } else { + // Normal DShot: idle LOW, pulse HIGH + set_mask = (1u << pin_num); // BSRR set (drive HIGH) + reset_mask = (1u << (pin_num + 16)); // BSRR reset (drive LOW) + } + + for (uint8_t bit = 0; bit < ONE_MOTOR_DATA_SIZE; bit++) { + unsigned base = bit * SUBPERIODS_PER_BIT; + bool is_one = (packet >> (15 - bit)) & 1; // MSB first + unsigned duty_point = is_one ? MOTOR_PWM_BIT_1 : MOTOR_PWM_BIT_0; + + buffer[base] |= set_mask; // Start of bit: begin pulse + buffer[base + duty_point] |= reset_mask; // End of duty cycle: end pulse + } +} + +void up_dshot_trigger(void) +{ + // Check all port groups are ready + for (uint8_t i = 0; i < _num_port_groups; i++) { + if (!_port_groups[i].cycle_complete) { + PX4_WARN("Bitbang cycle not complete for port group %u", i); + return; + } + } + + for (uint8_t pg_idx = 0; pg_idx < _num_port_groups; pg_idx++) { + port_group_t *pg = &_port_groups[pg_idx]; + + if (pg->timer_index == UINT8_MAX || pg->pin_count == 0) { + continue; + } + + // Zero BSRR buffer, then rebuild from stored packets + memset(_bsrr_buffer[pg_idx], 0, sizeof(_bsrr_buffer[pg_idx])); + + for (uint8_t p = 0; p < pg->pin_count; p++) { + uint8_t ch = pg->output_channels[p]; + + if (_channel_initialized[ch]) { + build_bsrr_for_channel(ch); + } + } + + // Add trailing word to return pins to idle state + if (pg->bidirectional) { + _bsrr_buffer[pg_idx][BSRR_BUFFER_SIZE - 1] = pg->pin_mask; // SET = idle HIGH + + } else { + _bsrr_buffer[pg_idx][BSRR_BUFFER_SIZE - 1] = pg->pin_mask << 16; // RESET = idle LOW + } + + if (pg->bidirectional) { + pg->cycle_complete = false; + } + + // Ensure GPIO is in output mode + gpio_set_mode_output(pg); + + // Flush dcache + up_clean_dcache((uintptr_t)_bsrr_buffer[pg_idx], + (uintptr_t)_bsrr_buffer[pg_idx] + DMA_ALIGN_UP(sizeof(_bsrr_buffer[pg_idx]))); + + // Free any previous DMA handle + if (pg->dma_handle != NULL) { + stm32_dmastop(pg->dma_handle); + stm32_dmafree(pg->dma_handle); + pg->dma_handle = NULL; + } + + // Allocate DMA + pg->dma_handle = stm32_dmachannel(io_timers[pg->timer_index].dshot.dma_map_up); + + if (pg->dma_handle == NULL) { + PX4_WARN("DMA allocation failed for port group %u", pg_idx); + pg->cycle_complete = true; + continue; + } + + // Setup DMA: memory -> GPIO BSRR + px4_stm32_dmasetup(pg->dma_handle, + pg->gpio_base + STM32_GPIO_BSRR_OFFSET, + (uint32_t)_bsrr_buffer[pg_idx], + BSRR_BUFFER_SIZE, + DSHOT_DMA_SCR_TX); + + // Clear UDE before starting + timer_set_ude(pg->timer_index, false); + + // Reset counter + rCNT(pg->timer_index) = 0; + rSR(pg->timer_index) = 0; + + // Start DMA with callback + stm32_dmastart(pg->dma_handle, tx_dma_complete_callback, &pg->pg_index, false); + + // Enable UDE then start timer + timer_set_ude(pg->timer_index, true); + timer_enable(pg->timer_index); + } +} + +static void tx_dma_complete_callback(DMA_HANDLE handle, uint8_t status, void *arg) +{ + uint8_t pg_index = *((uint8_t *)arg); + port_group_t *pg = &_port_groups[pg_index]; + + // Stop DMA and timer + stm32_dmastop(pg->dma_handle); + stm32_dmafree(pg->dma_handle); + pg->dma_handle = NULL; + timer_set_ude(pg->timer_index, false); + timer_disable(pg->timer_index); + + // For non-BDShot or during ESC boot: done + if (!pg->bidirectional || (hrt_absolute_time() <= ESC_BOOT_DELAY_US)) { + if (pg->bidirectional) { + pg->cycle_complete = true; + } + + return; + } + + // Switch GPIO to input mode for capture + gpio_set_mode_input(pg); + + // Schedule capture start after ESC turnaround time (~30us) + hrt_call_after(&_capture_calls[pg_index], 30, capture_start_callback, &pg->pg_index); +} + +static void capture_start_callback(void *arg) +{ + uint8_t pg_index = *((uint8_t *)arg); + port_group_t *pg = &_port_groups[pg_index]; + + // Zero capture buffer + memset(_idr_buffer[pg_index], 0, sizeof(_idr_buffer[pg_index])); + up_clean_dcache((uintptr_t)_idr_buffer[pg_index], + (uintptr_t)_idr_buffer[pg_index] + DMA_ALIGN_UP(sizeof(_idr_buffer[pg_index]))); + + // Allocate DMA (reuse same DMAMUX route) + pg->dma_handle = stm32_dmachannel(io_timers[pg->timer_index].dshot.dma_map_up); + + if (pg->dma_handle == NULL) { + PX4_WARN("DMA allocation failed for capture, port group %u", pg_index); + gpio_set_mode_output(pg); + gpio_set_idle_state(pg); + pg->cycle_complete = true; + return; + } + + // Setup DMA: GPIO IDR -> memory + px4_stm32_dmasetup(pg->dma_handle, + pg->gpio_base + STM32_GPIO_IDR_OFFSET, + (uint32_t)_idr_buffer[pg_index], + CAPTURE_SAMPLE_COUNT, + DSHOT_DMA_SCR_RX); + + // Clear UDE before starting + timer_set_ude(pg->timer_index, false); + rCNT(pg->timer_index) = 0; + rSR(pg->timer_index) = 0; + + // Start DMA (no callback - we use hrt timer for completion) + stm32_dmastart(pg->dma_handle, NULL, NULL, false); + + // Enable UDE then start timer + timer_set_ude(pg->timer_index, true); + timer_enable(pg->timer_index); + + // Schedule capture complete after GCR frame time + margin + // GCR frame = 21 bits at DShot rate * 4/5 + // Frame time in us = 21 * (1e6 / (dshot_freq * 5/4)) = 21 * 4e6 / (5 * dshot_freq) + hrt_abstime frame_us = (21 * 4 * 1000000ULL) / (5 * _dshot_frequency); + hrt_abstime delay = frame_us + 20; // +20us margin + hrt_call_after(&_capture_calls[pg_index], delay, capture_complete_callback, &pg->pg_index); +} + +static void capture_complete_callback(void *arg) +{ + uint8_t pg_index = *((uint8_t *)arg); + port_group_t *pg = &_port_groups[pg_index]; + + // Stop DMA and timer + if (pg->dma_handle != NULL) { + stm32_dmastop(pg->dma_handle); + stm32_dmafree(pg->dma_handle); + pg->dma_handle = NULL; + } + + timer_set_ude(pg->timer_index, false); + timer_disable(pg->timer_index); + + // Invalidate dcache + up_invalidate_dcache((uintptr_t)_idr_buffer[pg_index], + (uintptr_t)_idr_buffer[pg_index] + DMA_ALIGN_UP(sizeof(_idr_buffer[pg_index]))); + + // Process capture for ALL motors on this port (not round-robin) + process_capture_results(pg_index); + + // Switch GPIO back to output mode and set idle state + gpio_set_mode_output(pg); + gpio_set_idle_state(pg); + + pg->cycle_complete = true; +} + +static void process_capture_results(uint8_t pg_index) +{ + port_group_t *pg = &_port_groups[pg_index]; + + for (uint8_t p = 0; p < pg->pin_count; p++) { + uint8_t output_channel = pg->output_channels[p]; + uint8_t pin_num = pg->pin_numbers[p]; + + if (!(_bdshot_channel_mask & (1 << output_channel))) { + continue; + } + + uint32_t value = convert_idr_to_bitstream(pg_index, pin_num, output_channel); + + // Decode RLL + value = (value ^ (value >> 1)); + + // Decode GCR + uint32_t payload = gcr_decode[value & 0x1f]; + payload |= gcr_decode[(value >> 5) & 0x1f] << 4; + payload |= gcr_decode[(value >> 10) & 0x1f] << 8; + payload |= gcr_decode[(value >> 15) & 0x1f] << 12; + + // Calculate checksum + uint32_t checksum = payload; + checksum = checksum ^ (checksum >> 8); + checksum = checksum ^ (checksum >> NIBBLES_SIZE); + + if ((checksum & 0xF) != 0xF) { + ++read_fail_crc[output_channel]; + + if (_consecutive_failures[output_channel]++ > BDSHOT_OFFLINE_COUNT) { + _consecutive_failures[output_channel] = BDSHOT_OFFLINE_COUNT; + _consecutive_successes[output_channel] = 0; + _bdshot_online[output_channel] = false; + } + + continue; + } + + ++read_ok[output_channel]; + + if (_consecutive_successes[output_channel]++ > BDSHOT_OFFLINE_COUNT) { + _consecutive_successes[output_channel] = BDSHOT_OFFLINE_COUNT; + _consecutive_failures[output_channel] = 0; + _bdshot_online[output_channel] = true; + } + + // Convert payload into telemetry type/value + struct BDShotTelemetry packet = {}; + payload = (payload >> 4) & 0xFFF; + decode_dshot_telemetry(payload, &packet); + + hrt_abstime now = hrt_absolute_time(); + + switch (packet.type) { + case DSHOT_EDT_ERPM: { + _erpms[output_channel].erpm = packet.value; + _erpms[output_channel].ready = true; + uint64_t last_timestamp = _erpms[output_channel].last_timestamp; + float last_rate_hz = _erpms[output_channel].rate_hz; + _erpms[output_channel].rate_hz = calculate_rate_hz(last_timestamp, last_rate_hz, now); + _erpms[output_channel].last_timestamp = now; + break; + } + + case DSHOT_EDT_TEMPERATURE: { + _edt_temp[output_channel].value = packet.value; + _edt_temp[output_channel].ready = true; + uint64_t last_timestamp = _edt_temp[output_channel].last_timestamp; + float last_rate_hz = _edt_temp[output_channel].rate_hz; + _edt_temp[output_channel].rate_hz = calculate_rate_hz(last_timestamp, last_rate_hz, now); + _edt_temp[output_channel].last_timestamp = now; + break; + } + + case DSHOT_EDT_VOLTAGE: { + _edt_volt[output_channel].value = packet.value; + _edt_volt[output_channel].ready = true; + uint64_t last_timestamp = _edt_volt[output_channel].last_timestamp; + float last_rate_hz = _edt_volt[output_channel].rate_hz; + _edt_volt[output_channel].rate_hz = calculate_rate_hz(last_timestamp, last_rate_hz, now); + _edt_volt[output_channel].last_timestamp = now; + break; + } + + case DSHOT_EDT_CURRENT: { + _edt_curr[output_channel].value = packet.value; + _edt_curr[output_channel].ready = true; + uint64_t last_timestamp = _edt_curr[output_channel].last_timestamp; + float last_rate_hz = _edt_curr[output_channel].rate_hz; + _edt_curr[output_channel].rate_hz = calculate_rate_hz(last_timestamp, last_rate_hz, now); + _edt_curr[output_channel].last_timestamp = now; + break; + } + + case DSHOT_EDT_STATE_EVENT: + break; + + default: + break; + } + } +} + +// Convert IDR capture samples to a GCR bitstream for a single pin +static uint32_t convert_idr_to_bitstream(uint8_t pg_index, uint8_t pin_num, uint8_t output_channel) +{ + uint16_t *buffer = _idr_buffer[pg_index]; + uint16_t pin_bit = (1u << pin_num); + + // First pass: extract edges and collect intervals + uint32_t intervals[64]; + unsigned interval_count = 0; + + // BDShot response starts HIGH (idle), then the ESC pulls LOW to begin. + // Skip initial idle samples, find the first falling edge. + unsigned start = 0; + + for (unsigned i = 1; i < CAPTURE_SAMPLE_COUNT; i++) { + bool prev = (buffer[i - 1] & pin_bit) != 0; + bool curr = (buffer[i] & pin_bit) != 0; + + if (prev && !curr) { + // Found first falling edge - start of GCR frame + start = i; + break; + } + } + + if (start == 0) { + // No falling edge found + return 0; + } + + // Collect edge intervals from the start of the frame + unsigned last_edge = start; + bool prev_state = (buffer[start] & pin_bit) != 0; + + for (unsigned i = start + 1; i < CAPTURE_SAMPLE_COUNT; i++) { + bool state = (buffer[i] & pin_bit) != 0; + + if (state != prev_state) { + uint32_t interval = i - last_edge; + + if (interval_count < 64) { + intervals[interval_count++] = interval; + } + + last_edge = i; + prev_state = state; + } + } + + if (interval_count == 0) { + return 0; + } + + // Calibrate base interval + uint32_t min_interval = intervals[0]; + + for (unsigned i = 1; i < interval_count; i++) { + if (intervals[i] < min_interval) { + min_interval = intervals[i]; + } + } + + // Filter out obviously invalid minimum intervals + if (min_interval < 4 || min_interval > 80) { + return 0; + } + + // Update base interval with low-pass filter + _base_interval[output_channel] = 0.9f * _base_interval[output_channel] + 0.1f * (float)min_interval; + + // Second pass: decode bits using adaptive threshold + uint32_t value = 0; + uint32_t high = 1; // First edge is falling (idle HIGH -> LOW), so first run is HIGH + unsigned shifted = 0; + + float base = _base_interval[output_channel]; + + for (unsigned i = 0; i < interval_count; i++) { + float interval_f = (float)intervals[i]; + uint32_t bits = (uint32_t)((interval_f / base) + 0.5f); + + if (bits < 1) { + bits = 1; + + } else if (bits > 4) { + bits = 4; + } + + for (unsigned bit = 0; bit < bits; ++bit) { + value = (value << 1) | high; + ++shifted; + } + + high = !high; + } + + // Flexible frame validation + if (shifted < 18 || shifted > 24) { + return 0; + } + + // Pad/trim to 21 bits + if (shifted < 21) { + value <<= (21 - shifted); + + } else if (shifted > 21) { + value >>= (shifted - 21); + } + + return value; +} + +static void decode_dshot_telemetry(uint32_t payload, struct BDShotTelemetry *packet) +{ + bool edt_enabled = _edt_enabled; + uint32_t mantissa = payload & 0x01FF; + bool is_telemetry = !(mantissa & 0x0100); + + if (edt_enabled && is_telemetry) { + packet->type = (payload & 0x0F00) >> 8; + packet->value = payload & 0x00FF; + + } else { + uint8_t exponent = ((payload >> 9) & 0x7); + uint16_t period = (payload & 0x1FF); + period = period << exponent; + + packet->type = DSHOT_EDT_ERPM; + + if (period == 65408 || period == 0) { + packet->value = 0; + + } else { + packet->value = (1000000 * 60 / 100 + period / 2) / period; + } + } +} + +static float calculate_rate_hz(uint64_t last_timestamp, float last_rate_hz, uint64_t timestamp) +{ + if (last_timestamp == 0 || timestamp <= last_timestamp) { + return last_rate_hz; + } + + uint64_t dt_us = timestamp - last_timestamp; + float instant_rate = 1000000.0f / dt_us; + float rate_hz = instant_rate * 0.125f + last_rate_hz * 0.875f; + return rate_hz; +} + +int up_dshot_arm(bool armed) +{ + _armed = armed; + + for (uint8_t pg_idx = 0; pg_idx < _num_port_groups; pg_idx++) { + port_group_t *pg = &_port_groups[pg_idx]; + + if (armed) { + gpio_set_mode_output(pg); + gpio_set_idle_state(pg); + + } else { + // Reset all motor pins LOW when disarmed + gpio_set_mode_output(pg); + putreg32(pg->pin_mask << 16, pg->gpio_base + STM32_GPIO_BSRR_OFFSET); + } + } + + return PX4_OK; +} + +int up_bdshot_num_errors(uint8_t channel) +{ + return read_fail_crc[channel]; +} + +int up_bdshot_get_erpm(uint8_t channel, int *erpm) +{ + if (channel >= MAX_TIMER_IO_CHANNELS || !_channel_initialized[channel]) { + return PX4_ERROR; + } + + if (_erpms[channel].ready) { + *erpm = _erpms[channel].erpm; + return PX4_OK; + } + + return PX4_ERROR; +} + +int up_bdshot_get_extended_telemetry(uint8_t channel, int type, uint8_t *value) +{ + int result = PX4_ERROR; + + switch (type) { + case DSHOT_EDT_TEMPERATURE: + if (_edt_temp[channel].ready) { + *value = _edt_temp[channel].value; + _edt_temp[channel].ready = false; + result = PX4_OK; + } + + break; + + case DSHOT_EDT_VOLTAGE: + if (_edt_volt[channel].ready) { + *value = _edt_volt[channel].value; + _edt_volt[channel].ready = false; + result = PX4_OK; + } + + break; + + case DSHOT_EDT_CURRENT: + if (_edt_curr[channel].ready) { + *value = _edt_curr[channel].value; + _edt_curr[channel].ready = false; + result = PX4_OK; + } + + break; + + default: + break; + } + + return result; +} + +int up_bdshot_channel_online(uint8_t channel) +{ + if (channel >= MAX_TIMER_IO_CHANNELS) { + return 0; + } + + return _bdshot_online[channel]; +} + +int up_bdshot_channel_capture_supported(uint8_t channel) +{ + if (channel >= MAX_TIMER_IO_CHANNELS) { + return 0; + } + + return _bdshot_capture_supported[channel]; +} + +void up_bdshot_status(void) +{ + PX4_INFO("dshot bitbang driver stats:"); + + for (uint8_t pg_idx = 0; pg_idx < _num_port_groups; pg_idx++) { + port_group_t *pg = &_port_groups[pg_idx]; + PX4_INFO(" Port group %u: GPIO port %c, %u pins, pin_mask=0x%04lx, pacer timer idx %u", + pg_idx, 'A' + pg->port_index, pg->pin_count, + (unsigned long)pg->pin_mask, pg->timer_index); + } + + if (_bdshot_channel_mask) { + PX4_INFO("BDShot channel mask: 0x%02x", (unsigned)_bdshot_channel_mask); + + for (int i = 0; i < MAX_TIMER_IO_CHANNELS; i++) { + if (_bdshot_channel_mask & (1 << i)) { + if (_bdshot_capture_supported[i]) { + PX4_INFO("Ch%u: read_ok %lu, fail_crc %lu", + i, read_ok[i], read_fail_crc[i]); + + } else { + PX4_INFO("Ch%u: capture not supported", i); + } + } + } + } + + if (_edt_enabled) { + PX4_INFO("BDShot EDT"); + + for (int i = 0; i < MAX_TIMER_IO_CHANNELS; i++) { + if (_bdshot_online[i]) { + PX4_INFO("Ch%d: eRPM: %dHz Temp: %dC (%.1fHz) Volt: %.2fV (%.1fHz) Curr: %.1fA (%.1fHz)", + i, + (int)_erpms[i].rate_hz, + (int)_edt_temp[i].value, + (double)_edt_temp[i].rate_hz, + (double)_edt_volt[i].value * 0.25, + (double)_edt_volt[i].rate_hz, + (double)_edt_curr[i].value * 0.5, + (double)_edt_curr[i].rate_hz); + } + } + } +} + +#endif