From f00e46f61856640dcef3d419dd39fc8c62758d0b Mon Sep 17 00:00:00 2001
From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
Date: Tue, 17 Mar 2026 16:38:33 -0800
Subject: [PATCH] feat(dshot): Extended Telemetry and EEPROM support
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
Overhauls the DShot driver with per-timer BDShot selection, multi-timer
sequential capture, Extended DShot Telemetry (EDT), and AM32 ESC EEPROM
read/write via MAVLink. Expands ESC support from 8 to 12 channels.
BDShot:
- Per-timer BDShot protocol selection via actuator config UI
- Multi-timer sequential burst/capture on any DMA-capable timer
- Adaptive per-channel GCR bitstream decoding
- Per-channel online/offline detection with hysteresis
Extended DShot Telemetry (EDT):
- Temperature, voltage, current from BDShot frames (no serial wire)
- New DSHOT_BIDIR_EDT parameter
- EDT data merged with serial telemetry when both available
AM32 EEPROM:
- Read/write AM32 ESC settings via MAVLink ESC_EEPROM message
- ESCSettingsInterface abstraction for future ESC firmware types
- New DSHOT_ESC_TYPE parameter
Other changes:
- Per-motor pole count params DSHOT_MOT_POL1–12 (replaces MOT_POLE_COUNT)
- EscStatus/EscReport expanded to 12 ESCs with uint16 bitmasks
- Numerous bounds-check, overflow, and concurrency fixes
- Updated DShot documentation
---
boards/ark/fmu-v6x/mavlink-dev.px4board | 1 +
boards/ark/fmu-v6x/src/board_config.h | 3 -
boards/ark/fpv/init/rc.board_airframes | 19 +
boards/ark/fpv/mavlink-dev.px4board | 1 +
boards/ark/fpv/src/board_config.h | 3 -
boards/ark/pi6x/mavlink-dev.px4board | 1 +
boards/ark/pi6x/src/board_config.h | 3 -
.../nuttx-config/include/board_dma_map.h | 12 +-
.../kakuteh7-wing/src/timer_config.cpp | 10 +-
docs/en/config_mc/filter_tuning.md | 2 +-
docs/en/esc/ark_4in1_esc.md | 12 +
docs/en/peripherals/dshot.md | 157 +-
docs/en/test_cards/mc_08_dshot.md | 2 +-
msg/CMakeLists.txt | 2 +
msg/EscEepromRead.msg | 7 +
msg/EscEepromWrite.msg | 8 +
msg/EscReport.msg | 49 +-
msg/EscStatus.msg | 48 +-
msg/versioned/VehicleCommand.msg | 8 +
.../nuttx/src/px4/nxp/imxrt/dshot/dshot.c | 59 +-
.../src/px4/stm/stm32_common/dshot/dshot.c | 997 ++++++----
.../vertiq_io/vertiq_telemetry_manager.cpp | 2 -
src/drivers/actuators/voxl_esc/voxl_esc.cpp | 6 +-
src/drivers/drv_dshot.h | 113 +-
src/drivers/dshot/CMakeLists.txt | 4 +-
src/drivers/dshot/DShot.cpp | 1635 +++++++++++------
src/drivers/dshot/DShot.h | 259 ++-
src/drivers/dshot/DShotCommon.h | 93 +
src/drivers/dshot/DShotTelemetry.cpp | 481 +++--
src/drivers/dshot/DShotTelemetry.h | 101 +-
src/drivers/dshot/esc/AM32Settings.cpp | 85 +
src/drivers/dshot/esc/AM32Settings.h | 57 +
src/drivers/dshot/esc/ESCSettingsInterface.h | 54 +
src/drivers/dshot/module.yaml | 35 +-
src/drivers/pwm_out/module.yaml | 3 +
src/drivers/uavcan/actuators/esc.cpp | 4 +-
src/drivers/uavcan/actuators/esc.hpp | 2 +-
src/lib/mixer_module/mixer_module.hpp | 2 +
src/lib/parameters/param_translation.cpp | 15 +
src/modules/commander/Commander.cpp | 1 +
src/modules/logger/logged_topics.cpp | 2 +-
src/modules/mavlink/mavlink_main.cpp | 15 +
src/modules/mavlink/mavlink_messages.cpp | 7 +
src/modules/mavlink/mavlink_receiver.cpp | 74 +-
src/modules/mavlink/mavlink_receiver.h | 12 +
src/modules/mavlink/streams/ESC_EEPROM.hpp | 89 +
src/modules/mavlink/streams/ESC_INFO.hpp | 87 +-
src/modules/mavlink/streams/ESC_STATUS.hpp | 47 +-
48 files changed, 2997 insertions(+), 1692 deletions(-)
create mode 100644 boards/ark/fmu-v6x/mavlink-dev.px4board
create mode 100644 boards/ark/fpv/init/rc.board_airframes
create mode 100644 boards/ark/fpv/mavlink-dev.px4board
create mode 100644 boards/ark/pi6x/mavlink-dev.px4board
create mode 100644 msg/EscEepromRead.msg
create mode 100644 msg/EscEepromWrite.msg
create mode 100644 src/drivers/dshot/DShotCommon.h
create mode 100644 src/drivers/dshot/esc/AM32Settings.cpp
create mode 100644 src/drivers/dshot/esc/AM32Settings.h
create mode 100644 src/drivers/dshot/esc/ESCSettingsInterface.h
create mode 100644 src/modules/mavlink/streams/ESC_EEPROM.hpp
diff --git a/boards/ark/fmu-v6x/mavlink-dev.px4board b/boards/ark/fmu-v6x/mavlink-dev.px4board
new file mode 100644
index 0000000000..83cac6e17b
--- /dev/null
+++ b/boards/ark/fmu-v6x/mavlink-dev.px4board
@@ -0,0 +1 @@
+CONFIG_MAVLINK_DIALECT="development"
diff --git a/boards/ark/fmu-v6x/src/board_config.h b/boards/ark/fmu-v6x/src/board_config.h
index 049a99d6e6..037fd11296 100644
--- a/boards/ark/fmu-v6x/src/board_config.h
+++ b/boards/ark/fmu-v6x/src/board_config.h
@@ -421,9 +421,6 @@
/* This board provides a DMA pool and APIs */
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
-/* This board has 4 DMA channels available for bidirectional dshot */
-#define BOARD_DMA_NUM_DSHOT_CHANNELS 4
-
/* This board provides the board_on_reset interface */
#define BOARD_HAS_ON_RESET 1
diff --git a/boards/ark/fpv/init/rc.board_airframes b/boards/ark/fpv/init/rc.board_airframes
new file mode 100644
index 0000000000..1cb11f713e
--- /dev/null
+++ b/boards/ark/fpv/init/rc.board_airframes
@@ -0,0 +1,19 @@
+4001_quad_x
+4050_generic_250
+6001_hexa_x
+12001_octo_cox
+13100_generic_vtol_tiltrotor
+5001_quad_+
+24001_dodeca_cox
+2100_standard_plane
+13000_generic_vtol_standard
+4601_droneblocks_dexi_5
+11001_hexa_cox
+14001_generic_mc_with_tilt
+16001_helicopter
+9001_octo_+
+7001_hexa_+
+3000_generic_wing
+2106_albatross
+13200_generic_vtol_tailsitter
+13030_generic_vtol_quad_tiltrotor
diff --git a/boards/ark/fpv/mavlink-dev.px4board b/boards/ark/fpv/mavlink-dev.px4board
new file mode 100644
index 0000000000..83cac6e17b
--- /dev/null
+++ b/boards/ark/fpv/mavlink-dev.px4board
@@ -0,0 +1 @@
+CONFIG_MAVLINK_DIALECT="development"
diff --git a/boards/ark/fpv/src/board_config.h b/boards/ark/fpv/src/board_config.h
index 17cac27a45..f75bec3e08 100644
--- a/boards/ark/fpv/src/board_config.h
+++ b/boards/ark/fpv/src/board_config.h
@@ -306,9 +306,6 @@
/* This board provides a DMA pool and APIs */
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
-/* This board has 3 DMA channels available for bidirectional dshot */
-#define BOARD_DMA_NUM_DSHOT_CHANNELS 3
-
/* This board provides the board_on_reset interface */
#define BOARD_HAS_ON_RESET 1
diff --git a/boards/ark/pi6x/mavlink-dev.px4board b/boards/ark/pi6x/mavlink-dev.px4board
new file mode 100644
index 0000000000..83cac6e17b
--- /dev/null
+++ b/boards/ark/pi6x/mavlink-dev.px4board
@@ -0,0 +1 @@
+CONFIG_MAVLINK_DIALECT="development"
diff --git a/boards/ark/pi6x/src/board_config.h b/boards/ark/pi6x/src/board_config.h
index fe2c3ac761..c76e42bded 100644
--- a/boards/ark/pi6x/src/board_config.h
+++ b/boards/ark/pi6x/src/board_config.h
@@ -311,9 +311,6 @@
/* This board provides a DMA pool and APIs */
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
-/* This board has 4 DMA channels available for bidirectional dshot */
-#define BOARD_DMA_NUM_DSHOT_CHANNELS 4
-
/* This board provides the board_on_reset interface */
#define BOARD_HAS_ON_RESET 1
diff --git a/boards/holybro/kakuteh7-wing/nuttx-config/include/board_dma_map.h b/boards/holybro/kakuteh7-wing/nuttx-config/include/board_dma_map.h
index 3bf265085c..0c54385483 100644
--- a/boards/holybro/kakuteh7-wing/nuttx-config/include/board_dma_map.h
+++ b/boards/holybro/kakuteh7-wing/nuttx-config/include/board_dma_map.h
@@ -58,11 +58,13 @@
//#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_0 /* 5 DMA1:71 PX4IO */
//#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_0 /* 6 DMA1:72 PX4IO */
-// Assigned in timer_config.cpp
-
-// TODO
-// Timer 4 /* 7 DMA1:32 TIM4UP */
-// Timer 5 /* 8 DMA1:50 TIM5UP */
+// Dynamically assigned in timer_config.cpp for DShot (allocated/freed per cycle):
+// Timer 1 TIM1UP (burst) + TIM1CH1-4 (capture)
+// Timer 2 TIM2UP (burst) + TIM2CH1-4 (capture)
+// Timer 3 TIM3UP (burst) + TIM3CH1-4 (capture)
+// Timer 4 TIM4UP (burst) + TIM4CH1-3 (capture, CH4 has no DMA)
+// Timer 5 TIM5UP (burst) + TIM5CH1-4 (capture)
+// Timer 15 - no TIM15UP DMA, cannot do DShot
// DMAMUX2 Using at most 8 Channels on DMA2 -------- Assigned
// V
diff --git a/boards/holybro/kakuteh7-wing/src/timer_config.cpp b/boards/holybro/kakuteh7-wing/src/timer_config.cpp
index eaa8d887e3..87eae8e023 100644
--- a/boards/holybro/kakuteh7-wing/src/timer_config.cpp
+++ b/boards/holybro/kakuteh7-wing/src/timer_config.cpp
@@ -38,18 +38,20 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
initIOTimer(Timer::Timer1, DMA{DMA::Index1}),
initIOTimer(Timer::Timer4, DMA{DMA::Index1}),
initIOTimer(Timer::Timer5, DMA{DMA::Index1}),
- initIOTimer(Timer::Timer15),
- initIOTimer(Timer::Timer3),
- initIOTimer(Timer::Timer2),
+ initIOTimer(Timer::Timer15), // Note: Timer15 has no TIM_UP DMA on STM32H7, cannot do DShot
+ initIOTimer(Timer::Timer3, DMA{DMA::Index1}),
+ initIOTimer(Timer::Timer2, DMA{DMA::Index1}),
};
+// Note: Timer4 Channel4 has no DMAMUX mapping on STM32H7, so BDShot telemetry capture
+// is not available on that channel. DShot output still works (uses TIM_UP DMA for burst).
constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin8}),
initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}),
initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}),
initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}),
initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}),
- initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}),
+ initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}), // no BDShot telemetry readback
initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}),
initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel1}, {GPIO::PortE, GPIO::Pin5}),
diff --git a/docs/en/config_mc/filter_tuning.md b/docs/en/config_mc/filter_tuning.md
index 868c350d9c..9400a20151 100644
--- a/docs/en/config_mc/filter_tuning.md
+++ b/docs/en/config_mc/filter_tuning.md
@@ -72,7 +72,7 @@ The ESC RPM feedback is used to track the rotor blade pass frequency and its har
ESC RPM feedback requires ESCs capable of providing RPM feedback such as [DShot](../peripherals/dshot.md) with telemetry connected, a bidirectional DShot set up ([work in progress](https://github.com/PX4/PX4-Autopilot/pull/23863)), or [UAVCAN/DroneCAN ESCs](../dronecan/escs.md).
Before enabling, make sure that the ESC RPM is correct.
-You might have to adjust the [pole count of the motors](../advanced_config/parameter_reference.md#MOT_POLE_COUNT).
+You might have to adjust the per-motor pole count (`DSHOT_MOT_POL1`–`DSHOT_MOT_POL12`).
The following parameters should be set to enable and configure dynamic notch filters:
diff --git a/docs/en/esc/ark_4in1_esc.md b/docs/en/esc/ark_4in1_esc.md
index 40fb8d607f..55445b6a94 100644
--- a/docs/en/esc/ark_4in1_esc.md
+++ b/docs/en/esc/ark_4in1_esc.md
@@ -51,6 +51,18 @@ Other
- Open source AM32 firmware
- [DIU Blue Framework Listed](https://www.diu.mil/blue-uas/framework)
+## PX4 Configuration
+
+The ARK 4IN1 ESC supports DShot 300/600, Bidirectional DShot, and PWM input protocols.
+
+- **Bidirectional DShot**: Select BDShot300 or BDShot600 in the [Actuator Configuration](../config/actuators.md) to enable eRPM telemetry.
+- **[Extended DShot Telemetry (EDT)](https://github.com/bird-sanctuary/extended-dshot-telemetry)**: AM32 firmware supports EDT, which provides temperature, voltage, and current through the BDShot signal. Enable with `DSHOT_BIDIR_EDT=1`.
+- **AM32 EEPROM Settings**: Set `DSHOT_ESC_TYPE=1` to enable reading and writing ESC firmware settings via a ground station.
+
+See [DShot ESCs](../peripherals/dshot.md) for full setup details.
+
## See Also
- [ARK 4IN1 ESC CONS](https://docs.arkelectron.com/electronic-speed-controller/ark-4in1-esc) (ARK Docs)
+- [DShot and Bidirectional DShot](https://brushlesswhoop.com/dshot-and-bidirectional-dshot/) (brushlesswhoop.com - General DShot reference)
+- [Extended DShot Telemetry (EDT) Specification](https://github.com/bird-sanctuary/extended-dshot-telemetry) (bird-sanctuary)
diff --git a/docs/en/peripherals/dshot.md b/docs/en/peripherals/dshot.md
index 23de545340..2dd1141c15 100644
--- a/docs/en/peripherals/dshot.md
+++ b/docs/en/peripherals/dshot.md
@@ -47,71 +47,12 @@ Then connect the battery and arm the vehicle.
The ESCs should initialize and the motors turn in the correct directions.
- If the motors do not spin in the correct direction (for the [selected airframe](../airframes/airframe_reference.md)) you can reverse them in the UI using the **Set Spin Direction** option (this option appears after you select DShot and assign motors).
- You can also reverse motors by sending an [ESC Command](#commands).
## ESC Commands {#commands}
Commands can be sent to the ESC via the [MAVLink shell](../debug/mavlink_shell.md).
See [here](../modules/modules_driver.md#dshot) for a full reference of the supported commands.
-The most important ones are:
-
-- Make a motor connected to FMU output pin 1 beep (helps with identifying motors)
-
- ```sh
- dshot beep1 -m 1
- ```
-
-- Retrieve ESC information (requires telemetry, see below):
-
- ```sh
- nsh> dshot esc_info -m 2
- INFO [dshot] ESC Type: #TEKKO32_4in1#
- INFO [dshot] MCU Serial Number: xxxxxx-xxxxxx-xxxxxx-xxxxxx
- INFO [dshot] Firmware version: 32.60
- INFO [dshot] Rotation Direction: normal
- INFO [dshot] 3D Mode: off
- INFO [dshot] Low voltage Limit: off
- INFO [dshot] Current Limit: off
- INFO [dshot] LED 0: unsupported
- INFO [dshot] LED 1: unsupported
- INFO [dshot] LED 2: unsupported
- INFO [dshot] LED 3: unsupported
- ```
-
-- Permanently set the spin direction of a motor connected to FMU output pin 1 (while motors are _not_ spinning):
- - Set spin direction to `reversed`:
-
- ```sh
- dshot reverse -m 1
- dshot save -m 1
- ```
-
- Retrieving ESC information will then show:
-
- ```sh
- Rotation Direction: reversed
- ```
-
- - Set spin direction to `normal`:
-
- ```sh
- dshot normal -m 1
- dshot save -m 1
- ```
-
- Retrieving ESC information will then show:
-
- ```sh
- Rotation Direction: normal
- ```
-
- ::: info
- - The commands will have no effect if the motors are spinning, or if the ESC is already set to the corresponding direction.
- - The ESC will revert to its last saved direction (normal or reversed) on reboot if `save` is not called after changing the direction.
-
- :::
-
## ESC Telemetry
Some ESCs are capable of sending telemetry back to the flight controller through a UART RX port.
@@ -130,60 +71,76 @@ To enable this feature (on ESCs that support it):
1. Join all the telemetry wires from all the ESCs together, and then connect them to one of the RX pins on an unused flight controller serial port.
2. Enable telemetry on that serial port using [DSHOT_TEL_CFG](../advanced_config/parameter_reference.md#DSHOT_TEL_CFG).
-After a reboot you can check if telemetry is working (make sure the battery is connected) using:
-
-```sh
-dshot esc_info -m 1
-```
-
-::: tip
-You may have to configure [MOT_POLE_COUNT](../advanced_config/parameter_reference.md#MOT_POLE_COUNT) to get the correct RPM values.
+:::tip
+You may have to configure the per-motor pole count parameters ([`DSHOT_MOT_POL1`–`DSHOT_MOT_POL12`](../advanced_config/parameter_reference.md#DSHOT_MOT_POL1)) to get correct RPM values.
+The default value for these is 14 poles, which is typical for 5-inch prop motors.
:::
-::: tip
-Not all DSHOT-capable ESCs support `[esc_info]`(e.g. APD 80F3x), even when telemetry is supported and enabled.
-The resulting error is:
-
-```sh
-ERROR [dshot] No data received. If telemetry is setup correctly, try again.
-```
-
-Check manufacturer documentation for confirmation/details.
+:::tip
+[Extended DShot Telemetry (EDT)](#extended-dshot-telemetry-edt) can provide temperature, voltage, and current through the BDShot signal — no serial telemetry wire needed.
:::
## Bidirectional DShot (Telemetry)
-Bidirectional DShot is a protocol that can provide telemetry including: high rate ESC RPM data, voltage, current, and temperature with a single wire.
+Bidirectional DShot (BDShot) enables the ESC to send eRPM telemetry back to the flight controller on the same signal wire used for throttle commands — no additional telemetry wire is needed for RPM data.
+High-rate eRPM data significantly improves the performance of [Dynamic Notch Filters](../config_mc/filter_tuning.md#dynamic-notch-filters) and enables more precise vehicle tuning.
-The PX4 implementation currently enables only ESC RPM (eRPM) data collection from each ESC at high frequencies.
-This telemetry significantly improves the performance of [Dynamic Notch Filters](../config_mc/filter_tuning.md#dynamic-notch-filters) and enables more precise vehicle tuning.
+With [Extended DShot Telemetry (EDT)](#extended-dshot-telemetry-edt) enabled, BDShot can also provide temperature, voltage, and current data.
+
+### Hardware Support
+
+BDShot requires a flight controller with DMA-capable timers.
+Any FMU output on a supported timer can be used for BDShot — multiple timers are supported through sequential burst/capture.
+
+Supported processors:
+
+- **STM32H7**: All FMU outputs on DMA-capable timers
+- **i.MXRT** (V6X-RT & Tropic): All FMU outputs
::: info
-The [ESC Telemetry](#esc-telemetry) described above is currently still necessary if you want voltage, current, or temperature information.
-It's setup and use is independent of bidirectional DShot.
-:::
-
-### Hardware Setup
-
The ESC must be connected to FMU outputs only.
-These will be labeled `MAIN` on flight controllers that only have one PWM bus, and `AUX` on controllers that have both `MAIN` and `AUX` ports (i.e. FCs that have an IO board).
-
-::: warning **Limited hardware support**
-This feature is only supported on flight controllers with the following processors:
-
-- STM32H7: First four FMU outputs
- - Must be connected to the first 4 FMU outputs, and these outputs must also be mapped to the same timer.
- - [KakuteH7](../flight_controller/kakuteh7v2.md) is not supported because the outputs are not mapped to the same timer.
-- [i.MXRT](../flight_controller/nxp_mr_vmu_rt1176.md) (V6X-RT & Tropic): 8 FMU outputs.
-
-No other boards are supported.
+These are labeled `MAIN` on controllers with a single PWM bus, and `AUX` on controllers with both `MAIN` and `AUX` ports (i.e. those with an IO board).
:::
-### Configuration {#bidirectional-dshot-configuration}
+### PX4 Configuration {#bidirectional-dshot-configuration}
-To enable bidirectional DShot, set the [DSHOT_BIDIR_EN](../advanced_config/parameter_reference.md#DSHOT_BIDIR_EN) parameter.
+BDShot is enabled **per-timer** in the [Actuator Configuration](../config/actuators.md) UI.
+Select **BDShot150**, **BDShot300**, or **BDShot600** as the output protocol instead of the corresponding DShot speed.
+There is no separate enable parameter — choosing a BDShot protocol activates bidirectional telemetry on that timer's outputs.
-The system calculates actual motor RPM from the received eRPM data using the [MOT_POLE_COUNT](../advanced_config/parameter_reference.md#MOT_POLE_COUNT) parameter.
-This parameter must be set correctly for accurate RPM reporting.
+The system calculates actual motor RPM from eRPM data using per-motor pole count parameters: `DSHOT_MOT_POL1` through `DSHOT_MOT_POL12` (one per motor output).
+The default is 14 poles, which is typical for 5-inch prop motors.
+If you are using AM32 ESCs, the motor pole count must also be set in the AM32 firmware configuration (e.g. via the AM32 configurator tool) to match.
+
+### Extended DShot Telemetry (EDT)
+
+EDT extends BDShot by interleaving temperature, voltage, and current data into the eRPM telemetry frames.
+This allows ESC health monitoring through the same signal wire, without requiring a separate serial telemetry connection.
+
+To enable EDT:
+
+1. Configure BDShot on the desired outputs (see above).
+2. Set `DSHOT_BIDIR_EDT` to `1` and reboot.
+
+The ESC firmware must support EDT (e.g. [AM32](https://github.com/am32-firmware/AM32)).
+
+When both serial telemetry and BDShot/EDT are enabled, the driver merges data from both sources.
+
+## AM32 ESC Settings (EEPROM)
+
+PX4 can read and write AM32 ESC firmware settings (EEPROM) via a ground station, enabling remote ESC configuration without connecting directly to each ESC.
+
+### Requirements
+
+- ESCs running [AM32 firmware](https://github.com/am32-firmware/AM32) with serial telemetry connected ([DSHOT_TEL_CFG](../advanced_config/parameter_reference.md#DSHOT_TEL_CFG))
+- `DSHOT_ESC_TYPE` set to `1` (AM32)
+- Ground station with ESC EEPROM support (QGroundControl feature in development)
+- MAVLink development dialect enabled on the flight controller
+
+### How It Works
+
+PX4 automatically reads the full EEPROM from each ESC on boot.
+The ground station can then display individual settings and allow the user to modify them.
+Changes are written back to the ESC one byte at a time using the DShot programming protocol.
diff --git a/docs/en/test_cards/mc_08_dshot.md b/docs/en/test_cards/mc_08_dshot.md
index d2012aabd5..575037cc0f 100644
--- a/docs/en/test_cards/mc_08_dshot.md
+++ b/docs/en/test_cards/mc_08_dshot.md
@@ -7,7 +7,7 @@ Regression test for DSHOT working with PX4
## Preflight
- Ensure vehicle is using a DSHOT ESC
-- Parameter [DSHOT_BIDIR_EN](../advanced_config/parameter_reference.md#DSHOT_BIDIR_EN) is enabled
+- Bidirectional DShot is configured (BDShot150/300/600 selected in [Actuator Configuration](../config/actuators.md))
- Parameter [DSHOT_TEL_CFG](../advanced_config/parameter_reference.md#DSHOT_TEL_CFG) is configured (if ESC supports telemetry)
- Parameter [SDLOG_PROFILE](../advanced_config/parameter_reference.md#SDLOG_PROFILE) has Debug (`5`) checked
diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index 66a9f7e599..06feb9a335 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -69,6 +69,8 @@ set(msg_files
DistanceSensorModeChangeRequest.msg
DronecanNodeStatus.msg
Ekf2Timestamps.msg
+ EscEepromRead.msg
+ EscEepromWrite.msg
EscReport.msg
EscStatus.msg
EstimatorAidSource1d.msg
diff --git a/msg/EscEepromRead.msg b/msg/EscEepromRead.msg
new file mode 100644
index 0000000000..c83c3881ce
--- /dev/null
+++ b/msg/EscEepromRead.msg
@@ -0,0 +1,7 @@
+uint64 timestamp # [us] Time since system start
+uint8 firmware # [-] ESC firmware type (see ESC_FIRMWARE enum in MAVLink)
+uint8 index # [-] Index of the ESC (0 = ESC1, 1 = ESC2, etc.)
+uint16 length # [-] Length of valid data
+uint8[48] data # [-] Raw ESC EEPROM data
+
+uint8 ORB_QUEUE_LENGTH = 8 # To support 8 queued up responses
diff --git a/msg/EscEepromWrite.msg b/msg/EscEepromWrite.msg
new file mode 100644
index 0000000000..8a83c097a6
--- /dev/null
+++ b/msg/EscEepromWrite.msg
@@ -0,0 +1,8 @@
+uint64 timestamp # [us] Time since system start
+uint8 firmware # [-] ESC firmware type (see ESC_FIRMWARE enum in MAVLink)
+uint8 index # [-] Index of the ESC (0 = ESC1, 1 = ESC2, etc, 255 = All)
+uint16 length # [-] Length of valid data
+uint8[48] data # [-] Raw ESC EEPROM data
+uint32[2] write_mask # [-] Bitmask indicating which bytes in the data array should be written (max 48 values)
+
+uint8 ORB_QUEUE_LENGTH = 8 # To support 8 queued up requests
diff --git a/msg/EscReport.msg b/msg/EscReport.msg
index e84f9f46de..755daf52f8 100644
--- a/msg/EscReport.msg
+++ b/msg/EscReport.msg
@@ -1,30 +1,31 @@
-uint64 timestamp # time since system start (microseconds)
-uint32 esc_errorcount # Number of reported errors by ESC - if supported
-int32 esc_rpm # Motor RPM, negative for reverse rotation [RPM] - if supported
-float32 esc_voltage # Voltage measured from current ESC [V] - if supported
-float32 esc_current # Current measured from current ESC [A] - if supported
-float32 esc_temperature # Temperature measured from current ESC [degC] - if supported
-int16 motor_temperature # Temperature measured from current motor [degC] - if supported
-uint8 esc_address # Address of current ESC (in most cases 1-8 / must be set by driver)
-uint8 esc_cmdcount # Counter of number of commands
+uint64 timestamp # [us] Time since system start
-uint8 esc_state # State of ESC - depend on Vendor
+uint32 esc_errorcount # [-] Number of reported errors by ESC - if supported
+int32 esc_rpm # [rpm] Motor RPM, negative for reverse rotation - if supported
+float32 esc_voltage # [V] Voltage measured from current ESC - if supported
+float32 esc_current # [A] Current measured from current ESC - if supported
+float32 esc_temperature # [degC] Temperature measured from current ESC - if supported
+uint8 esc_address # [-] Address of current ESC (in most cases 1-12 / must be set by driver)
+int16 motor_temperature # [degC] Temperature measured from current motor - if supported
+
+uint8 esc_state # [-] State of ESC - depend on Vendor
+
+uint8 actuator_function # [-] Actuator output function (one of Motor1...MotorN)
-uint8 actuator_function # actuator output function (one of Motor1...MotorN)
uint8 ACTUATOR_FUNCTION_MOTOR1 = 101
uint8 ACTUATOR_FUNCTION_MOTOR_MAX = 112 # output_functions.yaml Motor.start + Motor.count - 1
-uint16 failures # Bitmask to indicate the internal ESC faults
-int8 esc_power # Applied power 0-100 in % (negative values reserved)
+uint16 failures # [@enum FAILURE] Bitmask to indicate the internal ESC faults
+int8 esc_power # [%] [@range 0,100] Applied power (negative values reserved)
-uint8 FAILURE_OVER_CURRENT = 0 # (1 << 0)
-uint8 FAILURE_OVER_VOLTAGE = 1 # (1 << 1)
-uint8 FAILURE_MOTOR_OVER_TEMPERATURE = 2 # (1 << 2)
-uint8 FAILURE_OVER_RPM = 3 # (1 << 3)
-uint8 FAILURE_INCONSISTENT_CMD = 4 # (1 << 4) Set if ESC received an inconsistent command (i.e out of boundaries)
-uint8 FAILURE_MOTOR_STUCK = 5 # (1 << 5)
-uint8 FAILURE_GENERIC = 6 # (1 << 6)
-uint8 FAILURE_MOTOR_WARN_TEMPERATURE = 7 # (1 << 7)
-uint8 FAILURE_WARN_ESC_TEMPERATURE = 8 # (1 << 8)
-uint8 FAILURE_OVER_ESC_TEMPERATURE = 9 # (1 << 9)
-uint8 ESC_FAILURE_COUNT = 10 # Counter - keep it as last element!
+uint8 FAILURE_OVER_CURRENT = 0 # (1 << 0)
+uint8 FAILURE_OVER_VOLTAGE = 1 # (1 << 1)
+uint8 FAILURE_MOTOR_OVER_TEMPERATURE = 2 # (1 << 2)
+uint8 FAILURE_OVER_RPM = 3 # (1 << 3)
+uint8 FAILURE_INCONSISTENT_CMD = 4 # (1 << 4) Set if ESC received an inconsistent command (i.e out of boundaries)
+uint8 FAILURE_MOTOR_STUCK = 5 # (1 << 5)
+uint8 FAILURE_GENERIC = 6 # (1 << 6)
+uint8 FAILURE_MOTOR_WARN_TEMPERATURE = 7 # (1 << 7)
+uint8 FAILURE_WARN_ESC_TEMPERATURE = 8 # (1 << 8)
+uint8 FAILURE_OVER_ESC_TEMPERATURE = 9 # (1 << 9)
+uint8 ESC_FAILURE_COUNT = 10 # Counter - keep it as last element!
diff --git a/msg/EscStatus.msg b/msg/EscStatus.msg
index e5e220ce0d..752b441a95 100644
--- a/msg/EscStatus.msg
+++ b/msg/EscStatus.msg
@@ -1,28 +1,32 @@
-uint64 timestamp # time since system start (microseconds)
-uint8 CONNECTED_ESC_MAX = 8 # The number of ESCs supported. Current (Q2/2013) we support 8 ESCs
+uint64 timestamp # [us] Time since system start
+uint8 CONNECTED_ESC_MAX = 12 # The number of ESCs supported (Motor1-Motor12)
-uint8 ESC_CONNECTION_TYPE_PPM = 0 # Traditional PPM ESC
-uint8 ESC_CONNECTION_TYPE_SERIAL = 1 # Serial Bus connected ESC
-uint8 ESC_CONNECTION_TYPE_ONESHOT = 2 # One Shot PPM
-uint8 ESC_CONNECTION_TYPE_I2C = 3 # I2C
-uint8 ESC_CONNECTION_TYPE_CAN = 4 # CAN-Bus
-uint8 ESC_CONNECTION_TYPE_DSHOT = 5 # DShot
+uint8 ESC_CONNECTION_TYPE_PPM = 0 # Traditional PPM ESC
+uint8 ESC_CONNECTION_TYPE_SERIAL = 1 # Serial Bus connected ESC
+uint8 ESC_CONNECTION_TYPE_ONESHOT = 2 # One Shot PPM
+uint8 ESC_CONNECTION_TYPE_I2C = 3 # I2C
+uint8 ESC_CONNECTION_TYPE_CAN = 4 # CAN-Bus
+uint8 ESC_CONNECTION_TYPE_DSHOT = 5 # DShot
-uint16 counter # incremented by the writing thread everytime new data is stored
+uint16 counter # [-] Incremented by the writing thread everytime new data is stored
-uint8 esc_count # number of connected ESCs
-uint8 esc_connectiontype # how ESCs connected to the system
+uint8 esc_count # [-] Number of connected ESCs
+uint8 esc_connectiontype # [@enum ESC_CONNECTION_TYPE] How ESCs connected to the system
-uint8 esc_online_flags # Bitmask indicating which ESC is online/offline
-# esc_online_flags bit 0 : Set to 1 if ESC0 is online
-# esc_online_flags bit 1 : Set to 1 if ESC1 is online
-# esc_online_flags bit 2 : Set to 1 if ESC2 is online
-# esc_online_flags bit 3 : Set to 1 if ESC3 is online
-# esc_online_flags bit 4 : Set to 1 if ESC4 is online
-# esc_online_flags bit 5 : Set to 1 if ESC5 is online
-# esc_online_flags bit 6 : Set to 1 if ESC6 is online
-# esc_online_flags bit 7 : Set to 1 if ESC7 is online
+uint16 esc_online_flags # Bitmask indicating which ESC is online/offline (in motor order)
+# esc_online_flags bit 0 : Set to 1 if Motor1 is online
+# esc_online_flags bit 1 : Set to 1 if Motor2 is online
+# esc_online_flags bit 2 : Set to 1 if Motor3 is online
+# esc_online_flags bit 3 : Set to 1 if Motor4 is online
+# esc_online_flags bit 4 : Set to 1 if Motor5 is online
+# esc_online_flags bit 5 : Set to 1 if Motor6 is online
+# esc_online_flags bit 6 : Set to 1 if Motor7 is online
+# esc_online_flags bit 7 : Set to 1 if Motor8 is online
+# esc_online_flags bit 8 : Set to 1 if Motor9 is online
+# esc_online_flags bit 9 : Set to 1 if Motor10 is online
+# esc_online_flags bit 10: Set to 1 if Motor11 is online
+# esc_online_flags bit 11: Set to 1 if Motor12 is online
-uint8 esc_armed_flags # Bitmask indicating which ESC is armed. For ESC's where the arming state is not known (returned by the ESC), the arming bits should always be set.
+uint16 esc_armed_flags # [-] Bitmask indicating which ESC is armed (in motor order)
-EscReport[8] esc
+EscReport[12] esc
diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg
index 9503477ce4..6ea47acc27 100644
--- a/msg/versioned/VehicleCommand.msg
+++ b/msg/versioned/VehicleCommand.msg
@@ -80,6 +80,7 @@ uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information
uint16 VEHICLE_CMD_MISSION_START = 300 # Start running a mission. |first_item: the first mission item to run|last_item: the last mission item to run (after this item is run, the mission ends)|
uint16 VEHICLE_CMD_ACTUATOR_TEST = 310 # Actuator testing command. |[@range -1,1] value|[s] timeout|Unused|Unused|output function|
uint16 VEHICLE_CMD_CONFIGURE_ACTUATOR = 311 # Actuator configuration command. |configuration|Unused|Unused|Unused|output function|
+uint16 VEHICLE_CMD_ESC_REQUEST_EEPROM = 312 # Request EEPROM data from an ESC. |ESC Index|Firmware Type|Unused|Unused|Unused|
uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component. |1 to arm, 0 to disarm.
uint16 VEHICLE_CMD_RUN_PREARM_CHECKS = 401 # Instructs a target system to run pre-arm checks.
uint16 VEHICLE_CMD_INJECT_FAILURE = 420 # Inject artificial failure for testing purposes.
@@ -129,6 +130,13 @@ uint8 VEHICLE_ROI_LOCATION = 3 # Point toward fixed location.
uint8 VEHICLE_ROI_TARGET = 4 # Point toward target.
uint8 VEHICLE_ROI_ENUM_END = 5
+uint8 ACTUATOR_CONFIGURATION_NONE = 0 # Do nothing.
+uint8 ACTUATOR_CONFIGURATION_BEEP = 1 # Command the actuator to beep now.
+uint8 ACTUATOR_CONFIGURATION_3D_MODE_ON = 2 # Permanently set the actuator (ESC) to 3D mode (reversible thrust).
+uint8 ACTUATOR_CONFIGURATION_3D_MODE_OFF = 3 # Permanently set the actuator (ESC) to non 3D mode (non-reversible thrust).
+uint8 ACTUATOR_CONFIGURATION_SPIN_DIRECTION1 = 4 # Permanently set the actuator (ESC) to spin direction 1 (which can be clockwise or counter-clockwise).
+uint8 ACTUATOR_CONFIGURATION_SPIN_DIRECTION2 = 5 # Permanently set the actuator (ESC) to spin direction 2 (opposite of direction 1).
+
uint8 PARACHUTE_ACTION_DISABLE = 0
uint8 PARACHUTE_ACTION_ENABLE = 1
uint8 PARACHUTE_ACTION_RELEASE = 2
diff --git a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c
index 7bd2508ddf..ff9d7dcc98 100644
--- a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c
+++ b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c
@@ -63,8 +63,6 @@ static const uint32_t gcr_decode[32] = {
0x0, 0x0, 0x8, 0x1, 0x0, 0x4, 0xC, 0x0
};
-uint32_t erpms[DSHOT_TIMERS];
-
typedef enum {
DSHOT_START = 0,
DSHOT_12BIT_FIFO,
@@ -321,8 +319,10 @@ static int flexio_irq_handler(int irq, void *context, void *arg)
}
-int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot)
+int up_dshot_init(uint32_t channel_mask, uint32_t bdshot_channel_mask, unsigned dshot_pwm_freq, bool edt_enable)
{
+ (void)edt_enable; // Not implemented
+
/* Calculate dshot timings based on dshot_pwm_freq */
dshot_tcmp = 0x2F00 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2) - 1) & 0xFF);
dshot_speed = dshot_pwm_freq;
@@ -360,7 +360,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi
imxrt_config_gpio(timer_io_channels[channel].dshot.pinmux | IOMUX_PULL_UP);
- if (enable_bidirectional_dshot) {
+ if (bdshot_channel_mask & (1 << channel)) {
dshot_inst[channel].bdshot = true;
dshot_inst[channel].bdshot_training_mask = 0;
dshot_inst[channel].bdshot_tcmp_offset = BDSHOT_TCMP_MIN_OFFSET;
@@ -383,7 +383,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi
flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET, 0,
FLEXIO_CTRL_FLEXEN_MASK);
- return channel_mask;
+ return dshot_mask;
}
void up_bdshot_training(uint32_t channel, uint32_t value)
@@ -497,24 +497,26 @@ void up_bdshot_erpm(void)
}
-int up_bdshot_num_erpm_ready(void)
+uint16_t up_bdshot_get_ready_mask(void)
{
- int num_ready = 0;
-
- for (unsigned i = 0; i < DSHOT_TIMERS; ++i) {
- // We only check that data has been received, rather than if it's valid.
- // This ensures data is published even if one channel has bit errors.
- if (bdshot_parsed_recv_mask & (1 << i)) {
- ++num_ready;
- }
- }
-
- return num_ready;
+ return (uint16_t)bdshot_parsed_recv_mask;
}
+int up_bdshot_num_errors(uint8_t channel)
+{
+ if (channel >= DSHOT_TIMERS) {
+ return 0;
+ }
+
+ return dshot_inst[channel].crc_error_cnt + dshot_inst[channel].frame_error_cnt + dshot_inst[channel].no_response_cnt;
+}
int up_bdshot_get_erpm(uint8_t channel, int *erpm)
{
+ if (channel >= DSHOT_TIMERS) {
+ return -1;
+ }
+
if (bdshot_parsed_recv_mask & (1 << channel)) {
*erpm = (int)dshot_inst[channel].erpm;
return 0;
@@ -523,13 +525,28 @@ int up_bdshot_get_erpm(uint8_t channel, int *erpm)
return -1;
}
-int up_bdshot_channel_status(uint8_t channel)
+int up_bdshot_get_extended_telemetry(uint8_t channel, int type, uint8_t *value)
+{
+ // NOT IMPLEMENTED
+ return -1;
+}
+
+int up_bdshot_channel_capture_supported(uint8_t channel)
+{
+ if (channel >= DSHOT_TIMERS) {
+ return 0;
+ }
+
+ return dshot_inst[channel].init && dshot_inst[channel].bdshot;
+}
+
+int up_bdshot_channel_online(uint8_t channel)
{
if (channel < DSHOT_TIMERS) {
return ((dshot_inst[channel].no_response_cnt - dshot_inst[channel].last_no_response_cnt) < BDSHOT_OFFLINE_COUNT);
}
- return -1;
+ return 0;
}
void up_bdshot_status(void)
@@ -538,7 +555,7 @@ void up_bdshot_status(void)
for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) {
if (dshot_inst[channel].init) {
- PX4_INFO("Channel %i %s Last erpm %i value", channel, up_bdshot_channel_status(channel) ? "online" : "offline",
+ PX4_INFO("Channel %i %s Last erpm %i value", channel, up_bdshot_channel_online(channel) ? "online" : "offline",
dshot_inst[channel].erpm);
PX4_INFO("BDSHOT Training done: %s TCMP offset: %d", dshot_inst[channel].bdshot_training_done ? "YES" : "NO",
dshot_inst[channel].bdshot_tcmp_offset);
@@ -601,7 +618,7 @@ uint64_t dshot_expand_data(uint16_t packet)
* bit 12 - dshot telemetry enable/disable
* bits 13-16 - XOR checksum
**/
-void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry)
+void dshot_motor_data_set(uint8_t channel, uint16_t throttle, bool telemetry)
{
if (channel < DSHOT_TIMERS && dshot_inst[channel].init) {
uint16_t csum_data;
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
index a667537d9f..7ce7b4b36d 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
+++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
@@ -1,7 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2024 PX4 Development Team. All rights reserved.
- * Author: Igor Misic
+ * 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
@@ -50,11 +49,6 @@
#include
-// This can be overriden for a specific board.
-#ifndef BOARD_DMA_NUM_DSHOT_CHANNELS
-#define BOARD_DMA_NUM_DSHOT_CHANNELS 1
-#endif
-
// DShot protocol definitions
#define ONE_MOTOR_DATA_SIZE 16u
#define MOTOR_PWM_BIT_1 14u
@@ -97,17 +91,20 @@ static void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void
static void capture_complete_callback(void *arg);
static void process_capture_results(uint8_t timer_index, uint8_t channel_index);
-static unsigned calculate_period(uint8_t timer_index, uint8_t channel_index);
+static uint32_t convert_edge_intervals_to_bitstream(uint8_t timer_index, uint8_t channel_index);
+static void decode_dshot_telemetry(uint32_t payload, struct BDShotTelemetry *packet);
+
+static void dshot_start_timer_burst(uint8_t timer_index);
// Timer configuration struct
typedef struct timer_config_t {
- DMA_HANDLE dma_handle; // DMA stream for DMA update and eRPM Capture Compare
+ DMA_HANDLE dma_handle; // DMA stream for DMA update and eRPM Capture Compare
bool enabled; // Timer enabled
bool enabled_channels[4]; // Timer Channels enabled (requested)
bool initialized; // Timer initialized
bool initialized_channels[4]; // Timer channels initialized (successfully started)
bool bidirectional; // Timer in bidir (inverted) mode
- int capture_channel_index; // Timer channel currently being catured in bidirectional mode
+ int capture_channel; // Timer channel currently being captured in bidirectional mode
uint8_t timer_index; // Timer index. Necessary to have memory for passing pointer to hrt callback
} timer_config_t;
@@ -119,32 +116,76 @@ px4_cache_aligned_data() = {};
static uint32_t *dshot_output_buffer[MAX_IO_TIMERS] = {};
// Buffer containing channel capture data for a single timer
-static uint16_t dshot_capture_buffer[MAX_NUM_CHANNELS_PER_TIMER][CHANNEL_CAPTURE_BUFF_SIZE]
+static uint16_t dshot_capture_buffer[MAX_IO_TIMERS][MAX_NUM_CHANNELS_PER_TIMER][CHANNEL_CAPTURE_BUFF_SIZE]
px4_cache_aligned_data() = {};
-static bool _bidirectional = false;
-static uint8_t _bidi_timer_index = 0; // TODO: BDSHOT_TIM param to select timer index?
+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
+};
+
+// Indicates when the bdshot capture cycle is finished. This is necessary since the captured data is
+// processed after a fixed delay in an hrt callback. System jitter can delay the firing of the hrt callback
+// and thus delay the processing of the data. This should never happen in a properly working system, as the
+// jitter would have to be longer than the control allocator update interval. A warning is issued if this
+// ever does occur.
+static volatile bool _bdshot_cycle_complete[MAX_IO_TIMERS] = { [0 ...(MAX_IO_TIMERS - 1)] = true };
+
+static uint32_t _bdshot_channel_mask = 0;
static uint32_t _dshot_frequency = 0;
+static hrt_abstime _bdshot_capture_delay = 0;
+static bool _edt_enabled = false; // Extended DShot Telemetry
-// eRPM data for channels on the singular timer
-static int32_t _erpms[MAX_TIMER_IO_CHANNELS] = {};
-static bool _erpms_ready[MAX_TIMER_IO_CHANNELS] = {};
-// hrt callback handle for captcomp post dma processing
-static struct hrt_call _cc_call;
+// Online flags, set if ESC is reponding with valid BDShot frames
+#define BDSHOT_OFFLINE_COUNT 200
+static volatile bool _bdshot_online[MAX_TIMER_IO_CHANNELS] = {};
+static volatile uint16_t _bdshot_ready_mask = 0;
+static bool _bdshot_capture_supported[MAX_TIMER_IO_CHANNELS] = {};
+static volatile int _consecutive_failures[MAX_TIMER_IO_CHANNELS] = {};
+static volatile 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, scaled by 8 for sub-tick precision (ticks_per_bit * 8)
+static uint32_t _base_interval_x8[MAX_TIMER_IO_CHANNELS] = { [0 ...(MAX_TIMER_IO_CHANNELS - 1)] = 168 }; // 21 * 8
+
+static volatile erpm_data_t _erpms[MAX_TIMER_IO_CHANNELS] = {};
+static volatile edt_data_t _edt_temp[MAX_TIMER_IO_CHANNELS] = {};
+static volatile edt_data_t _edt_volt[MAX_TIMER_IO_CHANNELS] = {};
+static volatile edt_data_t _edt_curr[MAX_TIMER_IO_CHANNELS] = {};
+
+static float calculate_rate_hz(uint64_t last_timestamp, float last_rate_hz, uint64_t timestamp);
+
+// hrt callback handle for captcomp post dma processing per timer
+static struct hrt_call _cc_calls[MAX_IO_TIMERS];
// decoding status for each channel
-static uint32_t read_ok[MAX_NUM_CHANNELS_PER_TIMER] = {};
-static uint32_t read_fail_nibble[MAX_NUM_CHANNELS_PER_TIMER] = {};
-static uint32_t read_fail_crc[MAX_NUM_CHANNELS_PER_TIMER] = {};
-static uint32_t read_fail_zero[MAX_NUM_CHANNELS_PER_TIMER] = {};
+static uint32_t read_ok[MAX_TIMER_IO_CHANNELS] = {};
+static uint32_t read_fail_crc[MAX_TIMER_IO_CHANNELS] = {};
-static perf_counter_t hrt_callback_perf = NULL;
+static perf_counter_t capture_complete_perf = NULL;
+static perf_counter_t burst_perf = NULL;
+static perf_counter_t capture_window_perf = NULL;
static void init_timer_config(uint32_t channel_mask)
{
- // Mark timers in use, channels in use, and timers for bidir dshot
- for (unsigned output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) {
+ // Mark timers in use, channels in use, and timers for bdshot
+ for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) {
if (channel_mask & (1 << output_channel)) {
uint8_t timer_index = timer_io_channels[output_channel].timer_index;
@@ -162,20 +203,13 @@ static void init_timer_config(uint32_t channel_mask)
continue;
}
- // NOTE: only 1 timer can be used if Bidirectional DShot is enabled
- if (_bidirectional && (timer_index != _bidi_timer_index)) {
- continue;
- }
-
- // NOTE: this is necessary to pass timer_index to DMA callback
timer_configs[timer_index].timer_index = timer_index;
- // Mark timer as enabled
timer_configs[timer_index].enabled = true;
- // Mark channel as enabled
timer_configs[timer_index].enabled_channels[timer_channel_index] = true;
+ // TODO: bdshot per timer not on all timers
// Mark timer as bidirectional
- if (_bidirectional && timer_index == _bidi_timer_index) {
+ if (_bdshot_channel_mask & (1 << output_channel)) {
timer_configs[timer_index].bidirectional = true;
}
}
@@ -197,11 +231,6 @@ static void init_timers_dma_up(void)
continue;
}
- // NOTE: only 1 timer can be used if Bidirectional DShot is enabled
- if (_bidirectional && (timer_index != _bidi_timer_index)) {
- continue;
- }
-
// Attempt to allocate DMA for Burst
timer_configs[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up);
@@ -216,14 +245,16 @@ static void init_timers_dma_up(void)
// Bidirectional DShot will free/allocate DMA stream on every update event. This is required
// in order to reconfigure the DMA stream between Timer Burst and CaptureCompare.
- if (_bidirectional) {
- // Free the allocated DMA channels
- for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
- if (timer_configs[timer_index].dma_handle != NULL) {
- stm32_dmafree(timer_configs[timer_index].dma_handle);
- timer_configs[timer_index].dma_handle = NULL;
- PX4_INFO("Freed DMA UP Timer Index %u", timer_index);
- }
+ for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
+
+ if (!timer_configs[timer_index].bidirectional) {
+ continue;
+ }
+
+ if (timer_configs[timer_index].dma_handle != NULL) {
+ stm32_dmafree(timer_configs[timer_index].dma_handle);
+ timer_configs[timer_index].dma_handle = NULL;
+ PX4_INFO("Freed DMA UP Timer Index %u", timer_index);
}
}
}
@@ -264,33 +295,34 @@ static int32_t init_timer_channels(uint8_t timer_index)
timer_configs[timer_index].initialized_channels[timer_channel_index] = true;
channels_init_mask |= (1 << output_channel);
- if (timer_configs[timer_index].bidirectional) {
- PX4_DEBUG("DShot initialized OutputChannel %u (bidirectional)", output_channel);
-
- } else {
- PX4_DEBUG("DShot initialized OutputChannel %u", output_channel);
- }
+ PX4_DEBUG("%sDShot initialized OutputChannel %u", timer_configs[timer_index].bidirectional ? "B" : "", output_channel);
}
}
return channels_init_mask;
}
-int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot)
+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;
- _bidirectional = enable_bidirectional_dshot;
+ _bdshot_channel_mask = bdshot_channel_mask;
+ _edt_enabled = edt_enable;
- if (_bidirectional) {
- PX4_INFO("Bidirectional DShot enabled, only one timer will be used");
- hrt_callback_perf = perf_alloc(PC_ELAPSED, "dshot: callback perf");
+ // Precompute capture delay: 30us switch time + frame time + 20us margin
+ hrt_abstime frame_us = (16 * 1000000) / _dshot_frequency;
+ _bdshot_capture_delay = 30 + frame_us + 20;
+
+ if (bdshot_channel_mask) {
+ // TODO: show which timers/channels it's enabled on
+ PX4_INFO("BDShot enabled");
+ capture_complete_perf = perf_alloc(PC_ELAPSED, "dshot: capture complete");
+ burst_perf = perf_alloc(PC_INTERVAL, "dshot: burst perf");
+ capture_window_perf = perf_alloc(PC_INTERVAL, "dshot: capture window");
}
- // NOTE: if bidirectional is enabled only 1 timer can be used. This is because Burst mode uses 1 DMA channel per timer
- // and CaptureCompare (for reading ESC eRPM) uses 4 DMA. Even if we only used CaptureCompare on a single timer
- // we would still need 5 DMA; 1 DMA for the second timer burst, and 4 DMA for the first timer CaptureCompare. The only
- // way to support more than 1 timer is to burst/captcomp sequentially for each timer enabled for dshot. The code is
- // structured in a way to allow extending support for this in the future.
+ // NOTE: All timers are triggered in a loop with a small delay.
+ // Each timer needs its own DMA channel, re-used for burst transmit and then capture.
+ // For bidirectional DShot, each timer uses round-robin capture (1 channel per timer per cycle).
// Initialize timer_config data based on enabled channels
init_timer_config(channel_mask);
@@ -306,7 +338,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi
unsigned output_buffer_offset = 0;
- for (unsigned timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
+ for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
if (timer_configs[timer_index].initialized) {
if (io_timers[timer_index].base == 0) { // no more timers configured
break;
@@ -327,89 +359,149 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi
}
}
+ // Mark BDShot channels without capture DMA as processed (so they don't block ready count)
+ // and track which channels support capture
+ for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) {
+ if (bdshot_channel_mask & (1 << output_channel)) {
+ uint8_t timer_index = timer_io_channels[output_channel].timer_index;
+ uint8_t timer_channel = timer_io_channels[output_channel].timer_channel;
+
+ if (timer_channel >= 1 && timer_channel <= 4) {
+ uint8_t timer_channel_index = timer_channel - 1;
+
+ if (io_timers[timer_index].dshot.dma_map_ch[timer_channel_index] != 0) {
+ _bdshot_capture_supported[output_channel] = true;
+
+ } else {
+ // No DMA for capture on this channel - mark as ready so it doesn't block
+ _bdshot_ready_mask |= (1u << output_channel);
+ PX4_WARN("BDShot capture not supported on output %u (no DMA)", output_channel);
+ }
+ }
+ }
+ }
+
return channels_init_mask;
}
-// Kicks off a DMA transmit for each configured timer and the associated channels
+// ESC boot delay - wait for ESCs to initialize before enabling BDShot capture
+static const uint64_t ESC_BOOT_DELAY_US = 3000000;
+
+// Start burst DMA for a single timer
+static void dshot_start_timer_burst(uint8_t timer_index)
+{
+ timer_config_t *timer = &timer_configs[timer_index];
+ io_timer_channel_mode_t mode = timer->bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot;
+
+ io_timer_set_enable(true, mode, io_timer_get_group(timer_index));
+
+ uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps;
+ io_timer_set_dshot_burst_mode(timer_index, _dshot_frequency, channel_count);
+
+ if (timer->bidirectional) {
+ // Deallocate DMA from previous transaction
+ if (timer->dma_handle != NULL) {
+ stm32_dmastop(timer->dma_handle);
+ stm32_dmafree(timer->dma_handle);
+ timer->dma_handle = NULL;
+ }
+
+ // Allocate DMA
+ timer->dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up);
+
+ if (timer->dma_handle == NULL) {
+ PX4_WARN("DMA allocation for timer %u failed", timer_index);
+ _bdshot_cycle_complete[timer_index] = true;
+ return;
+ }
+ }
+
+ // Flush cache so DMA sees the data
+ up_clean_dcache((uintptr_t)dshot_output_buffer[timer_index],
+ (uintptr_t)dshot_output_buffer[timer_index] +
+ DSHOT_OUTPUT_BUFFER_SIZE(channel_count));
+
+ px4_stm32_dmasetup(timer->dma_handle,
+ io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET,
+ (uint32_t)(dshot_output_buffer[timer_index]),
+ channel_count * CHANNEL_OUTPUT_BUFF_SIZE,
+ DSHOT_DMA_SCR);
+
+ // Clean UDE flag before DMA is started
+ io_timer_update_dma_req(timer_index, false);
+
+ // Trigger DMA. For BDShot after boot delay, use callback for capture setup.
+ if (timer->bidirectional && (hrt_absolute_time() > ESC_BOOT_DELAY_US)) {
+ perf_begin(burst_perf);
+ stm32_dmastart(timer->dma_handle, dma_burst_finished_callback, &timer->timer_index, false);
+
+ } else {
+ // Standard DShot or BDShot during boot - no capture needed
+ stm32_dmastart(timer->dma_handle, NULL, NULL, false);
+
+ if (timer->bidirectional) {
+ // During boot, mark BDShot cycle complete immediately
+ _bdshot_cycle_complete[timer_index] = true;
+ }
+ }
+
+ // Enable DMA update request
+ io_timer_update_dma_req(timer_index, true);
+}
+
+// Kicks off DMA transmit for all configured timers simultaneously
void up_dshot_trigger()
{
- // Enable DShot inverted on all channels
- io_timer_set_enable(true, _bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot,
- IO_TIMER_ALL_MODES_CHANNELS);
+ for (int i = 0; i < MAX_IO_TIMERS; i++) {
+ if (!_bdshot_cycle_complete[i]) {
+ PX4_WARN("Cycle not complete, check system jitter!");
+ return;
+ }
+ }
- // For each timer, begin DMA transmit
- for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
- if (timer_configs[timer_index].enabled && timer_configs[timer_index].initialized) {
+ // Mark all BDShot timers as cycle incomplete and start all timers
+ uint8_t bdshot_timers_started = 0;
- uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps;
-
- io_timer_set_dshot_burst_mode(timer_index, _dshot_frequency, channel_count);
-
- if (_bidirectional) {
- // Deallocate DMA from previous transaction
- if (timer_configs[timer_index].dma_handle != NULL) {
- stm32_dmastop(timer_configs[timer_index].dma_handle);
- stm32_dmafree(timer_configs[timer_index].dma_handle);
- timer_configs[timer_index].dma_handle = NULL;
+ for (uint8_t i = 0; i < MAX_IO_TIMERS; i++) {
+ if (timer_configs[i].enabled && timer_configs[i].initialized) {
+ if (timer_configs[i].bidirectional) {
+ // Stagger between bidirectional timer bursts to prevent response frame overlap.
+ // Response frames arrive ~30us after burst, and reconfiguring for capture takes >15us.
+ if (bdshot_timers_started > 0) {
+ px4_udelay(10);
}
- // Allocate DMA
- timer_configs[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up);
-
- if (timer_configs[timer_index].dma_handle == NULL) {
- PX4_WARN("DMA allocation for timer %u failed", timer_index);
- continue;
- }
+ _bdshot_cycle_complete[i] = false;
+ bdshot_timers_started++;
}
- // Flush cache so DMA sees the data
- up_clean_dcache((uintptr_t) dshot_output_buffer[timer_index],
- (uintptr_t) dshot_output_buffer[timer_index] +
- DSHOT_OUTPUT_BUFFER_SIZE(channel_count));
-
- px4_stm32_dmasetup(timer_configs[timer_index].dma_handle,
- io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET,
- (uint32_t)(dshot_output_buffer[timer_index]),
- channel_count * CHANNEL_OUTPUT_BUFF_SIZE,
- DSHOT_DMA_SCR);
-
- // Clean UDE flag before DMA is started
- io_timer_update_dma_req(timer_index, false);
-
- // Trigger DMA (DShot Outputs)
- if (timer_configs[timer_index].bidirectional) {
- stm32_dmastart(timer_configs[timer_index].dma_handle, dma_burst_finished_callback,
- &timer_configs[timer_index].timer_index,
- false);
-
- } else {
- stm32_dmastart(timer_configs[timer_index].dma_handle, NULL, NULL, false);
- }
-
- // Enable DMA update request
- io_timer_update_dma_req(timer_index, true);
+ dshot_start_timer_burst(i);
}
}
}
static void select_next_capture_channel(uint8_t timer_index)
{
- bool found = false;
- int next_index = timer_configs[timer_index].capture_channel_index;
+ int current = timer_configs[timer_index].capture_channel;
- while (!found) {
- next_index++;
+ // Try each of the 4 possible channels, starting from current+1
+ for (int i = 1; i <= 4; i++) {
+ int next = (current + i) % 4;
- if (next_index > 3) {
- next_index = 0;
- }
+ if (timer_configs[timer_index].initialized_channels[next]) {
+ uint8_t output_channel = output_channel_from_timer_channel(timer_index, next);
- if (timer_configs[timer_index].initialized_channels[next_index]) {
- found = true;
+ if (output_channel >= MAX_TIMER_IO_CHANNELS) {
+ continue;
+ }
+
+ // Only capture from channels that support BDShot (have DMA mapping)
+ if (_bdshot_capture_supported[output_channel]) {
+ timer_configs[timer_index].capture_channel = next;
+ return;
+ }
}
}
-
- timer_configs[timer_index].capture_channel_index = next_index;
}
static uint8_t output_channel_from_timer_channel(uint8_t timer_index, uint8_t timer_channel_index)
@@ -425,19 +517,29 @@ static uint8_t output_channel_from_timer_channel(uint8_t timer_index, uint8_t ti
}
}
- // TODO: error handling?
- return 0;
+ return MAX_TIMER_IO_CHANNELS;
}
void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg)
{
uint8_t timer_index = *((uint8_t *)arg);
+ timer_config_t *timer = &timer_configs[timer_index];
+
+ // TODO: revisit logic for DMA failure handling (does it occur?)
+ // if (status != DMA_STATUS_SUCCESS) {
+ // }
+
+ // For standard DShot or BDShot during boot: no capture needed
+ if (!timer->bidirectional || (hrt_absolute_time() <= ESC_BOOT_DELAY_US)) {
+ return;
+ }
+
// Clean DMA UP configuration
- if (timer_configs[timer_index].dma_handle != NULL) {
- stm32_dmastop(timer_configs[timer_index].dma_handle);
- stm32_dmafree(timer_configs[timer_index].dma_handle);
- timer_configs[timer_index].dma_handle = NULL;
+ if (timer->dma_handle != NULL) {
+ stm32_dmastop(timer->dma_handle);
+ stm32_dmafree(timer->dma_handle);
+ timer->dma_handle = NULL;
}
// Disable DMA update request
@@ -447,28 +549,37 @@ void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg)
io_timer_unallocate_timer(timer_index);
// Flush cache so DMA sees the data
- memset(dshot_capture_buffer, 0, sizeof(dshot_capture_buffer));
- up_clean_dcache((uintptr_t) dshot_capture_buffer,
- (uintptr_t) dshot_capture_buffer + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER));
+ memset(dshot_capture_buffer[timer_index], 0, sizeof(dshot_capture_buffer[timer_index]));
+ up_clean_dcache((uintptr_t) dshot_capture_buffer[timer_index],
+ (uintptr_t) dshot_capture_buffer[timer_index] + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER));
- // Unallocate timer channel for currently selected capture_channel
- uint8_t capture_channel = timer_configs[timer_index].capture_channel_index;
- uint8_t output_channel = output_channel_from_timer_channel(timer_index, capture_channel);
+ // Re-initialize the current capture channel back to CaptureDMA
+ uint8_t capture_output_channel = output_channel_from_timer_channel(timer_index, timer_configs[timer_index].capture_channel);
- // Re-initialize output for CaptureDMA for next time
- io_timer_unallocate_channel(output_channel);
- io_timer_channel_init(output_channel, IOTimerChanMode_CaptureDMA, NULL, NULL);
+ if (capture_output_channel < MAX_TIMER_IO_CHANNELS) {
+ io_timer_unallocate_channel(capture_output_channel);
+ io_timer_channel_init(capture_output_channel, IOTimerChanMode_CaptureDMA, NULL, NULL);
+ }
// Select the next capture channel
select_next_capture_channel(timer_index);
// Allocate DMA for currently selected capture_channel
- capture_channel = timer_configs[timer_index].capture_channel_index;
+ uint8_t capture_channel = timer_configs[timer_index].capture_channel;
timer_configs[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_ch[capture_channel]);
- // If DMA handler is valid, start DMA
- if (timer_configs[timer_index].dma_handle == NULL) {
+ // If DMA handler is invalid, skip capture
+ if (timer->dma_handle == NULL) {
PX4_WARN("failed to allocate dma for timer %u channel %u", timer_index, capture_channel);
+
+ // Restore the capture channel back to DshotInverted (was set to CaptureDMA above)
+ if (capture_output_channel < MAX_TIMER_IO_CHANNELS) {
+ io_timer_unallocate_channel(capture_output_channel);
+ io_timer_channel_init(capture_output_channel, IOTimerChanMode_DshotInverted, NULL, NULL);
+ }
+
+ perf_end(burst_perf);
+ _bdshot_cycle_complete[timer_index] = true;
return;
}
@@ -482,7 +593,7 @@ void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg)
// Setup DMA for this channel
px4_stm32_dmasetup(timer_configs[timer_index].dma_handle,
periph_addr,
- (uint32_t) dshot_capture_buffer[capture_channel],
+ (uint32_t) dshot_capture_buffer[timer_index][capture_channel],
CHANNEL_CAPTURE_BUFF_SIZE,
DSHOT_BIDIRECTIONAL_DMA_SCR);
@@ -490,25 +601,26 @@ void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg)
// we use an hrt callback to schedule the processing of the received and DMAd eRPM frames.
stm32_dmastart(timer_configs[timer_index].dma_handle, NULL, NULL, false);
- // Enable CaptureDMA and on all configured channels
- io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, IO_TIMER_ALL_MODES_CHANNELS);
+ // Enable CaptureDMA on this timer's channels only
+ io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, io_timer_get_group(timer_index));
- // 30us to switch regardless of DShot frequency + eRPM frame time + 10us for good measure
- hrt_abstime frame_us = (16 * 1000000) / _dshot_frequency; // 16 bits * us_per_s / bits_per_s
- hrt_abstime delay = 30 + frame_us + 10;
- hrt_call_after(&_cc_call, delay, capture_complete_callback, arg);
+ perf_end(burst_perf);
+ perf_begin(capture_window_perf);
+
+ hrt_call_after(&_cc_calls[timer_index], _bdshot_capture_delay, capture_complete_callback, arg);
}
static void capture_complete_callback(void *arg)
{
- perf_begin(hrt_callback_perf);
+ perf_end(capture_window_perf);
+ perf_begin(capture_complete_perf);
uint8_t timer_index = *((uint8_t *)arg);
// Unallocate the timer as CaptureDMA
io_timer_unallocate_timer(timer_index);
- uint8_t capture_channel = timer_configs[timer_index].capture_channel_index;
+ uint8_t capture_channel = timer_configs[timer_index].capture_channel;
// Disable capture DMA
io_timer_capture_dma_req(timer_index, capture_channel, false);
@@ -524,7 +636,6 @@ static void capture_complete_callback(void *arg)
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
if (is_this_timer && channel_initialized) {
-
io_timer_unallocate_channel(output_channel);
// Initialize back to DShotInverted to bring IO back to the expected idle state
io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL);
@@ -532,49 +643,285 @@ static void capture_complete_callback(void *arg)
}
// Invalidate the dcache to ensure most recent data is available
- up_invalidate_dcache((uintptr_t) dshot_capture_buffer,
- (uintptr_t) dshot_capture_buffer + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER));
+ up_invalidate_dcache((uintptr_t) dshot_capture_buffer[timer_index],
+ (uintptr_t) dshot_capture_buffer[timer_index] + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER));
// Process eRPM frames from all channels on this timer
process_capture_results(timer_index, capture_channel);
- // Enable all channels configured as DShotInverted
- io_timer_set_enable(true, IOTimerChanMode_DshotInverted, IO_TIMER_ALL_MODES_CHANNELS);
+ // Enable this timer's channels as DShotInverted
+ io_timer_set_enable(true, IOTimerChanMode_DshotInverted, io_timer_get_group(timer_index));
- perf_end(hrt_callback_perf);
+ perf_end(capture_complete_perf);
+
+ _bdshot_cycle_complete[timer_index] = true;
}
void process_capture_results(uint8_t timer_index, uint8_t channel_index)
{
- const unsigned period = calculate_period(timer_index, channel_index);
+ uint8_t output_channel = output_channel_from_timer_channel(timer_index, channel_index);
+
+ if (output_channel >= MAX_TIMER_IO_CHANNELS) {
+ return;
+ }
+
+ uint32_t value = convert_edge_intervals_to_bitstream(timer_index, channel_index);
+
+ // 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;
+ }
+
+ _bdshot_ready_mask |= (1u << output_channel);
+ return;
+ }
+
+ ++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 telem 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:
+ // TODO: Handle these?
+ break;
+
+ default:
+ PX4_DEBUG("unknown EDT type %d", packet.type);
+ break;
+ }
+
+ _bdshot_ready_mask |= (1u << output_channel);
+}
+
+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;
+
+ // Simple exponential moving average with fixed alpha
+ // Alpha = 0.125 (1/8) works well across all rates
+ float rate_hz = instant_rate * 0.125f + last_rate_hz * 0.875f;
+
+ return rate_hz;
+}
+
+// Converts captured edge timestamps into a raw bit stream.
+// Measures the time intervals between signal edges to determine how many consecutive
+// 1s or 0s to shift in, alternating the bit value with each edge transition.
+// Uses adaptive per-channel timing calibration to handle ESC oscillator variation.
+// Returns a 20 bit raw value that still needs RLL and GCR decoding.
+uint32_t convert_edge_intervals_to_bitstream(uint8_t timer_index, uint8_t channel_index)
+{
+ // First pass: collect all intervals
+ uint32_t intervals[CHANNEL_CAPTURE_BUFF_SIZE] = {};
+ unsigned interval_count = 0;
+
+ // We can ignore the very first data point as it's the pulse before it starts.
+ unsigned previous = dshot_capture_buffer[timer_index][channel_index][1];
+
+ for (unsigned i = 2; i < CHANNEL_CAPTURE_BUFF_SIZE; ++i) {
+ if (dshot_capture_buffer[timer_index][channel_index][i] == 0) {
+ // Once we get zeros we're through
+ break;
+ }
+
+ uint32_t interval = dshot_capture_buffer[timer_index][channel_index][i] - previous;
+
+ // Filter out noise: reject intervals <10 or >100 ticks
+ if (interval >= 10 && interval <= 100) {
+ intervals[interval_count++] = interval;
+ }
+
+ previous = dshot_capture_buffer[timer_index][channel_index][i];
+ }
+
+ if (interval_count == 0) {
+ return 0;
+ }
+
+ // Calibrate base interval using minimum interval (always 1 bit in GCR)
+ // The shortest valid interval in the frame represents a single bit period
+ uint32_t min_interval = intervals[0];
+
+ for (unsigned i = 1; i < interval_count; i++) {
+ if (intervals[i] < min_interval) {
+ min_interval = intervals[i];
+ }
+ }
uint8_t output_channel = output_channel_from_timer_channel(timer_index, channel_index);
-
- if (period == 0) {
- // If the parsing failed, set the eRPM to 0
- _erpms[output_channel] = 0;
-
- } else if (period == 65408) {
- // Special case for zero motion (e.g., stationary motor)
- _erpms[output_channel] = 0;
-
- } else {
- // Convert the period to eRPM
- _erpms[output_channel] = (1000000 * 60 / 100 + period / 2) / period;
+ if (output_channel >= MAX_TIMER_IO_CHANNELS) {
+ return 0;
}
- // We set it ready anyway, not to hold up other channels when used in round robin.
- _erpms_ready[output_channel] = true;
+ // Update base interval with low-pass filter (alpha = 1/8) using x8 fixed-point
+ // base_x8 = base_x8 * 7/8 + min_interval (which is min * 8/8 in x8 representation)
+ _base_interval_x8[output_channel] = _base_interval_x8[output_channel] - (_base_interval_x8[output_channel] >> 3) + min_interval;
+
+ // Second pass: decode bits using adaptive threshold (integer math)
+ uint32_t value = 0;
+ uint32_t high = 1;
+ unsigned shifted = 0;
+
+ uint32_t base_x8 = _base_interval_x8[output_channel];
+
+ for (unsigned i = 0; i < interval_count; i++) {
+ // Convert interval to bit count: bits = interval/base + 0.5, using x8 fixed-point rounding
+ uint32_t bits = (intervals[i] * 8 + base_x8 / 2) / base_x8;
+
+ // Clamp to valid range (1-4 bits typical in GCR encoding)
+ if (bits < 1) {
+ bits = 1;
+
+ } else if (bits > 4) {
+ bits = 4;
+ }
+
+ // Shift in the bits
+ for (unsigned bit = 0; bit < bits; ++bit) {
+ value = (value << 1) | high;
+ ++shifted;
+ }
+
+ high = !high;
+ }
+
+ // Flexible frame validation: accept 18-24 bits instead of forcing exactly 21
+ // This handles ESC timing variation and incomplete frames
+ if (shifted < 18 || shifted > 24) {
+ // Frame too short or too long - likely corrupted
+ return 0;
+ }
+
+ // Pad to 21 bits for GCR decoder (which expects exactly 21 bits)
+ if (shifted < 21) {
+ value <<= (21 - shifted);
+
+ } else if (shifted > 21) {
+ // Trim excess bits (shouldn't happen often with proper decoding)
+ value >>= (shifted - 21);
+ }
+
+ return value;
}
-/**
-* bits 1-11 - throttle value (0-47 are reserved for commands, 48-2047 give 2000 steps of throttle resolution)
-* bit 12 - dshot telemetry enable/disable
-* bits 13-16 - XOR checksum
-**/
-void dshot_motor_data_set(unsigned channel, uint16_t data, bool telemetry)
+void decode_dshot_telemetry(uint32_t payload, struct BDShotTelemetry *packet)
{
+ // Extended DShot Telemetry
+ bool edt_enabled = _edt_enabled;
+ uint32_t mantissa = payload & 0x01FF;
+ bool is_telemetry = !(mantissa & 0x0100); // if the msb of the mantissa is zero, then this is an extended telemetry packet
+
+ if (edt_enabled && is_telemetry) {
+ packet->type = (payload & 0x0F00) >> 8;
+ packet->value = payload & 0x00FF; // extended telemetry value is 8 bits wide
+
+ } else {
+ // otherwise it's an eRPM frame
+ uint8_t exponent = ((payload >> 9) & 0x7); // 3 bit: exponent
+ uint16_t period = (payload & 0x1FF); // 9 bit: period base
+ period = period << exponent; // Period in usec
+
+ packet->type = DSHOT_EDT_ERPM;
+
+ if (period == 65408 || period == 0) {
+ // 65408 is a special case for zero motion (e.g., stationary motor)
+ packet->value = 0;
+
+ } else {
+ packet->value = (1000000 * 60 / 100 + period / 2) / period;
+ }
+ }
+}
+
+// bits 1-11 - throttle value (0-47 are reserved for commands, 48-2047 give 2000 steps of throttle resolution)
+// bit 12 - dshot telemetry enable/disable
+// bits 13-16 - XOR checksum
+void dshot_motor_data_set(uint8_t channel, uint16_t data, bool telemetry)
+{
+ if (channel >= MAX_TIMER_IO_CHANNELS) {
+ return;
+ }
+
uint8_t timer_index = timer_io_channels[channel].timer_index;
uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1;
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
@@ -591,7 +938,7 @@ void dshot_motor_data_set(unsigned channel, uint16_t data, bool telemetry)
uint16_t csum_data = packet;
- /* XOR checksum calculation */
+ // XOR checksum calculation
csum_data >>= NIBBLES_SIZE;
for (uint8_t i = 0; i < DSHOT_NUMBER_OF_NIBBLES; i++) {
@@ -599,14 +946,13 @@ void dshot_motor_data_set(unsigned channel, uint16_t data, bool telemetry)
csum_data >>= NIBBLES_SIZE;
}
- if (_bidirectional) {
+ if (_bdshot_channel_mask & (1 << channel)) {
packet |= ((~checksum) & 0x0F);
} else {
packet |= ((checksum) & 0x0F);
}
-
const io_timers_channel_mapping_element_t *mapping = &io_timers_channel_mapping.element[timer_index];
uint8_t num_motors = mapping->channel_count_including_gaps;
uint8_t timer_channel = timer_io_channels[channel].timer_channel - mapping->lowest_timer_channel;
@@ -620,209 +966,154 @@ void dshot_motor_data_set(unsigned channel, uint16_t data, bool telemetry)
int up_dshot_arm(bool armed)
{
- return io_timer_set_enable(armed, _bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot,
- IO_TIMER_ALL_MODES_CHANNELS);
-}
+ int ret = PX4_OK;
-int up_bdshot_num_erpm_ready(void)
-{
- int num_ready = 0;
-
- for (unsigned i = 0; i < MAX_TIMER_IO_CHANNELS; ++i) {
- if (_erpms_ready[i]) {
- ++num_ready;
- }
+ for (int timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
+ io_timer_channel_mode_t mode = timer_configs[timer_index].bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot;
+ ret |= io_timer_set_enable(armed, mode, io_timer_get_group(timer_index));
}
- return num_ready;
+ return ret;
}
-int up_bdshot_get_erpm(uint8_t output_channel, int *erpm)
+uint16_t up_bdshot_get_ready_mask(void)
{
- uint8_t timer_index = timer_io_channels[output_channel].timer_index;
- uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1;
- bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
+ return _bdshot_ready_mask;
+}
- if (channel_initialized) {
- *erpm = _erpms[output_channel];
- _erpms_ready[output_channel] = false;
- return PX4_OK;
+int up_bdshot_num_errors(uint8_t channel)
+{
+ if (channel >= MAX_TIMER_IO_CHANNELS) {
+ return 0;
}
- // this channel is not configured for dshot
- return PX4_ERROR;
+ return read_fail_crc[channel];
}
-int up_bdshot_channel_status(uint8_t channel)
+int up_bdshot_get_erpm(uint8_t channel, int *erpm)
{
+ int status = PX4_ERROR;
+
+ if (channel >= MAX_TIMER_IO_CHANNELS) {
+ return status;
+ }
+
uint8_t timer_index = timer_io_channels[channel].timer_index;
uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1;
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
- // TODO: track that each channel is communicating using the decode stats
- if (channel_initialized) {
- return 1;
+ if (channel_initialized && _erpms[channel].ready) {
+ *erpm = _erpms[channel].erpm;
+ status = PX4_OK;
}
- return 0;
+ // Mark sample read — only for channels with capture support
+ if (_bdshot_capture_supported[channel]) {
+ _bdshot_ready_mask &= ~(1u << channel);
+ }
+
+ return status;
+}
+
+int up_bdshot_get_extended_telemetry(uint8_t channel, int type, uint8_t *value)
+{
+ int result = PX4_ERROR;
+
+ if (channel >= MAX_TIMER_IO_CHANNELS) {
+ return result;
+ }
+
+ 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 driver stats:");
- if (_bidirectional) {
- PX4_INFO("Bidirectional DShot enabled");
- }
+ if (_bdshot_channel_mask) {
+ PX4_INFO("BDShot channel mask: 0x%04x", (unsigned)_bdshot_channel_mask);
- uint8_t timer_index = _bidi_timer_index;
+ // Always show read counters for BDShot
+ 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]);
- for (uint8_t timer_channel_index = 0; timer_channel_index < MAX_NUM_CHANNELS_PER_TIMER; timer_channel_index++) {
- bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
-
- if (channel_initialized) {
- PX4_INFO("Timer %u, Channel %u: read %lu, failed nibble %lu, failed CRC %lu, invalid/zero %lu",
- timer_index, timer_channel_index,
- read_ok[timer_channel_index],
- read_fail_nibble[timer_channel_index],
- read_fail_crc[timer_channel_index],
- read_fail_zero[timer_channel_index]);
+ } else {
+ PX4_INFO("Ch%u: no DMA for capture", i);
+ }
+ }
}
}
-}
-uint8_t nibbles_from_mapped(uint8_t mapped)
-{
- switch (mapped) {
- case 0x19:
- return 0x00;
+ if (_edt_enabled) {
+ PX4_INFO("BDShot EDT");
- case 0x1B:
- return 0x01;
-
- case 0x12:
- return 0x02;
-
- case 0x13:
- return 0x03;
-
- case 0x1D:
- return 0x04;
-
- case 0x15:
- return 0x05;
-
- case 0x16:
- return 0x06;
-
- case 0x17:
- return 0x07;
-
- case 0x1a:
- return 0x08;
-
- case 0x09:
- return 0x09;
-
- case 0x0A:
- return 0x0A;
-
- case 0x0B:
- return 0x0B;
-
- case 0x1E:
- return 0x0C;
-
- case 0x0D:
- return 0x0D;
-
- case 0x0E:
- return 0x0E;
-
- case 0x0F:
- return 0x0F;
-
- default:
- // Unknown mapped
- return 0xFF;
- }
-}
-
-unsigned calculate_period(uint8_t timer_index, uint8_t channel_index)
-{
- uint32_t value = 0;
- uint32_t high = 1; // We start off with high
- unsigned shifted = 0;
-
- // We can ignore the very first data point as it's the pulse before it starts.
- unsigned previous = dshot_capture_buffer[channel_index][1];
-
- // Loop through the capture buffer for the specified channel
- for (unsigned i = 2; i < CHANNEL_CAPTURE_BUFF_SIZE; ++i) {
-
- if (dshot_capture_buffer[channel_index][i] == 0) {
- // Once we get zeros we're through
- break;
+ 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);
+ }
}
-
- // This seemss to work with dshot 150, 300, 600, 1200
- // The values were found by trial and error to get the quantization just right.
- const uint32_t bits = (dshot_capture_buffer[channel_index][i] - previous + 5) / 20;
-
- // Convert GCR encoded pulse train into value
- for (unsigned bit = 0; bit < bits; ++bit) {
- value = (value << 1) | high;
- ++shifted;
- }
-
- // The next edge toggles.
- high = !high;
-
- previous = dshot_capture_buffer[channel_index][i];
}
-
- if (shifted == 0) {
- // no data yet, or this time
- ++read_fail_zero[channel_index];
- return 0;
- }
-
- // We need to make sure we shifted 21 times. We might have missed some low "pulses" at the very end.
- value <<= (21 - shifted);
-
- // From GCR to eRPM according to:
- // https://brushlesswhoop.com/dshot-and-bidirectional-dshot/#erpm-transmission
- unsigned gcr = (value ^ (value >> 1));
-
- uint32_t data = 0;
-
- // 20bits -> 5 mapped -> 4 nibbles
- for (unsigned i = 0; i < 4; ++i) {
- uint32_t nibble = nibbles_from_mapped(gcr & 0x1F) << (4 * i);
-
- if (nibble == 0xFF) {
- ++read_fail_nibble[channel_index];;
- return 0;
- }
-
- data |= nibble;
- gcr >>= 5;
- }
-
- unsigned shift = (data & 0xE000) >> 13;
- unsigned period = ((data & 0x1FF0) >> 4) << shift;
- unsigned crc = data & 0xF;
-
- unsigned payload = (data & 0xFFF0) >> 4;
- unsigned calculated_crc = (~(payload ^ (payload >> 4) ^ (payload >> 8))) & 0x0F;
-
- if (crc != calculated_crc) {
- ++read_fail_crc[channel_index];;
- return 0;
- }
-
- ++read_ok[channel_index];;
- return period;
}
#endif
diff --git a/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp b/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp
index ff5e7d267d..9c88c20df3 100644
--- a/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp
+++ b/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp
@@ -83,7 +83,6 @@ void VertiqTelemetryManager::StartPublishing(uORB::Publication *es
_esc_status.esc[i].esc_address = 0;
_esc_status.esc[i].esc_rpm = 0;
_esc_status.esc[i].esc_state = 0;
- _esc_status.esc[i].esc_cmdcount = 0;
_esc_status.esc[i].esc_voltage = 0;
_esc_status.esc[i].esc_current = 0;
_esc_status.esc[i].esc_temperature = 0;
@@ -124,7 +123,6 @@ uint16_t VertiqTelemetryManager::UpdateTelemetry()
_esc_status.esc[_current_module_id_target_index].esc_temperature = telem_response.mcu_temp *
0.01; //"If you ask other escs for their temp, they're giving you the micro temp, so go with that"
_esc_status.esc[_current_module_id_target_index].esc_state = 0; //not implemented
- _esc_status.esc[_current_module_id_target_index].esc_cmdcount = 0; //not implemented
_esc_status.esc[_current_module_id_target_index].failures = 0; //not implemented
//Update the overall _esc_status timestamp and our counter
diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.cpp b/src/drivers/actuators/voxl_esc/voxl_esc.cpp
index 002cba3b2c..65e1fd562d 100644
--- a/src/drivers/actuators/voxl_esc/voxl_esc.cpp
+++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp
@@ -66,7 +66,6 @@ VoxlEsc::VoxlEsc() :
_esc_status.esc[i].esc_address = 0;
_esc_status.esc[i].esc_rpm = 0;
_esc_status.esc[i].esc_state = 0;
- _esc_status.esc[i].esc_cmdcount = 0;
_esc_status.esc[i].esc_voltage = 0;
_esc_status.esc[i].esc_current = 0;
_esc_status.esc[i].esc_temperature = 0;
@@ -526,7 +525,6 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
_esc_status.esc[id].esc_rpm = fb.rpm;
_esc_status.esc[id].esc_power = fb.power;
_esc_status.esc[id].esc_state = fb.id_state & 0x0F;
- _esc_status.esc[id].esc_cmdcount = fb.cmd_counter;
_esc_status.esc[id].esc_voltage = _esc_chans[id].voltage;
_esc_status.esc[id].esc_current = _esc_chans[id].current;
_esc_status.esc[id].failures = 0; //not implemented
@@ -563,11 +561,11 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
//print ESC status just for debugging
/*
- PX4_INFO("[%lld] ID=%d, ADDR %d, STATE=%d, RPM=%5d, PWR=%3d%%, V=%.2fdV, I=%.2fA, T=%+3dC, CNT %d, FAIL %d",
+ PX4_INFO("[%lld] ID=%d, ADDR %d, STATE=%d, RPM=%5d, PWR=%3d%%, V=%.2fdV, I=%.2fA, T=%+3dC, FAIL %d",
_esc_status.esc[id].timestamp, id, _esc_status.esc[id].esc_address,
_esc_status.esc[id].esc_state, _esc_status.esc[id].esc_rpm, _esc_status.esc[id].esc_power,
(double)_esc_status.esc[id].esc_voltage, (double)_esc_status.esc[id].esc_current, _esc_status.esc[id].esc_temperature,
- _esc_status.esc[id].esc_cmdcount, _esc_status.esc[id].failures);
+ _esc_status.esc[id].failures);
*/
}
}
diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h
index 792d6076fc..797f70af93 100644
--- a/src/drivers/drv_dshot.h
+++ b/src/drivers/drv_dshot.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ * 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
@@ -31,11 +31,6 @@
*
****************************************************************************/
-/**
- * @file drv_dshot.h
- *
- */
-
#pragma once
#include
@@ -48,39 +43,40 @@
__BEGIN_DECLS
-typedef enum {
- DShot_cmd_motor_stop = 0,
- DShot_cmd_beacon1,
- DShot_cmd_beacon2,
- DShot_cmd_beacon3,
- DShot_cmd_beacon4,
- DShot_cmd_beacon5,
- DShot_cmd_esc_info, // V2 includes settings
- DShot_cmd_spin_direction_1,
- DShot_cmd_spin_direction_2,
- DShot_cmd_3d_mode_off,
- DShot_cmd_3d_mode_on,
- DShot_cmd_settings_request, // Currently not implemented
- DShot_cmd_save_settings,
- DShot_cmd_spin_direction_normal = 20,
- DShot_cmd_spin_direction_reversed = 21,
- DShot_cmd_led0_on, // BLHeli32 only
- DShot_cmd_led1_on, // BLHeli32 only
- DShot_cmd_led2_on, // BLHeli32 only
- DShot_cmd_led3_on, // BLHeli32 only
- DShot_cmd_led0_off, // BLHeli32 only
- DShot_cmd_led1_off, // BLHeli32 only
- DShot_cmd_led2_off, // BLHeli32 only
- DShot_cmd_led4_off, // BLHeli32 only
- DShot_cmd_audio_stream_mode_on_off = 30, // KISS audio Stream mode on/off
- DShot_cmd_silent_mode_on_off = 31, // KISS silent Mode on/off
- DShot_cmd_signal_line_telemetry_disable = 32,
- DShot_cmd_signal_line_continuous_erpm_telemetry = 33,
- DShot_cmd_MAX = 47, // >47 are throttle values
- DShot_cmd_MIN_throttle = 48,
- DShot_cmd_MAX_throttle = 2047
-} dshot_command_t;
+// https://brushlesswhoop.com/dshot-and-bidirectional-dshot/#special-commands
+enum {
+ DSHOT_CMD_MOTOR_STOP = 0,
+ DSHOT_CMD_BEEP1 = 1,
+ DSHOT_CMD_ESC_INFO = 6,
+ DSHOT_CMD_SPIN_DIRECTION_1 = 7,
+ DSHOT_CMD_SPIN_DIRECTION_2 = 8,
+ DSHOT_CMD_3D_MODE_OFF = 9,
+ DSHOT_CMD_3D_MODE_ON = 10,
+ DSHOT_CMD_SAVE_SETTINGS = 12,
+ DSHOT_EXTENDED_TELEMETRY_ENABLE = 13,
+ DSHOT_CMD_ENTER_PROGRAMMING_MODE = 36,
+ DSHOT_CMD_EXIT_PROGRAMMING_MODE = 37,
+ DSHOT_CMD_MAX = 47, // >47 are throttle values
+ DSHOT_CMD_MIN_THROTTLE = 48,
+ DSHOT_CMD_MAX_THROTTLE = 2047
+};
+// Extended DShot Telemetry
+enum {
+ DSHOT_EDT_ERPM = 0x00,
+ DSHOT_EDT_TEMPERATURE = 0x02, // C
+ DSHOT_EDT_VOLTAGE = 0x04, // 0.25V per step
+ DSHOT_EDT_CURRENT = 0x06, // A
+ DSHOT_EDT_DEBUG1 = 0x08,
+ DSHOT_EDT_DEBUG2 = 0x0A,
+ DSHOT_EDT_DEBUG3 = 0x0C,
+ DSHOT_EDT_STATE_EVENT = 0x0E,
+};
+
+struct BDShotTelemetry {
+ int type;
+ int32_t value;
+};
/**
* Intialise the Dshot outputs using the specified configuration.
@@ -91,12 +87,12 @@ typedef enum {
* @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, or DSHOT600
* @return <0 on error, the initialized channels mask.
*/
-__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot);
+__EXPORT extern int up_dshot_init(uint32_t channel_mask, uint32_t bdshot_channel_mask, unsigned dshot_pwm_freq, bool edt_enable);
/**
* Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method)
*/
-__EXPORT extern void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry);
+__EXPORT extern void dshot_motor_data_set(uint8_t channel, uint16_t throttle, bool telemetry);
/**
* Set the current dshot throttle value for a channel (motor).
@@ -105,9 +101,9 @@ __EXPORT extern void dshot_motor_data_set(unsigned channel, uint16_t throttle, b
* @param throttle The output dshot throttle value in [0 = DSHOT_DISARM_VALUE, 1 = DSHOT_MIN_THROTTLE, 1999 = DSHOT_MAX_THROTTLE].
* @param telemetry If true, request telemetry from that motor
*/
-static inline void up_dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry)
+static inline void up_dshot_motor_data_set(uint8_t channel, uint16_t throttle, bool telemetry)
{
- dshot_motor_data_set(channel, throttle + DShot_cmd_MIN_throttle, telemetry);
+ dshot_motor_data_set(channel, throttle + DSHOT_CMD_MIN_THROTTLE, telemetry);
}
/**
@@ -142,17 +138,18 @@ __EXPORT extern int up_dshot_arm(bool armed);
*/
__EXPORT extern void up_bdshot_status(void);
+/**
+ * Get bitmask of channels that have processed BDShot data ready to read.
+ * Each bit corresponds to an output channel index.
+ */
+__EXPORT extern uint16_t up_bdshot_get_ready_mask(void);
/**
- * Get how many bidirectional erpm channels are ready
- *
- * When we get the erpm round-robin style, we need to get
- * and publish the erpms less often.
- *
- * @return <0 on error, OK on succes
+ * Get the total number of errors for a channel
+ * @param channel Dshot channel
+ * @return The total number of recorded errors
*/
-__EXPORT extern int up_bdshot_num_erpm_ready(void);
-
+__EXPORT extern int up_bdshot_num_errors(uint8_t channel);
/**
* Get bidrectional dshot erpm for a channel
@@ -162,6 +159,14 @@ __EXPORT extern int up_bdshot_num_erpm_ready(void);
*/
__EXPORT extern int up_bdshot_get_erpm(uint8_t channel, int *erpm);
+/**
+ * Get bidrectional dshot extended telemetry for a channel
+ * @param channel Dshot channel
+ * @param type The type of telemetry value to get
+ * @param value pointer to write the telemetry value
+ * @return <0 on error, OK on succes
+ */
+__EXPORT extern int up_bdshot_get_extended_telemetry(uint8_t channel, int type, uint8_t *value);
/**
* Get bidrectional dshot status for a channel
@@ -169,7 +174,13 @@ __EXPORT extern int up_bdshot_get_erpm(uint8_t channel, int *erpm);
* @param erpm pointer to write the erpm value
* @return <0 on error / not supported, 0 on offline, 1 on online
*/
-__EXPORT extern int up_bdshot_channel_status(uint8_t channel);
+__EXPORT extern int up_bdshot_channel_online(uint8_t channel);
+/**
+ * Check if bidrectional dshot capture is supported for a channel
+ * @param channel Dshot channel
+ * @return 0 if not supported (no DMA), 1 if supported
+ */
+__EXPORT extern int up_bdshot_channel_capture_supported(uint8_t channel);
__END_DECLS
diff --git a/src/drivers/dshot/CMakeLists.txt b/src/drivers/dshot/CMakeLists.txt
index 82f7c3db51..58f2894f88 100644
--- a/src/drivers/dshot/CMakeLists.txt
+++ b/src/drivers/dshot/CMakeLists.txt
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+# Copyright (c) 2019-2026 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
@@ -42,9 +42,11 @@ px4_add_module(
MAIN dshot
COMPILE_FLAGS
-DPARAM_PREFIX="${PARAM_PREFIX}"
+ # -DDEBUG_BUILD
SRCS
DShot.cpp
DShotTelemetry.cpp
+ esc/AM32Settings.cpp
DEPENDS
arch_io_pins
arch_dshot
diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp
index 64bd2c44b7..100f5cdb5d 100644
--- a/src/drivers/dshot/DShot.cpp
+++ b/src/drivers/dshot/DShot.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2026 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
@@ -32,14 +32,12 @@
****************************************************************************/
#include "DShot.h"
-
#include
-
#include
ModuleBase::Descriptor DShot::desc{task_spawn, custom_command, print_usage};
-char DShot::_telemetry_device[] {};
+char DShot::_serial_port_path[20] {};
bool DShot::_telemetry_swap_rxtx{false};
px4::atomic_bool DShot::_request_telemetry_init{false};
@@ -52,6 +50,9 @@ DShot::DShot() :
// Avoid using the PWM failsafe params
_mixing_output.setAllFailsafeValues(UINT16_MAX);
+
+ // Advertise early to ensure we beat uavcan
+ _esc_status_pub.advertise();
}
DShot::~DShot()
@@ -60,370 +61,99 @@ DShot::~DShot()
up_dshot_arm(false);
perf_free(_cycle_perf);
- perf_free(_bdshot_rpm_perf);
- perf_free(_dshot_telem_perf);
-
- delete _telemetry;
+ perf_free(_bdshot_recv_perf);
+ perf_free(_bdshot_error_perf);
+ perf_free(_serial_telem_success_perf);
+ perf_free(_serial_telem_error_perf);
+ perf_free(_serial_telem_timeout_perf);
+ perf_free(_serial_telem_allsampled_perf);
}
int DShot::init()
{
- _output_mask = (1u << _num_outputs) - 1;
-
- // Getting initial parameter values
update_params();
- ScheduleNow();
-
- return OK;
-}
-
-int DShot::task_spawn(int argc, char *argv[])
-{
- DShot *instance = new DShot();
-
- if (instance) {
- desc.object.store(instance);
- desc.task_id = task_id_is_work_queue;
-
- if (instance->init() == PX4_OK) {
- return PX4_OK;
- }
-
- } else {
- PX4_ERR("alloc failed");
+ if (initialize_dshot()) {
+ ScheduleNow();
+ return PX4_OK;
}
- delete instance;
- desc.object.store(nullptr);
- desc.task_id = -1;
-
return PX4_ERROR;
}
-void DShot::enable_dshot_outputs(const bool enabled)
+void DShot::Run()
{
- if (enabled && !_outputs_initialized) {
- unsigned int dshot_frequency = 0;
- uint32_t dshot_frequency_param = 0;
-
- for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
- uint32_t channels = io_timer_get_group(timer);
-
- if (channels == 0) {
- continue;
- }
-
- char param_name[17];
- snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
-
- int32_t tim_config = 0;
- param_t handle = param_find(param_name);
- param_get(handle, &tim_config);
- unsigned int dshot_frequency_request = 0;
-
- if (tim_config == -5) {
- dshot_frequency_request = DSHOT150;
-
- } else if (tim_config == -4) {
- dshot_frequency_request = DSHOT300;
-
- } else if (tim_config == -3) {
- dshot_frequency_request = DSHOT600;
-
- } else {
- _output_mask &= ~channels; // don't use for dshot
- }
-
- if (dshot_frequency_request != 0) {
- if (dshot_frequency != 0 && dshot_frequency != dshot_frequency_request) {
- PX4_WARN("Only supporting a single frequency, adjusting param %s", param_name);
- param_set_no_notification(handle, &dshot_frequency_param);
-
- } else {
- dshot_frequency = dshot_frequency_request;
- dshot_frequency_param = tim_config;
- }
- }
- }
-
- _bidirectional_dshot_enabled = _param_bidirectional_enable.get();
-
- int ret = up_dshot_init(_output_mask, dshot_frequency, _bidirectional_dshot_enabled);
-
- if (ret < 0) {
- PX4_ERR("up_dshot_init failed (%i)", ret);
- return;
- }
-
- _output_mask = ret;
-
- // disable unused functions
- for (unsigned i = 0; i < _num_outputs; ++i) {
- if (((1 << i) & _output_mask) == 0) {
- _mixing_output.disableFunction(i);
-
- }
- }
-
- if (_output_mask == 0) {
- // exit the module if no outputs used
- request_stop();
- return;
- }
-
- _outputs_initialized = true;
+ if (should_exit()) {
+ ScheduleClear();
+ _mixing_output.unregister();
+ exit_and_cleanup(desc);
+ return;
}
- if (_outputs_initialized) {
- up_dshot_arm(enabled);
- _outputs_on = enabled;
+ perf_begin(_cycle_perf);
+
+ _mixing_output.update();
+
+ bool serial_updated = process_serial_telemetry();
+ bool bdshot_updated = process_bdshot_telemetry();
+
+ if (serial_updated || bdshot_updated) {
+ _esc_status.timestamp = hrt_absolute_time();
+ _esc_status.esc_count = _motor_count;
+ _esc_status.counter++;
+ _esc_status_pub.publish(_esc_status);
}
+
+ if (_parameter_update_sub.updated()) {
+ update_params();
+ }
+
+ // Telemetry init hook
+ if (_request_telemetry_init.load()) {
+ init_telemetry(_serial_port_path, _telemetry_swap_rxtx);
+ _request_telemetry_init.store(false);
+ }
+
+ handle_vehicle_commands();
+
+ // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
+ _mixing_output.updateSubscriptions(true);
+
+ perf_end(_cycle_perf);
}
-void DShot::update_num_motors()
+bool DShot::updateOutputs(float *outputs, unsigned num_outputs, unsigned num_control_groups_updated)
{
- int motor_count = 0;
-
- for (unsigned i = 0; i < _num_outputs; ++i) {
- if (_mixing_output.isFunctionSet(i)) {
- _actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i);
- ++motor_count;
- }
- }
-
- _num_motors = motor_count;
-}
-
-void DShot::init_telemetry(const char *device, bool swap_rxtx)
-{
- if (!_telemetry) {
- _telemetry = new DShotTelemetry{};
-
- if (!_telemetry) {
- PX4_ERR("alloc failed");
- return;
- }
- }
-
- if (device != NULL) {
- int ret = _telemetry->init(device, swap_rxtx);
-
- if (ret != 0) {
- PX4_ERR("telemetry init failed (%i)", ret);
- }
- }
-
- update_num_motors();
-}
-
-int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data, bool ignore_rpm)
-{
- int ret = 0;
- // fill in new motor data
- esc_status_s &esc_status = esc_status_pub.get();
-
- if (telemetry_index < esc_status_s::CONNECTED_ESC_MAX) {
- esc_status.esc_online_flags |= 1 << telemetry_index;
-
- esc_status.esc[telemetry_index].actuator_function = _actuator_functions[telemetry_index];
-
- if (!ignore_rpm) {
- // If we also have bidirectional dshot, we use rpm and timestamps from there.
- esc_status.esc[telemetry_index].timestamp = data.time;
- esc_status.esc[telemetry_index].esc_rpm = (static_cast(data.erpm) * 100) /
- (_param_mot_pole_count.get() / 2);
- }
-
- esc_status.esc[telemetry_index].esc_voltage = static_cast(data.voltage) * 0.01f;
- esc_status.esc[telemetry_index].esc_current = static_cast(data.current) * 0.01f;
- esc_status.esc[telemetry_index].esc_temperature = static_cast(data.temperature);
- // TODO: accumulate consumption and use for battery estimation
- }
-
- // publish when motor index wraps (which is robust against motor timeouts)
- if (telemetry_index <= _last_telemetry_index) {
- esc_status.timestamp = hrt_absolute_time();
- esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
- esc_status.esc_count = _num_motors;
- ++esc_status.counter;
-
- ret = 1; // Indicate we wrapped, so we publish data
- }
-
- _last_telemetry_index = telemetry_index;
-
- perf_count(_dshot_telem_perf);
-
- return ret;
-}
-
-void DShot::publish_esc_status(void)
-{
- esc_status_s &esc_status = esc_status_pub.get();
- int telemetry_index = 0;
-
- // clear data of the esc that are offline
- for (int index = 0; (index < _last_telemetry_index); index++) {
- if ((esc_status.esc_online_flags & (1 << index)) == 0) {
- memset(&esc_status.esc[index], 0, sizeof(struct esc_report_s));
- }
- }
-
- // FIXME: mark all UART Telemetry ESC's as online, otherwise commander complains even for a single dropout
- esc_status.esc_count = _num_motors;
- esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1;
- esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1;
-
- if (_bidirectional_dshot_enabled) {
- for (unsigned i = 0; i < _num_outputs; i++) {
- if (_mixing_output.isFunctionSet(i)) {
- if (up_bdshot_channel_status(i)) {
- esc_status.esc_online_flags |= 1 << i;
-
- } else {
- esc_status.esc_online_flags &= ~(1 << i);
- }
-
- ++telemetry_index;
- }
- }
- }
-
- if (!esc_status_pub.advertised()) {
- esc_status_pub.advertise();
-
- } else {
- esc_status_pub.update();
- }
-
- // reset esc online flags
- esc_status.esc_online_flags = 0;
-}
-
-int DShot::handle_new_bdshot_erpm(void)
-{
- int num_erpms = 0;
- int telemetry_index = 0;
- int erpm;
- esc_status_s &esc_status = esc_status_pub.get();
-
- esc_status.timestamp = hrt_absolute_time();
- esc_status.counter = _esc_status_counter++;
- esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
- esc_status.esc_armed_flags = _outputs_on;
-
- // We wait until all are ready.
- if (up_bdshot_num_erpm_ready() < _num_motors) {
- return 0;
- }
-
- for (unsigned i = 0; i < _num_outputs; i++) {
- if (_mixing_output.isFunctionSet(i)) {
- if (up_bdshot_get_erpm(i, &erpm) == 0) {
- num_erpms++;
- esc_status.esc_online_flags |= 1 << telemetry_index;
- esc_status.esc[telemetry_index].timestamp = hrt_absolute_time();
- esc_status.esc[telemetry_index].esc_rpm = (erpm * 100) / (_param_mot_pole_count.get() / 2);
- esc_status.esc[telemetry_index].actuator_function = _actuator_functions[telemetry_index];
- }
-
- ++telemetry_index;
- }
- }
-
- perf_count(_bdshot_rpm_perf);
-
- return num_erpms;
-}
-
-int DShot::send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index)
-{
- Command cmd{};
- cmd.command = command;
-
- if (motor_index == -1) {
- cmd.motor_mask = 0xff;
-
- } else {
- cmd.motor_mask = 1 << motor_index;
- }
-
- cmd.num_repetitions = num_repetitions;
- _new_command.store(&cmd);
-
- hrt_abstime timestamp_for_timeout = hrt_absolute_time();
-
- // wait until main thread processed it
- while (_new_command.load()) {
-
- if (hrt_elapsed_time(×tamp_for_timeout) < 2_s) {
- px4_usleep(1000);
-
- } else {
- _new_command.store(nullptr);
- PX4_WARN("DShot command timeout!");
- }
- }
-
- return 0;
-}
-
-void DShot::mixerChanged()
-{
- update_num_motors();
-}
-
-bool DShot::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated)
-{
- if (!_outputs_on) {
+ if (!_hardware_initialized || !_motor_count) {
return false;
}
- int requested_telemetry_index = -1;
+ uint16_t hw_outputs[MAX_ACTUATORS] = {};
- if (_telemetry) {
- requested_telemetry_index = _telemetry->getRequestMotorIndex();
+ for (unsigned i = 0; i < num_outputs; i++) {
+ hw_outputs[i] = static_cast(lroundf(outputs[i]));
}
- int telemetry_index = 0;
+ _esc_status.esc_armed_flags = esc_armed_mask(hw_outputs, num_outputs);
+ bool armed = _esc_status.esc_armed_flags != 0;
- for (int i = 0; i < (int)num_outputs; i++) {
+ if (!armed) {
+ // Select next command to send (if any)
+ if (_telemetry.telemetryResponseFinished() &&
+ _current_command.finished() && _telemetry.commandResponseFinished()) {
+ select_next_command();
+ }
- uint16_t output = static_cast(lroundf(outputs[i]));
-
- if (output == DSHOT_DISARM_VALUE) {
-
- if (_current_command.valid() && (_current_command.motor_mask & (1 << i))) {
- up_dshot_motor_command(i, _current_command.command, true);
-
- } else {
- up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index);
- }
+ // Send command if available
+ if (!_current_command.finished()) {
+ update_motor_commands(num_outputs);
} else {
-
- if (_param_dshot_3d_enable.get() || (_reversible_outputs & (1u << i))) {
- output = convert_output_to_3d_scaling(output);
- }
-
- up_dshot_motor_data_set(i, math::min(output, static_cast(DSHOT_MAX_THROTTLE)),
- telemetry_index == requested_telemetry_index);
+ update_motor_outputs(hw_outputs, num_outputs);
}
- telemetry_index += _mixing_output.isFunctionSet(i);
- }
-
- // Decrement the command counter
- if (_current_command.valid()) {
- --_current_command.num_repetitions;
-
- // Queue a save command after the burst if save has been requested
- if (_current_command.num_repetitions == 0 && _current_command.save) {
- _current_command.save = false;
- _current_command.num_repetitions = 10;
- _current_command.command = dshot_command_t::DShot_cmd_save_settings;
- }
+ } else {
+ update_motor_outputs(hw_outputs, num_outputs);
}
up_dshot_trigger();
@@ -431,13 +161,579 @@ bool DShot::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, un
return true;
}
+// TODO: this needs a refactor
+void DShot::select_next_command()
+{
+ // Settings Programming
+ // NOTE: only update when we're not actively programming an ESC
+ if (!_dshot_programming_active) {
+
+ if (_esc_eeprom_write_sub.updated()) {
+ auto last_generation = _esc_eeprom_write_sub.get_last_generation();
+ _esc_eeprom_write_sub.copy(&_esc_eeprom_write);
+ auto current_generation = _esc_eeprom_write_sub.get_last_generation();
+
+ if (current_generation != last_generation + 1) {
+ PX4_ERR("esc_eeprom_write lost, generation %u -> %u", last_generation, current_generation);
+ }
+
+ if (_esc_eeprom_write.index != 255 && _esc_eeprom_write.index >= DSHOT_MAX_MOTORS) {
+ PX4_ERR("esc_eeprom_write: invalid index %u", _esc_eeprom_write.index);
+
+ } else {
+ PX4_DEBUG("ESC%u: starting programming mode", _esc_eeprom_write.index + 1);
+ _dshot_programming_active = true;
+ }
+ }
+ }
+
+ // Command order or priority:
+ // - EDT Request
+ // - Settings Request
+ // - Settings Programming
+
+ // EDT Request mask
+ uint16_t needs_edt_request_mask = _bdshot_telem_online_mask & ~_bdshot_edt_requested_mask;
+
+ // Settings Request mask
+ uint16_t needs_settings_request_mask = _serial_telem_online_mask & ~_settings_requested_mask;
+
+ bool serial_telem_delay_elapsed = hrt_absolute_time() > _serial_telem_delay_until;
+
+ _current_command.clear();
+
+ // EDT Request: use motor-order masks since needs_edt_request_mask is in motor order
+ uint16_t edt_motors_to_request = _bdshot_motor_mask & needs_edt_request_mask;
+
+ if (_bdshot_edt_enabled && edt_motors_to_request != 0) {
+ // Find first motor that needs EDT request and has been online long enough
+ hrt_abstime now = hrt_absolute_time();
+
+ for (int motor_index = 0; motor_index < DSHOT_MAX_MOTORS; motor_index++) {
+ if (edt_motors_to_request & (1 << motor_index)) {
+ // Wait 1 second after ESC comes online before sending EDT (ESC init sequence)
+ if (_bdshot_telem_online_timestamps[motor_index] == 0
+ || (now - _bdshot_telem_online_timestamps[motor_index]) < 1_s) {
+ continue;
+ }
+
+ _current_command.num_repetitions = 10;
+ _current_command.command = DSHOT_EXTENDED_TELEMETRY_ENABLE;
+ _current_command.motor_mask = (1 << motor_index);
+ _bdshot_edt_requested_mask |= (1 << motor_index);
+ PX4_DEBUG("ESC%d: requesting EDT at time %.2fs", motor_index + 1, (double)now / 1000000.);
+ break;
+ }
+ }
+
+ } else if (_esc_type != 0 && _serial_telemetry_enabled && serial_telem_delay_elapsed && (_motor_mask & needs_settings_request_mask)) {
+ // Settings Request: use motor-order masks since needs_settings_request_mask is in motor order
+ uint16_t settings_motors_to_request = _motor_mask & needs_settings_request_mask;
+
+ // Find first motor that needs settings request
+ for (int motor_index = 0; motor_index < DSHOT_MAX_MOTORS; motor_index++) {
+ if (settings_motors_to_request & (1 << motor_index)) {
+ auto now = hrt_absolute_time();
+ _current_command.num_repetitions = 6;
+ _current_command.command = DSHOT_CMD_ESC_INFO;
+ _current_command.motor_mask = (1 << motor_index);
+ _current_command.expect_response = true;
+ _settings_requested_mask |= (1 << motor_index);
+ PX4_DEBUG("ESC%d: requesting Settings at time %.2fs", motor_index + 1, (double)now / 1000000.);
+ break;
+ }
+ }
+
+ } else if (_dshot_programming_active) {
+ // Motor mask for programming: all motors or single motor
+ const uint16_t programming_motor_mask = _esc_eeprom_write.index == 255
+ ? (uint16_t)((1u << DSHOT_MAX_MOTORS) - 1)
+ : (uint16_t)(1u << _esc_eeprom_write.index);
+
+ // Settings programming state machine
+ if (_programming_state == ProgrammingState::Idle) {
+ // Get next setting address/value to program
+ int next_index = -1;
+
+ // Find settings that need to be written but haven't been yet
+ int max_length = math::min((int)_esc_eeprom_write.length, (int)sizeof(_esc_eeprom_write.data));
+
+ for (int i = 0; i < max_length; i++) {
+ int array_index = i / 32;
+ int bit_index = i % 32;
+
+ bool needs_write = _esc_eeprom_write.write_mask[array_index] & (1 << bit_index);
+ bool already_written = _settings_written_mask[array_index] & (1 << bit_index);
+
+ if (needs_write && !already_written) {
+ next_index = i;
+ break;
+ }
+ }
+
+ if (next_index >= 0) {
+ if (_esc_eeprom_write.index == 255) {
+ PX4_DEBUG("ESC ALL: Writing setting at index %d, value %u", next_index, _esc_eeprom_write.data[next_index]);
+
+ } else {
+ PX4_DEBUG("ESC%d: Writing setting at index %d, value %u", _esc_eeprom_write.index + 1, next_index,
+ _esc_eeprom_write.data[next_index]);
+ }
+
+ _programming_address = next_index;
+ _programming_value = _esc_eeprom_write.data[next_index];
+ _programming_state = ProgrammingState::EnterMode;
+
+ // Pre-emptively Mark this setting as written
+ int array_index = next_index / 32;
+ int bit_index = next_index % 32;
+ _settings_written_mask[array_index] |= (1 << bit_index);
+
+ } else {
+ // All settings have been written
+ PX4_DEBUG("All settings written at time %.2fs", (double)hrt_absolute_time() / 1000000.);
+
+ _dshot_programming_active = false;
+ _current_command.command = DSHOT_CMD_SAVE_SETTINGS;
+ _current_command.num_repetitions = 6;
+ _current_command.motor_mask = programming_motor_mask;
+ _programming_state = ProgrammingState::Idle;
+
+ // Clear the written mask for this motor for next time
+ _settings_written_mask[0] = 0;
+ _settings_written_mask[1] = 0;
+
+ // Mark as unread so that we read again
+ _settings_requested_mask &= ~(programming_motor_mask);
+ _serial_telem_delay_until = hrt_absolute_time() + 500_ms;
+ }
+ }
+
+ switch (_programming_state) {
+ case ProgrammingState::EnterMode:
+ _current_command.command = DSHOT_CMD_ENTER_PROGRAMMING_MODE;
+ _current_command.num_repetitions = 6;
+ _current_command.motor_mask = programming_motor_mask;
+ _programming_state = ProgrammingState::SendAddress;
+ break;
+
+ case ProgrammingState::SendAddress:
+ _current_command.command = _programming_address;
+ _current_command.num_repetitions = 1;
+ _current_command.motor_mask = programming_motor_mask;
+ _programming_state = ProgrammingState::SendValue;
+ break;
+
+ case ProgrammingState::SendValue:
+ _current_command.command = _programming_value;
+ _current_command.num_repetitions = 1;
+ _current_command.motor_mask = programming_motor_mask;
+ _programming_state = ProgrammingState::ExitMode;
+ break;
+
+ case ProgrammingState::ExitMode:
+ _current_command.command = DSHOT_CMD_EXIT_PROGRAMMING_MODE;
+ _current_command.num_repetitions = 1;
+ _current_command.motor_mask = programming_motor_mask;
+ _programming_state = ProgrammingState::Idle;
+ break;
+
+ default:
+ break;
+ }
+ }
+}
+
+void DShot::update_motor_outputs(uint16_t outputs[MAX_ACTUATORS], int num_outputs)
+{
+ for (int i = 0; i < num_outputs; i++) {
+ int motor_index = motor_index_from_output(i);
+
+ if (motor_index < 0) {
+ up_dshot_motor_command(i, DSHOT_CMD_MOTOR_STOP, false);
+ continue;
+ }
+
+ bool set_telemetry_bit = (_telemetry_motor_index == motor_index
+ && _serial_telemetry_enabled
+ && _telemetry.telemetryResponseFinished()
+ && _telemetry.commandResponseFinished()
+ && hrt_absolute_time() > _serial_telem_delay_until);
+
+ if (set_telemetry_bit) {
+ _telemetry.startTelemetryRequest();
+ }
+
+ if (outputs[i] == DSHOT_DISARM_VALUE) {
+ up_dshot_motor_command(i, DSHOT_CMD_MOTOR_STOP, set_telemetry_bit);
+
+ } else {
+ up_dshot_motor_data_set(i, calculate_output_value(outputs[i], i), set_telemetry_bit);
+ }
+ }
+}
+
+void DShot::update_motor_commands(int num_outputs)
+{
+ bool command_sent = false;
+
+ for (int i = 0; i < num_outputs; i++) {
+ uint16_t command = DSHOT_CMD_MOTOR_STOP;
+ int motor_index = motor_index_from_output(i);
+
+ if (motor_index >= 0 && (_current_command.motor_mask & (1 << motor_index))) {
+ if (_current_command.expect_response) {
+ _telemetry.setExpectCommandResponse(motor_index, _current_command.command);
+ }
+
+ command = _current_command.command;
+ command_sent = true;
+ }
+
+ up_dshot_motor_command(i, command, false);
+ }
+
+ if (command_sent) {
+ --_current_command.num_repetitions;
+
+ // Queue a save command if it has been requested
+ if (_current_command.num_repetitions == 0 && _current_command.save) {
+ _current_command.save = false;
+ _current_command.num_repetitions = 10;
+ _current_command.command = DSHOT_CMD_SAVE_SETTINGS;
+ }
+ }
+}
+
+uint16_t DShot::esc_armed_mask(uint16_t *outputs, uint8_t num_outputs)
+{
+ uint16_t mask = 0;
+
+ for (uint8_t i = 0; i < num_outputs; i++) {
+ int motor_index = motor_index_from_output(i);
+
+ if (motor_index >= 0 && outputs[i] != DSHOT_DISARM_VALUE) {
+ mask |= (1 << motor_index);
+ }
+ }
+
+ return mask;
+}
+
+uint16_t DShot::calculate_output_value(uint16_t raw, int index)
+{
+ uint16_t output = raw;
+
+ // Reverse output if required
+ if (_3d_enabled || (_mixing_output.reversibleOutputs() & (1u << index))) {
+ output = convert_output_to_3d_scaling(raw);
+ }
+
+ output = math::min(output, DSHOT_MAX_THROTTLE);
+
+ return output;
+}
+
+bool DShot::process_serial_telemetry()
+{
+ if (!_serial_telemetry_enabled) {
+ return false;
+ }
+
+ bool all_telem_sampled = false;
+
+ if (!_telemetry.commandResponseFinished()) {
+ if (_telemetry.commandResponseStarted()) {
+ _telemetry.parseCommandResponse();
+ }
+
+ } else if (_telemetry_motor_index < 0) {
+ // Bootstrap: pick the first motor to poll
+ set_next_telemetry_index();
+
+ } else {
+
+ int motor_index = _telemetry_motor_index;
+
+ EscData esc {};
+ esc.source = TelemetrySource::Serial;
+ esc.motor_index = motor_index;
+
+ switch (_telemetry.parseTelemetryPacket(&esc)) {
+ case TelemetryStatus::NotStarted:
+ // no-op, should not hit this case
+ break;
+
+ case TelemetryStatus::NotReady:
+ // no-op, will eventually timeout
+ break;
+
+ case TelemetryStatus::Ready:
+
+ // Reset consecutive timeout counter on any successful response
+ _serial_telem_consecutive_timeouts[motor_index] = 0;
+ _serial_telem_skip_mask &= ~(1 << motor_index);
+
+ // Online mask is in motor order for consistency with BDShot
+ if (_serial_telem_online_mask & (1 << motor_index)) {
+ consume_esc_data(esc);
+ all_telem_sampled = set_next_telemetry_index();
+ perf_count(_serial_telem_success_perf);
+
+ } else {
+ hrt_abstime now = hrt_absolute_time();
+
+ // Timestamps are in actuator order to match _esc_status.esc[]
+ if (_serial_telem_online_timestamps[motor_index] == 0) {
+ _serial_telem_online_timestamps[motor_index] = now;
+ }
+
+ // Mark as online only after 100_ms without errors
+ if (now - _serial_telem_online_timestamps[motor_index] > 100_ms) {
+ _serial_telem_online_mask |= (1 << motor_index);
+ }
+ }
+
+ break;
+
+ case TelemetryStatus::Timeout:
+ // Set ESC data to zeroes
+ // Error counts and timestamps are in actuator order to match _esc_status.esc[]
+ _serial_telem_errors[motor_index]++;
+ _serial_telem_online_mask &= ~(1 << motor_index);
+ _serial_telem_online_timestamps[motor_index] = 0;
+
+ // Track consecutive timeouts for adaptive skip
+ if (_serial_telem_consecutive_timeouts[motor_index] < SERIAL_TELEM_SKIP_THRESHOLD) {
+ _serial_telem_consecutive_timeouts[motor_index]++;
+
+ if (_serial_telem_consecutive_timeouts[motor_index] >= SERIAL_TELEM_SKIP_THRESHOLD) {
+ _serial_telem_skip_mask |= (1 << motor_index);
+ PX4_WARN("ESC%d serial telemetry lost, skipping", motor_index + 1);
+ }
+ }
+
+ // Consume an empty EscData to zero the data
+ consume_esc_data(esc);
+ all_telem_sampled = set_next_telemetry_index();
+ perf_count(_serial_telem_timeout_perf);
+ break;
+
+ case TelemetryStatus::ParseError:
+ // Set ESC data to zeroes
+ PX4_WARN("Telem parse error, ESC %d", motor_index);
+ _serial_telem_errors[motor_index]++;
+ _serial_telem_online_mask &= ~(1 << motor_index);
+ _serial_telem_online_timestamps[motor_index] = 0;
+ // Consume an empty EscData to zero the data
+ consume_esc_data(esc);
+ all_telem_sampled = set_next_telemetry_index();
+ _serial_telem_delay_until = hrt_absolute_time() + 1_s;
+ perf_count(_serial_telem_error_perf);
+ break;
+ }
+ }
+
+ return all_telem_sampled;
+}
+
+bool DShot::set_next_telemetry_index()
+{
+ // Round-robin through motor indices (Motor1=0, Motor2=1, ...).
+ // _telemetry_motor_index and _telemetry_requested_mask are in motor-index domain.
+
+ // Active motors are those that exist and aren't being skipped
+ uint16_t active_motor_mask = _motor_mask & ~_serial_telem_skip_mask;
+
+ if (active_motor_mask == 0) {
+ _telemetry_motor_index = -1;
+ _telemetry_requested_mask = 0;
+ return true;
+ }
+
+ int start = (_telemetry_motor_index < 0) ? 0 : (_telemetry_motor_index + 1) % DSHOT_MAX_MOTORS;
+ int motor = start;
+
+ do {
+ if ((active_motor_mask & (1 << motor)) && !(_telemetry_requested_mask & (1 << motor))) {
+ _telemetry_motor_index = motor;
+ _telemetry_requested_mask |= (1 << motor);
+ return false;
+ }
+
+ motor = (motor + 1) % DSHOT_MAX_MOTORS;
+ } while (motor != start);
+
+ // All active motors have been sampled
+ _telemetry_motor_index = -1;
+ _telemetry_requested_mask = 0;
+ perf_count(_serial_telem_allsampled_perf);
+ return true;
+}
+
+bool DShot::process_bdshot_telemetry()
+{
+ if (!_bdshot_output_mask) {
+ return false;
+ }
+
+ hrt_abstime now = hrt_absolute_time();
+
+ // Don't try to process any telem data until after ESCs have been given time to boot
+ if (now < ESC_INIT_TELEM_DELAY) {
+ return false;
+ }
+
+ // We wait until all BDShot channels are ready.
+ if ((up_bdshot_get_ready_mask() & _bdshot_output_mask) != _bdshot_output_mask) {
+ return false;
+ }
+
+ for (uint8_t output_channel = 0; output_channel < DSHOT_MAXIMUM_CHANNELS; output_channel++) {
+ if (!(_bdshot_output_mask & (1 << output_channel))) {
+ continue;
+ }
+
+ int motor_index = motor_index_from_output(output_channel);
+
+ if (motor_index < 0) {
+ continue;
+ }
+
+ EscData esc = {};
+ esc.source = TelemetrySource::BDShot;
+ esc.motor_index = motor_index;
+ esc.timestamp = now;
+
+ _bdshot_telem_errors[motor_index] = up_bdshot_num_errors(output_channel);
+
+ if (up_bdshot_channel_online(output_channel)) {
+
+ // Record when this motor first came online
+ if (!(_bdshot_telem_online_mask & (1 << motor_index))) {
+ _bdshot_telem_online_timestamps[motor_index] = now;
+ }
+
+ // Online mask is in motor order for command/request logic
+ _bdshot_telem_online_mask |= (1 << motor_index);
+
+ // Only update RPM if online
+ int erpm = 0;
+
+ if (up_bdshot_get_erpm(output_channel, &erpm) == PX4_OK) {
+ esc.erpm = erpm * 100;
+
+ } else {
+ // Use previous value (esc_status is indexed by motor_index)
+ int pole_count = math::max(get_pole_count(motor_index), 2); // constrain to 2 to avoid divide by zero
+ esc.erpm = _esc_status.esc[motor_index].esc_rpm * (pole_count / 2);
+ }
+
+ // Extended DShot Telemetry
+ if (_bdshot_edt_enabled) {
+
+ uint8_t value = 0;
+
+ if (up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_TEMPERATURE, &value) == PX4_OK) {
+ esc.temperature = value; // BDShot temperature is in C
+
+ } else {
+ esc.temperature = _esc_status.esc[motor_index].esc_temperature; // use previous
+ }
+
+ if (up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_VOLTAGE, &value) == PX4_OK) {
+ esc.voltage = static_cast(value) / 4.f; // BDShot voltage is in 0.25V
+
+ } else {
+ esc.voltage = _esc_status.esc[motor_index].esc_voltage; // use previous
+ }
+
+ if (up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_CURRENT, &value) == PX4_OK) {
+ esc.current = static_cast(value) / 2.f; // BDShot current is in 0.5A
+
+ } else {
+ esc.current = _esc_status.esc[motor_index].esc_current; // use previous
+ }
+ }
+
+ } else {
+ _bdshot_telem_online_mask &= ~(1 << motor_index);
+ _bdshot_telem_online_timestamps[motor_index] = 0;
+ _bdshot_edt_requested_mask &= ~(1 << motor_index);
+ perf_count(_bdshot_error_perf);
+ }
+
+ consume_esc_data(esc);
+ }
+
+ perf_count(_bdshot_recv_perf);
+
+ return true;
+}
+
+void DShot::consume_esc_data(const EscData &esc)
+{
+ int motor_index = esc.motor_index;
+
+ if (!math::isInRange(motor_index, 0, DSHOT_MAX_MOTORS - 1)) {
+ return;
+ }
+
+ // Determine if this motor is online based on its telemetry sources.
+ // Intentionally conservative: when both BDShot and serial telemetry are enabled,
+ // both must report online. A motor reporting offline on either source indicates
+ // a degraded setup that should be surfaced to the operator.
+ uint16_t motor_bit = 1u << motor_index;
+ bool is_bdshot = _bdshot_motor_mask & motor_bit;
+
+ // Both sources must report online when enabled (conservative)
+ bool motor_online = (!is_bdshot || (_bdshot_telem_online_mask & motor_bit))
+ && (!_serial_telemetry_enabled || (_serial_telem_online_mask & motor_bit));
+
+ if (motor_online) {
+ _esc_status.esc_online_flags |= motor_bit;
+
+ } else {
+ _esc_status.esc_online_flags &= ~motor_bit;
+ }
+
+ // esc_status is indexed by motor_index (Motor1=0, Motor2=1, ...)
+ _esc_status.esc[motor_index].esc_errorcount = _serial_telem_errors[motor_index] +
+ _bdshot_telem_errors[motor_index];
+
+ if (esc.source == TelemetrySource::Serial) {
+ // Only use SerialTelemetry eRPM when BDShot is disabled
+ if (!is_bdshot) {
+ _esc_status.esc[motor_index].timestamp = esc.timestamp;
+ int pole_count = math::max(get_pole_count(motor_index), 2); // constrain to 2 to avoid divide by zero
+ _esc_status.esc[motor_index].esc_rpm = esc.erpm / (pole_count / 2);
+ }
+
+ _esc_status.esc[motor_index].esc_voltage = esc.voltage;
+ _esc_status.esc[motor_index].esc_current = esc.current;
+ _esc_status.esc[motor_index].esc_temperature = esc.temperature;
+
+ } else if (esc.source == TelemetrySource::BDShot) {
+ _esc_status.esc[motor_index].timestamp = esc.timestamp;
+ int pole_count = math::max(get_pole_count(motor_index), 2); // constrain to 2 to avoid divide by zero
+ _esc_status.esc[motor_index].esc_rpm = esc.erpm / (pole_count / 2);
+
+ // Only use BDShot Volt/Curr/Temp when Serial Telemetry is disabled
+ if (!_serial_telemetry_enabled) {
+ _esc_status.esc[motor_index].esc_voltage = esc.voltage;
+ _esc_status.esc[motor_index].esc_current = esc.current;
+ _esc_status.esc[motor_index].esc_temperature = esc.temperature;
+ }
+ }
+}
+
uint16_t DShot::convert_output_to_3d_scaling(uint16_t output)
{
// DShot 3D splits the throttle ranges in two.
// This is in terms of DShot values, code below is in terms of actuator_output
// Direction 1) 48 is the slowest, 1047 is the fastest.
// Direction 2) 1049 is the slowest, 2047 is the fastest.
- if (output >= _param_dshot_3d_dead_l.get() && output < _param_dshot_3d_dead_h.get()) {
+ if (output >= _3d_dead_l && output < _3d_dead_h) {
return DSHOT_DISARM_VALUE;
}
@@ -451,7 +747,7 @@ uint16_t DShot::convert_output_to_3d_scaling(uint16_t output)
}
float max_output = 999.f;
- float min_output = max_output * _param_dshot_min.get();
+ float min_output = max_output * _dshot_min;
output = math::min(max_output, (min_output + output * (max_output - min_output) / max_output));
if (upper_range) {
@@ -461,159 +757,130 @@ uint16_t DShot::convert_output_to_3d_scaling(uint16_t output)
return output;
}
-void DShot::Run()
+void DShot::handle_vehicle_commands()
{
- if (should_exit()) {
- ScheduleClear();
- _mixing_output.unregister();
+ vehicle_command_s command = {};
- exit_and_cleanup(desc);
+ while (_current_command.finished() && _vehicle_command_sub.update(&command)) {
+
+ switch (command.command) {
+ case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR:
+ handle_configure_actuator(command);
+ break;
+
+ case vehicle_command_s::VEHICLE_CMD_ESC_REQUEST_EEPROM:
+ handle_esc_request_eeprom(command);
+ break;
+
+ default:
+ break;
+ }
+ }
+}
+
+void DShot::handle_configure_actuator(const vehicle_command_s &command)
+{
+ int function = (int)(command.param5 + 0.5);
+
+ if (function > 1000) {
+ // NOTE: backwards compatibility for QGC - 1101=Motor1, 1102=Motor2, etc
+ function -= 1000;
+ }
+
+ int motor_index = -1;
+
+ if (function >= (int)OutputFunction::Motor1 && function < ((int)OutputFunction::Motor1 + DSHOT_MAXIMUM_CHANNELS)) {
+ for (int i = 0; i < DSHOT_MAXIMUM_CHANNELS; ++i) {
+ if ((int)_mixing_output.outputFunction(i) == function && _mixing_output.isMotor(i)) {
+ motor_index = (int)_mixing_output.outputFunction(i) - (int)OutputFunction::Motor1;
+ }
+ }
+ }
+
+ vehicle_command_ack_s command_ack = {};
+ command_ack.command = command.command;
+ command_ack.target_system = command.source_system;
+ command_ack.target_component = command.source_component;
+ command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+
+ if ((motor_index >= 0) && (motor_index < DSHOT_MAX_MOTORS)) {
+ int type = lroundf(command.param1);
+ PX4_DEBUG("motor_index: %i type: %i", motor_index, type);
+ _current_command.clear();
+ _current_command.command = DSHOT_CMD_MOTOR_STOP;
+ _current_command.num_repetitions = 10;
+ _current_command.save = false;
+
+
+ switch (type) {
+ case vehicle_command_s::ACTUATOR_CONFIGURATION_BEEP:
+ _current_command.command = DSHOT_CMD_BEEP1;
+ break;
+
+ case vehicle_command_s::ACTUATOR_CONFIGURATION_3D_MODE_OFF:
+ _current_command.command = DSHOT_CMD_3D_MODE_OFF;
+ _current_command.save = true;
+ break;
+
+ case vehicle_command_s::ACTUATOR_CONFIGURATION_3D_MODE_ON:
+ _current_command.command = DSHOT_CMD_3D_MODE_ON;
+ _current_command.save = true;
+ break;
+
+ case vehicle_command_s::ACTUATOR_CONFIGURATION_SPIN_DIRECTION1:
+ _current_command.command = DSHOT_CMD_SPIN_DIRECTION_1;
+ _current_command.save = true;
+ break;
+
+ case vehicle_command_s::ACTUATOR_CONFIGURATION_SPIN_DIRECTION2:
+ _current_command.command = DSHOT_CMD_SPIN_DIRECTION_2;
+ _current_command.save = true;
+ break;
+
+ default:
+ PX4_WARN("unknown command: %i", type);
+ break;
+ }
+
+ if (_current_command.command != DSHOT_CMD_MOTOR_STOP) {
+ command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+ _current_command.motor_mask = 1 << motor_index;
+ }
+ }
+
+ command_ack.timestamp = hrt_absolute_time();
+ _command_ack_pub.publish(command_ack);
+}
+
+void DShot::handle_esc_request_eeprom(const vehicle_command_s &command)
+{
+ PX4_DEBUG("Received ESC_REQUEST_EEPROM");
+ PX4_DEBUG("esc_index: %d", (int)command.param2);
+
+ int esc_index = lroundf(command.param2);
+
+ if (esc_index != 255 && (esc_index < 0 || esc_index >= DSHOT_MAX_MOTORS)) {
+ PX4_ERR("ESC_REQUEST_EEPROM: invalid esc_index %d", esc_index);
return;
}
- perf_begin(_cycle_perf);
+ if (esc_index == 255) {
+ PX4_DEBUG("mark all unread");
+ _settings_requested_mask = 0;
- _mixing_output.update();
-
- // update output status if armed or if mixer is loaded
- bool outputs_on = true;
-
- if (_outputs_on != outputs_on) {
- enable_dshot_outputs(outputs_on);
+ } else {
+ PX4_DEBUG("mark one unread");
+ _settings_requested_mask &= ~(1 << esc_index);
}
-
- if (_telemetry) {
- const int telem_update = _telemetry->update(_num_motors);
-
- if (telem_update >= 0) {
- const int need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->latestESCData(),
- _bidirectional_dshot_enabled);
-
- // We don't want to publish twice, once by telemetry and once by bidirectional dishot.
- if (!_bidirectional_dshot_enabled && need_to_publish) {
- publish_esc_status();
- }
- }
- }
-
- if (_bidirectional_dshot_enabled) {
- // Add bdshot data to esc status
- const int need_to_publish = handle_new_bdshot_erpm();
-
- if (need_to_publish) {
- publish_esc_status();
- }
- }
-
- if (_parameter_update_sub.updated()) {
- update_params();
- }
-
- // telemetry device update request?
- if (_request_telemetry_init.load()) {
- init_telemetry(_telemetry_device, _telemetry_swap_rxtx);
- _request_telemetry_init.store(false);
- }
-
- // new command?
- if (!_current_command.valid()) {
- Command *new_command = _new_command.load();
-
- if (new_command) {
- _current_command = *new_command;
- _new_command.store(nullptr);
- }
- }
-
- handle_vehicle_commands();
-
- if (!_mixing_output.armed().armed) {
- if (_reversible_outputs != _mixing_output.reversibleOutputs()) {
- _reversible_outputs = _mixing_output.reversibleOutputs();
- update_params();
- }
- }
-
- // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
- _mixing_output.updateSubscriptions(true);
-
- perf_end(_cycle_perf);
}
-void DShot::handle_vehicle_commands()
+int DShot::get_pole_count(int motor_index) const
{
- vehicle_command_s vehicle_command;
-
- while (!_current_command.valid() && _vehicle_command_sub.update(&vehicle_command)) {
-
- if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR) {
- int function = (int)(vehicle_command.param5 + 0.5);
-
- if (function < 1000) {
- const int first_motor_function = 1; // from MAVLink ACTUATOR_OUTPUT_FUNCTION
- const int first_servo_function = 33;
-
- if (function >= first_motor_function && function < first_motor_function + actuator_test_s::MAX_NUM_MOTORS) {
- function = function - first_motor_function + actuator_test_s::FUNCTION_MOTOR1;
-
- } else if (function >= first_servo_function && function < first_servo_function + actuator_test_s::MAX_NUM_SERVOS) {
- function = function - first_servo_function + actuator_test_s::FUNCTION_SERVO1;
-
- } else {
- function = INT32_MAX;
- }
-
- } else {
- function -= 1000;
- }
-
- int type = (int)(vehicle_command.param1 + 0.5f);
- int index = -1;
-
- for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
- if ((int)_mixing_output.outputFunction(i) == function) {
- index = i;
- }
- }
-
- vehicle_command_ack_s command_ack{};
- command_ack.command = vehicle_command.command;
- command_ack.target_system = vehicle_command.source_system;
- command_ack.target_component = vehicle_command.source_component;
- command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
-
- if (index != -1) {
- PX4_DEBUG("setting command: index: %i type: %i", index, type);
- _current_command.command = dshot_command_t::DShot_cmd_motor_stop;
-
- switch (type) {
- case 1: _current_command.command = dshot_command_t::DShot_cmd_beacon1; break;
-
- case 2: _current_command.command = dshot_command_t::DShot_cmd_3d_mode_on; break;
-
- case 3: _current_command.command = dshot_command_t::DShot_cmd_3d_mode_off; break;
-
- case 4: _current_command.command = dshot_command_t::DShot_cmd_spin_direction_1; break;
-
- case 5: _current_command.command = dshot_command_t::DShot_cmd_spin_direction_2; break;
- }
-
- if (_current_command.command == dshot_command_t::DShot_cmd_motor_stop) {
- PX4_WARN("unknown command: %i", type);
-
- } else {
- command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
- _current_command.motor_mask = 1 << index;
- _current_command.num_repetitions = 10;
- _current_command.save = true;
- }
-
- }
-
- command_ack.timestamp = hrt_absolute_time();
- _command_ack_pub.publish(command_ack);
- }
+ if (motor_index >= 0 && motor_index < DSHOT_MAX_MOTORS) {
+ return _pole_count_params[motor_index];
}
+
+ return 14;
}
void DShot::update_params()
@@ -623,36 +890,316 @@ void DShot::update_params()
updateParams();
- // we use a minimum value of 1, since 0 is for disarmed
- _mixing_output.setAllMinValues(math::constrain(static_cast((_param_dshot_min.get() *
- static_cast(DSHOT_MAX_THROTTLE))),
- DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE));
+ // Cache params used in hot path
+ _bdshot_edt_enabled = _param_dshot_bidir_edt.get();
+ _3d_enabled = _param_dshot_3d_enable.get();
+ _3d_dead_l = _param_dshot_3d_dead_l.get();
+ _3d_dead_h = _param_dshot_3d_dead_h.get();
+ _dshot_min = _param_dshot_min.get();
+ _esc_type = _param_dshot_esc_type.get();
+
+ // Calculate minimum DShot output as percent of throttle and constrain.
+ float min_value = _dshot_min * (float)DSHOT_MAX_THROTTLE;
+ uint16_t dshot_min_value = math::constrain((uint16_t)min_value, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
+
+ _mixing_output.setAllMinValues(dshot_min_value);
// Do not use the minimum parameter for reversible outputs
- for (unsigned i = 0; i < _num_outputs; ++i) {
- if ((1 << i) & _reversible_outputs) {
+ for (uint8_t i = 0; i < DSHOT_MAXIMUM_CHANNELS; i++) {
+ if (_mixing_output.reversibleOutputs() & (1 << i)) {
_mixing_output.minValue(i) = DSHOT_MIN_THROTTLE;
}
}
+
+ // Update per-motor pole count param handles and cached values
+ for (int i = 0; i < DSHOT_MAX_MOTORS; i++) {
+ char name[20];
+ snprintf(name, sizeof(name), "DSHOT_MOT_POL%d", i + 1);
+ _param_pole_count_handles[i] = param_find(name);
+
+ int32_t pole_count = 14;
+ param_get(_param_pole_count_handles[i], &pole_count);
+ _pole_count_params[i] = pole_count;
+ }
+}
+
+void DShot::mixerChanged()
+{
+ PX4_DEBUG("mixerChanged");
+
+ _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
+
+ // Build actuator-order and motor-order masks from actual motor assignments
+ uint32_t new_output_mask = 0;
+ uint32_t new_motor_mask = 0;
+
+ for (int actuator_channel = 0; actuator_channel < DSHOT_MAXIMUM_CHANNELS; actuator_channel++) {
+ int motor_index = motor_index_from_output(actuator_channel);
+
+ if (motor_index >= 0) {
+ new_output_mask |= (1 << actuator_channel);
+ new_motor_mask |= (1 << motor_index);
+ _esc_status.esc[motor_index].actuator_function = (uint8_t)_mixing_output.outputFunction(actuator_channel);
+ }
+ }
+
+ // Check if we need to (re)initialize hardware
+ // bool needs_init = !_hardware_initialized || (new_output_mask != _output_mask);
+
+ if (!_hardware_initialized) {
+ PX4_DEBUG("Output mask changed: 0x%" PRIx32 " -> 0x%" PRIx32, _output_mask, new_output_mask);
+ _output_mask = new_output_mask;
+ _motor_mask = new_motor_mask;
+ _motor_count = __builtin_popcount(_output_mask);
+
+ uint32_t new_bdshot_output_mask = _bdshot_timer_channels & _output_mask;
+ PX4_DEBUG("BDShot Output mask changed: 0x%" PRIx32 " -> 0x%" PRIx32, _bdshot_output_mask, new_bdshot_output_mask);
+ _bdshot_output_mask = new_bdshot_output_mask;
+
+ // Compute motor-order BDShot mask
+ uint32_t new_bdshot_motor_mask = 0;
+
+ for (int actuator_channel = 0; actuator_channel < DSHOT_MAXIMUM_CHANNELS; actuator_channel++) {
+ int motor_index = motor_index_from_output(actuator_channel);
+
+ if (motor_index >= 0 && (new_bdshot_output_mask & (1 << actuator_channel))) {
+ new_bdshot_motor_mask |= (1 << motor_index);
+ }
+ }
+
+ _bdshot_motor_mask = new_bdshot_motor_mask;
+ PX4_DEBUG("Motor mask: 0x%" PRIx32 ", BDShot motor mask: 0x%" PRIx32, _motor_mask, _bdshot_motor_mask);
+
+ up_dshot_init(_output_mask, _bdshot_output_mask, _dshot_frequency, _bdshot_edt_enabled);
+ up_dshot_arm(true);
+ _hardware_initialized = true;
+ }
+}
+
+bool DShot::initialize_dshot()
+{
+ uint32_t dshot_timer_channels = 0; // Channels on DShot-enabled timers
+
+ // Iterate through timers to determine DShot frequency and BDShot channels
+ for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
+
+ // Get mask of actuator channels associated with this timer group
+ uint32_t timer_channels = io_timer_get_group(timer_index);
+
+ if (timer_channels == 0) {
+ continue;
+ }
+
+ char param_name[17] = {};
+ snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer_index);
+
+ int32_t tim_config = 0;
+ param_t handle = param_find(param_name);
+ param_get(handle, &tim_config);
+
+ // Map timer config to DShot frequency and BDShot flag
+ uint32_t freq = 0;
+ bool is_bdshot = false;
+
+ switch (tim_config) {
+ case TIM_CONFIG_DSHOT150: freq = DSHOT150; break;
+
+ case TIM_CONFIG_DSHOT300: freq = DSHOT300; break;
+
+ case TIM_CONFIG_DSHOT600: freq = DSHOT600; break;
+
+ case TIM_CONFIG_BDSHOT150: freq = DSHOT150; is_bdshot = true; break;
+
+ case TIM_CONFIG_BDSHOT300: freq = DSHOT300; is_bdshot = true; break;
+
+ case TIM_CONFIG_BDSHOT600: freq = DSHOT600; is_bdshot = true; break;
+
+ default: break;
+ }
+
+ if (freq > 0) {
+ if (_dshot_frequency != 0 && _dshot_frequency != freq) {
+ PX4_WARN("Mixed DShot frequencies across timers, using freq: %lu", freq);
+ }
+
+ _dshot_frequency = freq;
+ dshot_timer_channels |= timer_channels;
+
+ if (is_bdshot) {
+ _bdshot_timer_channels |= timer_channels;
+ }
+ }
+ }
+
+ if (dshot_timer_channels == 0) {
+ PX4_WARN("No channels configured");
+ return false;
+ }
+
+ return true;
+}
+
+void DShot::init_telemetry(const char *device, bool swap_rxtx)
+{
+ if (!device) {
+ return;
+ }
+
+ if (_telemetry.init(device, swap_rxtx) != PX4_OK) {
+ PX4_ERR("telemetry init failed");
+ return;
+ }
+
+ // Enable serial telemetry now that we've successfully initialized
+ _serial_telemetry_enabled = true;
+
+ // Initialize ESC settings handlers based on ESC type
+ ESCType esc_type = static_cast(_param_dshot_esc_type.get());
+ _telemetry.initSettingsHandlers(esc_type, _motor_mask);
+}
+
+static void print_spacer()
+{
+ for (int i = 0; i < 64; i++) {
+ PX4_INFO_RAW("-");
+ }
+
+ PX4_INFO_RAW("\n");
+}
+
+int DShot::print_status()
+{
+ print_spacer();
+ PX4_INFO("DShot Driver Status");
+ print_spacer();
+
+ // Configuration
+ PX4_INFO("Configuration:");
+ PX4_INFO(" Output Mask: 0x%02lx (%u channels)", (unsigned long)_output_mask, _motor_count);
+ PX4_INFO(" BDShot Telemetry: %s", _bdshot_output_mask ? "Enabled" : "Disabled");
+ PX4_INFO(" Serial Telemetry: %s", _serial_telemetry_enabled ? "Enabled" : "Disabled");
+ PX4_INFO(" Extended DShot: %s", _bdshot_edt_enabled ? "Enabled" : "Disabled");
+
+ const char *esc_type_str = "Unknown";
+
+ switch (_param_dshot_esc_type.get()) {
+ case 1: esc_type_str = "AM32"; break;
+
+ default: break;
+ }
+
+ PX4_INFO(" ESC Type: %s (%ld)", esc_type_str, _param_dshot_esc_type.get());
+ PX4_INFO(" 3D Mode: %s", _param_dshot_3d_enable.get() ? "Enabled" : "Disabled");
+
+ // Per-motor pole counts
+ PX4_INFO(" Motor Poles:");
+
+ for (int i = 0; i < DSHOT_MAX_MOTORS; i++) {
+ if (_motor_mask & (1 << i)) {
+ PX4_INFO(" Motor %d: %d poles", i + 1, get_pole_count(i));
+ }
+ }
+
+ // Telemetry Status
+ if (_bdshot_output_mask || _serial_telemetry_enabled) {
+ print_spacer();
+ PX4_INFO("Telemetry Status:");
+ PX4_INFO(" %-4s %-6s %-8s %-8s %-12s %-12s", "Ch", "Motor", "BDShot", "Serial", "BDShot Err", "Serial Err");
+
+ for (int actuator_channel = 0; actuator_channel < DSHOT_MAXIMUM_CHANNELS; actuator_channel++) {
+ int motor_index = motor_index_from_output(actuator_channel);
+
+ if (motor_index < 0) {
+ continue;
+ }
+
+ const char *bdshot_status = "-";
+ const char *serial_status = "-";
+
+ // Online masks are in motor order
+ if (_bdshot_output_mask & (1 << actuator_channel)) {
+ if (!up_bdshot_channel_capture_supported(actuator_channel)) {
+ bdshot_status = "No DMA";
+
+ } else {
+ bdshot_status = (_bdshot_telem_online_mask & (1 << motor_index)) ? "Online" : "Offline";
+ }
+ }
+
+ if (_serial_telemetry_enabled) {
+ serial_status = (_serial_telem_online_mask & (1 << motor_index)) ? "Online" : "Offline";
+ }
+
+ // Error arrays are in actuator order
+ PX4_INFO(" %-4d %-6d %-8s %-8s %-12lu %-12lu",
+ actuator_channel + 1,
+ motor_index + 1,
+ bdshot_status,
+ serial_status,
+ (unsigned long)_bdshot_telem_errors[motor_index],
+ (unsigned long)_serial_telem_errors[motor_index]);
+ }
+
+ if (_bdshot_output_mask && _bdshot_edt_enabled) {
+ PX4_INFO(" EDT Requested Mask (motor order): 0x%02x", _bdshot_edt_requested_mask);
+ }
+
+ if (_serial_telemetry_enabled) {
+ PX4_INFO(" Settings Requested Mask (motor order): 0x%02x", _settings_requested_mask);
+ }
+ }
+
+ // Bidirectional DShot hardware status
+ if (_bdshot_output_mask) {
+ print_spacer();
+ PX4_INFO("Bidirectional DShot Hardware:");
+ up_bdshot_status();
+ }
+
+ // Serial telemetry stats
+ if (_serial_telemetry_enabled) {
+ print_spacer();
+ PX4_INFO("Serial Telemetry Stats:");
+ _telemetry.printStatus();
+ }
+
+ print_spacer();
+ PX4_INFO("Mixer Information:");
+ _mixing_output.printStatus();
+
+ print_spacer();
+ PX4_INFO("Performance Counters:");
+ perf_print_counter(_cycle_perf);
+
+ if (_bdshot_output_mask) {
+ perf_print_counter(_bdshot_recv_perf);
+ perf_print_counter(_bdshot_error_perf);
+ }
+
+ if (_serial_telemetry_enabled) {
+ perf_print_counter(_serial_telem_success_perf);
+ perf_print_counter(_serial_telem_error_perf);
+ perf_print_counter(_serial_telem_timeout_perf);
+ perf_print_counter(_serial_telem_allsampled_perf);
+ }
+
+ print_spacer();
+
+ return 0;
}
int DShot::custom_command(int argc, char *argv[])
{
const char *verb = argv[0];
- int motor_index = -1; // select motor index, default: -1=all
int myoptind = 1;
bool swap_rxtx = false;
const char *device_name = nullptr;
int ch;
const char *myoptarg = nullptr;
- while ((ch = px4_getopt(argc, argv, "m:xd:", &myoptind, &myoptarg)) != EOF) {
+ while ((ch = px4_getopt(argc, argv, "xd:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
- case 'm':
- motor_index = strtol(myoptarg, nullptr, 10) - 1;
- break;
-
case 'x':
swap_rxtx = true;
break;
@@ -669,8 +1216,8 @@ int DShot::custom_command(int argc, char *argv[])
if (!strcmp(verb, "telemetry")) {
if (device_name) {
// telemetry can be requested before the module is started
- strncpy(_telemetry_device, device_name, sizeof(_telemetry_device) - 1);
- _telemetry_device[sizeof(_telemetry_device) - 1] = '\0';
+ strncpy(_serial_port_path, device_name, sizeof(_serial_port_path) - 1);
+ _serial_port_path[sizeof(_serial_port_path) - 1] = '\0';
_telemetry_swap_rxtx = swap_rxtx;
_request_telemetry_init.store(true);
}
@@ -678,36 +1225,6 @@ int DShot::custom_command(int argc, char *argv[])
return 0;
}
- struct VerbCommand {
- const char *name;
- dshot_command_t command;
- int num_repetitions;
- };
-
- constexpr VerbCommand commands[] = {
- {"reverse", DShot_cmd_spin_direction_2, 10},
- {"normal", DShot_cmd_spin_direction_1, 10},
- {"save", DShot_cmd_save_settings, 10},
- {"3d_on", DShot_cmd_3d_mode_on, 10},
- {"3d_off", DShot_cmd_3d_mode_off, 10},
- {"beep1", DShot_cmd_beacon1, 1},
- {"beep2", DShot_cmd_beacon2, 1},
- {"beep3", DShot_cmd_beacon3, 1},
- {"beep4", DShot_cmd_beacon4, 1},
- {"beep5", DShot_cmd_beacon5, 1},
- };
-
- for (unsigned i = 0; i < sizeof(commands) / sizeof(commands[0]); ++i) {
- if (!strcmp(verb, commands[i].name)) {
- if (!is_running(desc)) {
- PX4_ERR("module not running");
- return -1;
- }
-
- return get_instance(desc)->send_command_thread_safe(commands[i].command, commands[i].num_repetitions, motor_index);
- }
- }
-
if (!is_running(desc)) {
int ret = DShot::task_spawn(argc, argv);
@@ -719,28 +1236,29 @@ int DShot::custom_command(int argc, char *argv[])
return print_usage("unknown command");
}
-int DShot::print_status()
+int DShot::task_spawn(int argc, char *argv[])
{
- PX4_INFO("Outputs initialized: %s", _outputs_initialized ? "yes" : "no");
- PX4_INFO("Outputs used: 0x%" PRIx32, _output_mask);
- PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no");
- perf_print_counter(_cycle_perf);
- perf_print_counter(_bdshot_rpm_perf);
- perf_print_counter(_dshot_telem_perf);
+ DShot *instance = new DShot();
- _mixing_output.printStatus();
+ if (instance) {
+ desc.object.store(instance);
+ desc.task_id = task_id_is_work_queue;
- if (_telemetry) {
- PX4_INFO("telemetry on: %s", _telemetry_device);
- _telemetry->printStatus();
+ if (instance->init() == PX4_OK) {
+ return PX4_OK;
+ }
+
+ PX4_INFO("Exiting");
+
+ } else {
+ PX4_ERR("alloc failed");
}
- /* Print dshot status */
- if (_bidirectional_dshot_enabled) {
- up_bdshot_status();
- }
+ delete instance;
+ desc.object.store(nullptr);
+ desc.task_id = -1;
- return 0;
+ return PX4_ERROR;
}
int DShot::print_usage(const char *reason)
@@ -752,22 +1270,13 @@ int DShot::print_usage(const char *reason)
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
-This is the DShot output driver. It is similar to the fmu driver, and can be used as drop-in replacement
+This is the DShot output driver. It can be used as drop-in replacement
to use DShot as ESC communication protocol instead of PWM.
-On startup, the module tries to occupy all available pins for DShot output.
-It skips all pins already in use (e.g. by a camera trigger module).
-
It supports:
- DShot150, DShot300, DShot600
- telemetry via separate UART and publishing as esc_status message
-- sending DShot commands via CLI
-### Examples
-Permanently reverse motor 1:
-$ dshot reverse -m 1
-$ dshot save -m 1
-After saving, the reversed direction will be regarded as the normal one. So to reverse again repeat the same commands.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("dshot", "driver");
@@ -777,28 +1286,6 @@ After saving, the reversed direction will be regarded as the normal one. So to r
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "UART device", false);
PRINT_MODULE_USAGE_PARAM_FLAG('x', "Swap RX/TX pins", true);
- // DShot commands
- PRINT_MODULE_USAGE_COMMAND_DESCR("reverse", "Reverse motor direction");
- PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
- PRINT_MODULE_USAGE_COMMAND_DESCR("normal", "Normal motor direction");
- PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
- PRINT_MODULE_USAGE_COMMAND_DESCR("save", "Save current settings");
- PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
- PRINT_MODULE_USAGE_COMMAND_DESCR("3d_on", "Enable 3D mode");
- PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
- PRINT_MODULE_USAGE_COMMAND_DESCR("3d_off", "Disable 3D mode");
- PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
- PRINT_MODULE_USAGE_COMMAND_DESCR("beep1", "Send Beep pattern 1");
- PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
- PRINT_MODULE_USAGE_COMMAND_DESCR("beep2", "Send Beep pattern 2");
- PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
- PRINT_MODULE_USAGE_COMMAND_DESCR("beep3", "Send Beep pattern 3");
- PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
- PRINT_MODULE_USAGE_COMMAND_DESCR("beep4", "Send Beep pattern 4");
- PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
- PRINT_MODULE_USAGE_COMMAND_DESCR("beep5", "Send Beep pattern 5");
- PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
-
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h
index fc2909e20d..e12cc88f82 100644
--- a/src/drivers/dshot/DShot.h
+++ b/src/drivers/dshot/DShot.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2026 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
@@ -39,23 +39,33 @@
#include
#include
#include
+#include
+#include "DShotCommon.h"
#include "DShotTelemetry.h"
using namespace time_literals;
-#if !defined(DIRECT_PWM_OUTPUT_CHANNELS)
-# error "board_config.h needs to define DIRECT_PWM_OUTPUT_CHANNELS"
-#endif
+static_assert(DSHOT_MAXIMUM_CHANNELS <= 16, "DShot driver uses uint16_t bitmasks");
-/** Dshot PWM frequency, Hz */
-static constexpr unsigned int DSHOT150 = 150000u;
-static constexpr unsigned int DSHOT300 = 300000u;
-static constexpr unsigned int DSHOT600 = 600000u;
+static constexpr hrt_abstime ESC_INIT_TELEM_DELAY = 5_s;
-static constexpr int DSHOT_DISARM_VALUE = 0;
-static constexpr int DSHOT_MIN_THROTTLE = 1;
-static constexpr int DSHOT_MAX_THROTTLE = 1999;
+/// Dshot PWM frequency (Hz)
+static constexpr uint32_t DSHOT150 = 150000u;
+static constexpr uint32_t DSHOT300 = 300000u;
+static constexpr uint32_t DSHOT600 = 600000u;
+
+/// Timer config values from PWM_TIM param (matches pwm_out/module.yaml enum)
+static constexpr int32_t TIM_CONFIG_DSHOT150 = -5;
+static constexpr int32_t TIM_CONFIG_DSHOT300 = -4;
+static constexpr int32_t TIM_CONFIG_DSHOT600 = -3;
+static constexpr int32_t TIM_CONFIG_BDSHOT150 = -8;
+static constexpr int32_t TIM_CONFIG_BDSHOT300 = -7;
+static constexpr int32_t TIM_CONFIG_BDSHOT600 = -6;
+
+static constexpr uint16_t DSHOT_DISARM_VALUE = 0;
+static constexpr uint16_t DSHOT_MIN_THROTTLE = 1;
+static constexpr uint16_t DSHOT_MAX_THROTTLE = 1999;
class DShot final : public ModuleBase, public OutputModuleInterface
{
@@ -65,121 +75,188 @@ public:
DShot();
~DShot() override;
- /** @see ModuleBase */
+ // @see ModuleBase
static int custom_command(int argc, char *argv[]);
+ // @see ModuleBase
+ int print_status() override;
+
+ // @see ModuleBase
+ static int print_usage(const char *reason = nullptr);
+
+ // @see ModuleBase
+ static int task_spawn(int argc, char *argv[]);
+
int init();
void mixerChanged() override;
- /** @see ModuleBase::print_status() */
- int print_status() override;
-
- /** @see ModuleBase */
- static int print_usage(const char *reason = nullptr);
-
- /**
- * Send a dshot command to one or all motors
- * This is expected to be called from another thread.
- * @param num_repetitions number of times to repeat, set at least to 1
- * @param motor_index index or -1 for all
- * @return 0 on success, <0 error otherwise
- */
- int send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index);
-
- /** @see ModuleBase */
- static int task_spawn(int argc, char *argv[]);
-
- bool telemetry_enabled() const { return _telemetry != nullptr; }
-
- bool updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override;
+ bool updateOutputs(float *outputs, unsigned num_outputs, unsigned num_control_groups_updated) override;
private:
- /** Disallow copy construction and move assignment. */
+ // Disallow copy construction and move assignment
DShot(const DShot &) = delete;
DShot operator=(const DShot &) = delete;
- enum class DShotConfig {
- Disabled = 0,
- DShot150 = 150,
- DShot300 = 300,
- DShot600 = 600,
- };
-
- struct Command {
- dshot_command_t command{};
- int num_repetitions{0};
- uint8_t motor_mask{0xff};
- bool save{false};
-
- bool valid() const { return num_repetitions > 0; }
- void clear() { num_repetitions = 0; }
- };
-
- int _last_telemetry_index{-1};
- uint8_t _actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {};
-
- void enable_dshot_outputs(const bool enabled);
-
+ bool initialize_dshot();
void init_telemetry(const char *device, bool swap_rxtx);
- int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data, bool ignore_rpm);
+ // Map output channel to motor index [0..DSHOT_MAX_MOTORS-1], or -1 if not a motor
+ int motor_index_from_output(int output_channel) const
+ {
+ if (!_mixing_output.isMotor(output_channel)) { return -1; }
- void publish_esc_status(void);
+ int idx = (int)_mixing_output.outputFunction(output_channel) - (int)OutputFunction::Motor1;
+ return (idx >= 0 && idx < DSHOT_MAX_MOTORS) ? idx : -1;
+ }
- int handle_new_bdshot_erpm(void);
+ uint16_t esc_armed_mask(uint16_t *outputs, uint8_t num_outputs);
- void Run() override;
+ void update_motor_outputs(uint16_t *outputs, int num_outputs);
+ void update_motor_commands(int num_outputs);
+ void select_next_command();
- void update_params();
+ bool set_next_telemetry_index(); // Returns true when the telemetry index has wrapped, indicating all configured motors have been sampled.
+ bool process_serial_telemetry();
+ bool process_bdshot_telemetry();
- void update_num_motors();
-
- void handle_vehicle_commands();
+ void consume_esc_data(const EscData &data);
+ uint16_t calculate_output_value(uint16_t raw, int index);
uint16_t convert_output_to_3d_scaling(uint16_t output);
- MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
- uint32_t _reversible_outputs{};
+ void Run() override;
+ void update_params();
- DShotTelemetry *_telemetry{nullptr};
+ // Mavlink command handlers
+ void handle_vehicle_commands();
+ void handle_configure_actuator(const vehicle_command_s &command);
+ void handle_esc_request_eeprom(const vehicle_command_s &command);
- uORB::PublicationMultiData esc_status_pub{ORB_ID(esc_status)};
+ // Mixer
+ MixingOutput _mixing_output{PARAM_PREFIX, DSHOT_MAXIMUM_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
- static char _telemetry_device[20];
- static bool _telemetry_swap_rxtx;
- static px4::atomic_bool _request_telemetry_init;
-
- px4::atomic _new_command{nullptr};
-
-
- bool _outputs_initialized{false};
- bool _outputs_on{false};
- bool _bidirectional_dshot_enabled{false};
-
- static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS};
+ // Actuator-order masks (indexed by output channel)
uint32_t _output_mask{0};
+ uint32_t _bdshot_output_mask{0};
- int _num_motors{0};
-
- perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
- perf_counter_t _bdshot_rpm_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot rpm")};
- perf_counter_t _dshot_telem_perf{perf_alloc(PC_COUNT, MODULE_NAME": dshot telem")};
-
- Command _current_command{};
+ // Motor-order masks (indexed by motor number: Motor1=0, Motor2=1, etc.)
+ uint32_t _motor_mask{0};
+ uint32_t _bdshot_motor_mask{0};
+ uint8_t _motor_count{0};
+ // uORB
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
- uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)};
- uint16_t _esc_status_counter{0};
+ uORB::Subscription _esc_eeprom_write_sub{ORB_ID(esc_eeprom_write)};
+ uORB::PublicationMultiData _esc_status_pub{ORB_ID(esc_status)};
+ uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)};
+
+ esc_status_s _esc_status{};
+
+ // Status information
+ uint32_t _bdshot_telem_online_mask = 0; // Mask indicating telem receive status for bidirectional dshot telem
+ uint32_t _serial_telem_online_mask = 0; // Mask indicating telem receive status for serial telem
+ uint32_t _serial_telem_errors[DSHOT_MAX_MOTORS] = {};
+ uint32_t _bdshot_telem_errors[DSHOT_MAX_MOTORS] = {};
+ uint16_t _bdshot_edt_requested_mask = 0;
+ uint16_t _settings_requested_mask = 0;
+
+ // Array of timestamps indicating when the telemetry came online
+ hrt_abstime _serial_telem_online_timestamps[DSHOT_MAX_MOTORS] = {};
+ hrt_abstime _bdshot_telem_online_timestamps[DSHOT_MAX_MOTORS] = {};
+
+ // Serial telemetry adaptive skip: stop polling motors that never respond
+ static constexpr int SERIAL_TELEM_SKIP_THRESHOLD = 10; // consecutive timeouts before skipping
+ uint16_t _serial_telem_skip_mask = 0; // motors to skip in round-robin
+ uint8_t _serial_telem_consecutive_timeouts[DSHOT_MAX_MOTORS] = {};
+
+ // Serial Telemetry
+ DShotTelemetry _telemetry;
+ static char _serial_port_path[20];
+ static bool _telemetry_swap_rxtx;
+ static px4::atomic_bool _request_telemetry_init;
+ int _telemetry_motor_index = -1;
+ uint32_t _telemetry_requested_mask = 0;
+ hrt_abstime _serial_telem_delay_until = ESC_INIT_TELEM_DELAY;
+
+ // Parameters we must load only at init
+ bool _serial_telemetry_enabled = false;
+ bool _bdshot_edt_enabled = false;
+
+ // Cached parameters (updated in update_params())
+ bool _3d_enabled = false;
+ int _3d_dead_l = 0;
+ int _3d_dead_h = 0;
+ float _dshot_min = 0.f;
+ int _esc_type = 0;
+
+ // Hardware initialization state
+ bool _hardware_initialized = false;
+ uint32_t _dshot_frequency = 0;
+ uint32_t _bdshot_timer_channels = 0;
+
+ // Perf counters
+ perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+ perf_counter_t _bdshot_recv_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot recv")};
+ perf_counter_t _bdshot_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot error")};
+ perf_counter_t _serial_telem_success_perf{perf_alloc(PC_COUNT, MODULE_NAME": serial telem success")};
+ perf_counter_t _serial_telem_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": serial telem error")};
+ perf_counter_t _serial_telem_timeout_perf{perf_alloc(PC_COUNT, MODULE_NAME": serial telem timeout")};
+ perf_counter_t _serial_telem_allsampled_perf{perf_alloc(PC_COUNT, MODULE_NAME": serial telem all sampled")};
+
+ // Commands
+ struct DShotCommand {
+ uint16_t command{};
+ int num_repetitions{0};
+ uint16_t motor_mask{(1u << DSHOT_MAXIMUM_CHANNELS) - 1};
+ bool save{false};
+ bool expect_response{false};
+
+ bool finished() const { return num_repetitions == 0; }
+ void clear()
+ {
+ command = 0;
+ num_repetitions = 0;
+ motor_mask = 0;
+ save = false;
+ expect_response = false;
+ }
+ };
+
+ DShotCommand _current_command{};
+
+ // DShot Programming Mode
+ enum class ProgrammingState {
+ Idle,
+ EnterMode,
+ SendAddress,
+ SendValue,
+ ExitMode
+ };
+
+ esc_eeprom_write_s _esc_eeprom_write{};
+ bool _dshot_programming_active = {false};
+ uint32_t _settings_written_mask[2] = {};
+
+ ProgrammingState _programming_state{ProgrammingState::Idle};
+
+ uint16_t _programming_address{};
+ uint16_t _programming_value{};
+
+ param_t _param_pole_count_handles[DSHOT_MAX_MOTORS] {};
+ int32_t _pole_count_params[DSHOT_MAX_MOTORS] {};
+ int get_pole_count(int motor_index) const;
+
+ // Parameters
DEFINE_PARAMETERS(
+ (ParamInt) _param_dshot_esc_type,
(ParamFloat) _param_dshot_min,
(ParamBool) _param_dshot_3d_enable,
(ParamInt) _param_dshot_3d_dead_h,
(ParamInt) _param_dshot_3d_dead_l,
- (ParamInt) _param_mot_pole_count,
- (ParamBool) _param_bidirectional_enable
+ (ParamBool) _param_dshot_bidir_edt
)
};
diff --git a/src/drivers/dshot/DShotCommon.h b/src/drivers/dshot/DShotCommon.h
new file mode 100644
index 0000000000..ff96282c6c
--- /dev/null
+++ b/src/drivers/dshot/DShotCommon.h
@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2026 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include
+#include
+#include
+
+#if !defined(DIRECT_PWM_OUTPUT_CHANNELS)
+# error "board_config.h needs to define DIRECT_PWM_OUTPUT_CHANNELS"
+#endif
+
+static constexpr int DSHOT_MAXIMUM_CHANNELS = DIRECT_PWM_OUTPUT_CHANNELS;
+
+// Motor-indexed arrays use this — bounded by both hardware channels and protocol limit
+static constexpr int DSHOT_MAX_MOTORS = DSHOT_MAXIMUM_CHANNELS < esc_status_s::CONNECTED_ESC_MAX
+ ? DSHOT_MAXIMUM_CHANNELS : esc_status_s::CONNECTED_ESC_MAX;
+
+enum class TelemetrySource {
+ Serial = 0,
+ BDShot = 1,
+};
+
+struct EscData {
+ int motor_index; // Motor index 0..(CONNECTED_ESC_MAX-1)
+ hrt_abstime timestamp; // Sample time
+ TelemetrySource source;
+
+ float temperature; // [C]
+ float voltage; // [V]
+ float current; // [A]
+ int32_t erpm; // [eRPM]
+};
+
+enum class TelemetryStatus {
+ NotStarted = 0,
+ NotReady = 1,
+ Ready = 2,
+ Timeout = 3,
+ ParseError = 4,
+};
+
+inline uint8_t crc8(const uint8_t *buf, unsigned len)
+{
+ auto update_crc8 = [](uint8_t crc, uint8_t crc_seed) {
+ uint8_t crc_u = crc ^ crc_seed;
+
+ for (unsigned i = 0; i < 8; ++i) {
+ crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
+ }
+
+ return crc_u;
+ };
+
+ uint8_t crc = 0;
+
+ for (unsigned i = 0; i < len; ++i) {
+ crc = update_crc8(buf[i], crc);
+ }
+
+ return crc;
+}
diff --git a/src/drivers/dshot/DShotTelemetry.cpp b/src/drivers/dshot/DShotTelemetry.cpp
index 0567759226..c3c85a7ff3 100644
--- a/src/drivers/dshot/DShotTelemetry.cpp
+++ b/src/drivers/dshot/DShotTelemetry.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2026 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
@@ -34,6 +34,7 @@
#include "DShotTelemetry.h"
#include
+#include
#include
#include
@@ -47,6 +48,14 @@ using namespace time_literals;
DShotTelemetry::~DShotTelemetry()
{
_uart.close();
+
+ // Clean up settings handlers
+ for (int i = 0; i < DSHOT_MAX_MOTORS; i++) {
+ if (_settings_handlers[i]) {
+ delete _settings_handlers[i];
+ _settings_handlers[i] = nullptr;
+ }
+ }
}
int DShotTelemetry::init(const char *port, bool swap_rxtx)
@@ -76,304 +85,224 @@ int DShotTelemetry::init(const char *port, bool swap_rxtx)
return PX4_OK;
}
-int DShotTelemetry::update(int num_motors)
+void DShotTelemetry::initSettingsHandlers(ESCType esc_type, uint16_t output_mask)
{
- if (_current_motor_index_request == -1) {
- // nothing in progress, start a request
- _current_motor_index_request = 0;
- _current_request_start = 0;
- _frame_position = 0;
- return -1;
+ if (_settings_initialized) {
+ return;
}
- // read from the uart. This must be non-blocking, so check first if there is data available
- int bytes_available = _uart.bytesAvailable();
+ _esc_type = esc_type;
- if (bytes_available <= 0) {
- // no data available. Check for a timeout
- const hrt_abstime now = hrt_absolute_time();
+ for (uint8_t i = 0; i < DSHOT_MAX_MOTORS; i++) {
- if (_current_request_start > 0 && now - _current_request_start > 30_ms) {
- if (_redirect_output) {
- // clear and go back to internal buffer
- _redirect_output = nullptr;
- _current_motor_index_request = -1;
+ bool output_enabled = (1 << i) & output_mask;
- } else {
- PX4_DEBUG("ESC telemetry timeout for motor %i (frame pos=%i)", _current_motor_index_request, _frame_position);
- ++_num_timeouts;
- }
-
- requestNextMotor(num_motors);
- return -2;
+ if (!output_enabled) {
+ continue;
}
- return -1;
- }
+ ESCSettingsInterface *interface = nullptr;
- uint8_t buf[ESC_FRAME_SIZE];
- int bytes = _uart.read(buf, sizeof(buf));
+ switch (esc_type) {
+ case ESCType::AM32:
+ interface = new AM32Settings(i);
+ break;
- int ret = -1;
+ case ESCType::Unknown:
+ break;
- for (int i = 0; i < bytes && ret == -1; ++i) {
- if (_redirect_output) {
- _redirect_output->buffer[_redirect_output->buf_pos++] = buf[i];
+ default:
+ PX4_WARN("Unsupported ESC type for settings: %d", (int)esc_type);
+ break;
+ }
- if (_redirect_output->buf_pos == sizeof(_redirect_output->buffer)) {
- // buffer full: return & go back to internal buffer
- _redirect_output = nullptr;
- ret = _current_motor_index_request;
- _current_motor_index_request = -1;
- requestNextMotor(num_motors);
- }
-
- } else {
- bool successful_decoding;
-
- if (decodeByte(buf[i], successful_decoding)) {
- if (successful_decoding) {
- ret = _current_motor_index_request;
- }
-
- requestNextMotor(num_motors);
- }
+ if (interface) {
+ _settings_handlers[i] = interface;
}
}
- return ret;
+ _settings_initialized = true;
}
-bool DShotTelemetry::decodeByte(uint8_t byte, bool &successful_decoding)
+void DShotTelemetry::resetCommandResponse()
{
- _frame_buffer[_frame_position++] = byte;
- successful_decoding = false;
+ _command_response_motor_index = -1;
+ _command_response_start = 0;
+ _command_response_position = 0;
+}
- if (_frame_position == ESC_FRAME_SIZE) {
- PX4_DEBUG("got ESC frame for motor %i", _current_motor_index_request);
- uint8_t checksum = crc8(_frame_buffer, ESC_FRAME_SIZE - 1);
- uint8_t checksum_data = _frame_buffer[ESC_FRAME_SIZE - 1];
-
- if (checksum == checksum_data) {
- _latest_data.time = hrt_absolute_time();
- _latest_data.temperature = _frame_buffer[0];
- _latest_data.voltage = (_frame_buffer[1] << 8) | _frame_buffer[2];
- _latest_data.current = (_frame_buffer[3] << 8) | _frame_buffer[4];
- _latest_data.consumption = (_frame_buffer[5]) << 8 | _frame_buffer[6];
- _latest_data.erpm = (_frame_buffer[7] << 8) | _frame_buffer[8];
- PX4_DEBUG("Motor %i: temp=%i, V=%i, cur=%i, consumpt=%i, rpm=%i", _current_motor_index_request,
- _latest_data.temperature, _latest_data.voltage, _latest_data.current, _latest_data.consumption,
- _latest_data.erpm);
- ++_num_successful_responses;
- successful_decoding = true;
-
- } else {
- ++_num_checksum_errors;
- }
-
- return true;
+void DShotTelemetry::parseCommandResponse()
+{
+ if (hrt_elapsed_time(&_command_response_start) > 1_s) {
+ PX4_WARN("Command response timed out: %d bytes received", _command_response_position);
+ PX4_WARN("At time %.2fs", (double)hrt_absolute_time() / 1000000.);
+ resetCommandResponse();
+ return;
}
- return false;
+ uint8_t buf[COMMAND_RESPONSE_MAX_SIZE] = {};
+ int bytes = _uart.read(buf, sizeof(buf));
+
+ if (bytes <= 0) {
+ return;
+ }
+
+ // Handle potential overflow, fail out
+ if (_command_response_position + bytes > COMMAND_RESPONSE_MAX_SIZE) {
+ PX4_ERR("command response overflow");
+ resetCommandResponse();
+ return;
+ }
+
+ // Add bytes to buffer
+ memcpy(&_command_response_buffer[_command_response_position], buf, bytes);
+ _command_response_position += bytes;
+
+ switch (_command_response_command) {
+ case DSHOT_CMD_ESC_INFO: {
+ if (_command_response_motor_index < 0 || _command_response_motor_index >= DSHOT_MAX_MOTORS) {
+ resetCommandResponse();
+ return;
+ }
+
+ auto handler = _settings_handlers[_command_response_motor_index];
+
+ if (!handler) {
+ resetCommandResponse();
+ break;
+ }
+
+ if (_command_response_position == handler->getExpectedResponseSize()) {
+ // TODO: handle command failures
+ handler->decodeInfoResponse(_command_response_buffer, _command_response_position);
+ resetCommandResponse();
+ }
+
+ break;
+ }
+
+ default:
+ break;
+ }
+}
+
+TelemetryStatus DShotTelemetry::parseTelemetryPacket(EscData *esc_data)
+{
+ if (telemetryResponseFinished()) {
+ return TelemetryStatus::NotStarted;
+ }
+
+ hrt_abstime elapsed = hrt_elapsed_time(&_telemetry_request_start);
+
+ // At 115200 baud the 10-byte response takes ~868us. Skip polling until data could have arrived.
+ if (elapsed < 800) {
+ return TelemetryStatus::NotReady;
+ }
+
+ uint8_t buf[TELEMETRY_FRAME_SIZE];
+ int bytes = _uart.read(buf, sizeof(buf));
+
+ if (bytes <= 0) {
+ if (elapsed > 5_ms) {
+ ++_num_timeouts;
+
+ // Mark telemetry request as finished
+ _telemetry_request_start = 0;
+ _frame_position = 0;
+ return TelemetryStatus::Timeout;
+ }
+
+ return TelemetryStatus::NotReady;
+ }
+
+ return decodeTelemetryResponse(buf, bytes, esc_data);
+}
+
+TelemetryStatus DShotTelemetry::decodeTelemetryResponse(uint8_t *buffer, int length, EscData *esc_data)
+{
+ auto status = TelemetryStatus::NotReady;
+
+ for (int i = 0; i < length; i++) {
+ _frame_buffer[_frame_position++] = buffer[i];
+
+ /*
+ * ESC Telemetry Frame Structure (10 bytes total)
+ * =============================================
+ * Byte 0: Temperature (uint8_t) [deg C]
+ * Byte 1-2: Voltage (uint16_t, big-endian) [0.01V]
+ * Byte 3-4: Current (uint16_t, big-endian) [0.01A]
+ * Byte 5-6: Consumption (uint16_t, big-endian) [mAh]
+ * Byte 7-8: eRPM (uint16_t, big-endian) [100ERPM]
+ * Byte 9: CRC8 Checksum
+ */
+
+ if (_frame_position == TELEMETRY_FRAME_SIZE) {
+ uint8_t checksum = crc8(_frame_buffer, TELEMETRY_FRAME_SIZE - 1);
+ uint8_t checksum_data = _frame_buffer[TELEMETRY_FRAME_SIZE - 1];
+
+ if (checksum == checksum_data) {
+
+ uint8_t temperature = _frame_buffer[0];
+ uint16_t voltage = (_frame_buffer[1] << 8) | _frame_buffer[2];
+ uint16_t current = (_frame_buffer[3] << 8) | _frame_buffer[4];
+ // int16_t consumption = (_frame_buffer[5]) << 8 | _frame_buffer[6];
+ uint16_t erpm = (_frame_buffer[7] << 8) | _frame_buffer[8];
+
+ esc_data->timestamp = hrt_absolute_time();
+ esc_data->temperature = (float)temperature;
+ esc_data->voltage = (float)voltage * 0.01f;
+ esc_data->current = (float)current * 0.01f;
+ esc_data->erpm = erpm * 100;
+
+ ++_num_successful_responses;
+ status = TelemetryStatus::Ready;
+
+ } else {
+ ++_num_checksum_errors;
+ status = TelemetryStatus::ParseError;
+ }
+
+ // Mark telemetry request as finished
+ _telemetry_request_start = 0;
+ _frame_position = 0;
+ }
+ }
+
+ return status;
+}
+
+void DShotTelemetry::setExpectCommandResponse(int motor_index, uint16_t command)
+{
+ _command_response_motor_index = motor_index;
+ _command_response_command = command;
+ _command_response_start = hrt_absolute_time();
+ _command_response_position = 0;
+}
+
+bool DShotTelemetry::commandResponseFinished()
+{
+ return _command_response_motor_index < 0;
+}
+
+bool DShotTelemetry::commandResponseStarted()
+{
+ return _command_response_start > 0;
+}
+
+void DShotTelemetry::startTelemetryRequest()
+{
+ _frame_position = 0;
+ _telemetry_request_start = hrt_absolute_time();
+}
+
+bool DShotTelemetry::telemetryResponseFinished()
+{
+ return _telemetry_request_start == 0;
}
void DShotTelemetry::printStatus() const
{
- PX4_INFO("Number of successful ESC frames: %i", _num_successful_responses);
- PX4_INFO("Number of timeouts: %i", _num_timeouts);
- PX4_INFO("Number of CRC errors: %i", _num_checksum_errors);
-}
-
-uint8_t DShotTelemetry::crc8(const uint8_t *buf, uint8_t len)
-{
- auto update_crc8 = [](uint8_t crc, uint8_t crc_seed) {
- uint8_t crc_u = crc ^ crc_seed;
-
- for (int i = 0; i < 8; ++i) {
- crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
- }
-
- return crc_u;
- };
-
- uint8_t crc = 0;
-
- for (int i = 0; i < len; ++i) {
- crc = update_crc8(buf[i], crc);
- }
-
- return crc;
-}
-
-void DShotTelemetry::requestNextMotor(int num_motors)
-{
- _current_motor_index_request = (_current_motor_index_request + 1) % num_motors;
- _current_request_start = 0;
- _frame_position = 0;
-}
-
-int DShotTelemetry::getRequestMotorIndex()
-{
- if (_current_request_start != 0) {
- // already in progress, do not send another request
- return -1;
- }
-
- _current_request_start = hrt_absolute_time();
- return _current_motor_index_request;
-}
-
-void DShotTelemetry::decodeAndPrintEscInfoPacket(const OutputBuffer &buffer)
-{
- static constexpr int version_position = 12;
- const uint8_t *data = buffer.buffer;
-
- if (buffer.buf_pos < version_position) {
- PX4_ERR("Not enough data received");
- return;
- }
-
- enum class ESCVersionInfo {
- BLHELI32,
- KissV1,
- KissV2,
- };
- ESCVersionInfo version;
- int packet_length;
-
- if (data[version_position] == 254) {
- version = ESCVersionInfo::BLHELI32;
- packet_length = esc_info_size_blheli32;
-
- } else if (data[version_position] == 255) {
- version = ESCVersionInfo::KissV2;
- packet_length = esc_info_size_kiss_v2;
-
- } else {
- version = ESCVersionInfo::KissV1;
- packet_length = esc_info_size_kiss_v1;
- }
-
- if (buffer.buf_pos != packet_length) {
- PX4_ERR("Packet length mismatch (%i != %i)", buffer.buf_pos, packet_length);
- return;
- }
-
- if (DShotTelemetry::crc8(data, packet_length - 1) != data[packet_length - 1]) {
- PX4_ERR("Checksum mismatch");
- return;
- }
-
- uint8_t esc_firmware_version = 0;
- uint8_t esc_firmware_subversion = 0;
- uint8_t esc_type = 0;
-
- switch (version) {
- case ESCVersionInfo::KissV1:
- esc_firmware_version = data[12];
- esc_firmware_subversion = (data[13] & 0x1f) + 97;
- esc_type = (data[13] & 0xe0) >> 5;
- break;
-
- case ESCVersionInfo::KissV2:
- case ESCVersionInfo::BLHELI32:
- esc_firmware_version = data[13];
- esc_firmware_subversion = data[14];
- esc_type = data[15];
- break;
- }
-
- const char *esc_type_str = "";
-
- switch (version) {
- case ESCVersionInfo::KissV1:
- case ESCVersionInfo::KissV2:
- switch (esc_type) {
- case 1: esc_type_str = "KISS8A";
- break;
-
- case 2: esc_type_str = "KISS16A";
- break;
-
- case 3: esc_type_str = "KISS24A";
- break;
-
- case 5: esc_type_str = "KISS Ultralite";
- break;
-
- default: esc_type_str = "KISS (unknown)";
- break;
- }
-
- break;
-
- case ESCVersionInfo::BLHELI32: {
- char *esc_type_mutable = (char *)(data + 31);
- esc_type_mutable[32] = 0;
- esc_type_str = esc_type_mutable;
- }
- break;
- }
-
- PX4_INFO("ESC Type: %s", esc_type_str);
-
- PX4_INFO("MCU Serial Number: %02x%02x%02x-%02x%02x%02x-%02x%02x%02x-%02x%02x%02x",
- data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7], data[8],
- data[9], data[10], data[11]);
-
- switch (version) {
- case ESCVersionInfo::KissV1:
- case ESCVersionInfo::KissV2:
- PX4_INFO("Firmware version: %d.%d%c", esc_firmware_version / 100, esc_firmware_version % 100,
- (char)esc_firmware_subversion);
- break;
-
- case ESCVersionInfo::BLHELI32:
- PX4_INFO("Firmware version: %d.%d", esc_firmware_version, esc_firmware_subversion);
- break;
- }
-
- if (version == ESCVersionInfo::KissV2 || version == ESCVersionInfo::BLHELI32) {
- PX4_INFO("Rotation Direction: %s", data[16] ? "reversed" : "normal");
- PX4_INFO("3D Mode: %s", data[17] ? "on" : "off");
- }
-
- if (version == ESCVersionInfo::BLHELI32) {
- uint8_t setting = data[18];
-
- switch (setting) {
- case 0:
- PX4_INFO("Low voltage Limit: off");
- break;
-
- case 255:
- PX4_INFO("Low voltage Limit: unsupported");
- break;
-
- default:
- PX4_INFO("Low voltage Limit: %d.%01d V", setting / 10, setting % 10);
- break;
- }
-
- setting = data[19];
-
- switch (setting) {
- case 0:
- PX4_INFO("Current Limit: off");
- break;
-
- case 255:
- PX4_INFO("Current Limit: unsupported");
- break;
-
- default:
- PX4_INFO("Current Limit: %d A", setting);
- break;
- }
-
- for (int i = 0; i < 4; ++i) {
- setting = data[i + 20];
- PX4_INFO("LED %d: %s", i, setting ? (setting == 255 ? "unsupported" : "on") : "off");
- }
- }
+ PX4_INFO("Successful ESC frames: %i", _num_successful_responses);
+ PX4_INFO("Timeouts: %i", _num_timeouts);
+ PX4_INFO("CRC errors: %i", _num_checksum_errors);
}
diff --git a/src/drivers/dshot/DShotTelemetry.h b/src/drivers/dshot/DShotTelemetry.h
index ccbe7b1ce0..c9593ac152 100644
--- a/src/drivers/dshot/DShotTelemetry.h
+++ b/src/drivers/dshot/DShotTelemetry.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2026 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
@@ -34,90 +34,59 @@
#pragma once
#include
-#include
+#include
+#include "DShotCommon.h"
+#include "esc/AM32Settings.h"
class DShotTelemetry
{
public:
- struct EscData {
- hrt_abstime time;
- int8_t temperature; ///< [deg C]
- int16_t voltage; ///< [0.01V]
- int16_t current; ///< [0.01A]
- int16_t consumption; ///< [mAh]
- int16_t erpm; ///< [100ERPM]
- };
-
- static constexpr int esc_info_size_blheli32 = 64;
- static constexpr int esc_info_size_kiss_v1 = 15;
- static constexpr int esc_info_size_kiss_v2 = 21;
- static constexpr int max_esc_info_size = esc_info_size_blheli32;
-
- struct OutputBuffer {
- uint8_t buffer[max_esc_info_size];
- int buf_pos{0};
- int motor_index;
- };
~DShotTelemetry();
int init(const char *uart_device, bool swap_rxtx);
-
- /**
- * Read telemetry from the UART (non-blocking) and handle timeouts.
- * @param num_motors How many DShot enabled motors
- * @return -1 if no update, -2 timeout, >= 0 for the motor index. Use @latestESCData() to get the data.
- */
- int update(int num_motors);
-
- bool redirectActive() const { return _redirect_output != nullptr; }
-
- /**
- * Get the motor index for which telemetry should be requested.
- * @return -1 if no request should be made, motor index otherwise
- */
- int getRequestMotorIndex();
-
- const EscData &latestESCData() const { return _latest_data; }
-
- /**
- * Check whether we are currently expecting to read new data from an ESC
- */
- bool expectingData() const { return _current_request_start != 0; }
-
void printStatus() const;
- static void decodeAndPrintEscInfoPacket(const OutputBuffer &buffer);
+ void startTelemetryRequest();
+ bool telemetryResponseFinished();
+
+ TelemetryStatus parseTelemetryPacket(EscData *esc_data);
+
+ // Attempt to parse a command response. Returns the index of the ESC or -1 on failure.
+ void parseCommandResponse();
+ bool commandResponseFinished();
+ bool commandResponseStarted();
+
+ void setExpectCommandResponse(int motor_index, uint16_t command);
+ void resetCommandResponse();
+ void initSettingsHandlers(ESCType esc_type, uint16_t output_mask);
private:
- static constexpr int ESC_FRAME_SIZE = 10;
+ static constexpr int COMMAND_RESPONSE_MAX_SIZE = 49;
+ static constexpr int TELEMETRY_FRAME_SIZE = 10;
+ TelemetryStatus decodeTelemetryResponse(uint8_t *buffer, int length, EscData *esc_data);
- void requestNextMotor(int num_motors);
+ device::Serial _uart{};
- /**
- * Decode a single byte from an ESC feedback frame
- * @param byte
- * @param successful_decoding set to true if checksum matches
- * @return true if received the expected amount of bytes and the next motor can be requested
- */
- bool decodeByte(uint8_t byte, bool &successful_decoding);
+ // Command response
+ int _command_response_motor_index{-1};
+ uint16_t _command_response_command{0};
+ uint8_t _command_response_buffer[COMMAND_RESPONSE_MAX_SIZE];
+ int _command_response_position{0};
+ hrt_abstime _command_response_start{0};
- static uint8_t crc8(const uint8_t *buf, uint8_t len);
-
- device::Serial _uart {};
-
- uint8_t _frame_buffer[ESC_FRAME_SIZE];
+ // Telemetry packet
+ uint8_t _frame_buffer[TELEMETRY_FRAME_SIZE];
int _frame_position{0};
-
- EscData _latest_data;
-
- int _current_motor_index_request{-1};
- hrt_abstime _current_request_start{0};
-
- OutputBuffer *_redirect_output{nullptr}; ///< if set, all read bytes are stored here instead of the internal buffer
+ hrt_abstime _telemetry_request_start{0};
// statistics
int _num_timeouts{0};
int _num_successful_responses{0};
int _num_checksum_errors{0};
+
+ // Settings
+ ESCSettingsInterface *_settings_handlers[DSHOT_MAX_MOTORS] = {nullptr};
+ ESCType _esc_type{ESCType::Unknown};
+ bool _settings_initialized{false};
};
diff --git a/src/drivers/dshot/esc/AM32Settings.cpp b/src/drivers/dshot/esc/AM32Settings.cpp
new file mode 100644
index 0000000000..7f67a6ac1a
--- /dev/null
+++ b/src/drivers/dshot/esc/AM32Settings.cpp
@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2026 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.
+ *
+ ****************************************************************************/
+
+#include "AM32Settings.h"
+#include "../DShotCommon.h"
+#include
+
+static constexpr int RESPONSE_SIZE = EEPROM_SIZE + 1; // 48B data + 1B CRC
+
+uORB::Publication AM32Settings::_esc_eeprom_read_pub{ORB_ID(esc_eeprom_read)};
+
+AM32Settings::AM32Settings(int index)
+ : _esc_index(index)
+{}
+
+int AM32Settings::getExpectedResponseSize()
+{
+ return RESPONSE_SIZE;
+}
+
+void AM32Settings::publish_latest()
+{
+ esc_eeprom_read_s data = {};
+ data.timestamp = hrt_absolute_time();
+ data.firmware = 1; // ESC_FIRMWARE_AM32
+ data.index = _esc_index;
+ memcpy(data.data, &_eeprom_data, sizeof(_eeprom_data));
+ data.length = sizeof(_eeprom_data);
+ _esc_eeprom_read_pub.publish(data);
+}
+
+bool AM32Settings::decodeInfoResponse(const uint8_t *buf, int size)
+{
+ if (size != RESPONSE_SIZE) {
+ return false;
+ }
+
+ uint8_t checksum = crc8(buf, EEPROM_SIZE);
+ uint8_t checksum_data = buf[EEPROM_SIZE];
+
+ if (checksum != checksum_data) {
+ PX4_WARN("Command Response checksum failed!");
+ return false;
+ }
+
+ PX4_DEBUG("Successfully received AM32 settings from ESC%d", _esc_index + 1);
+
+ // Store data for retrieval later if requested
+ memcpy(&_eeprom_data, buf, EEPROM_SIZE);
+
+ // Publish data immediately
+ publish_latest();
+
+ return true;
+}
diff --git a/src/drivers/dshot/esc/AM32Settings.h b/src/drivers/dshot/esc/AM32Settings.h
new file mode 100644
index 0000000000..c3e8f6f365
--- /dev/null
+++ b/src/drivers/dshot/esc/AM32Settings.h
@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2026 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "ESCSettingsInterface.h"
+#include
+#include
+
+static constexpr int EEPROM_SIZE = 48;
+
+class AM32Settings : public ESCSettingsInterface
+{
+public:
+ AM32Settings(int index);
+
+ int getExpectedResponseSize() override;
+ bool decodeInfoResponse(const uint8_t *buf, int size) override;
+
+ void publish_latest() override;
+
+private:
+ int _esc_index{};
+ uint8_t _eeprom_data[EEPROM_SIZE] {};
+
+ static uORB::Publication _esc_eeprom_read_pub;
+};
diff --git a/src/drivers/dshot/esc/ESCSettingsInterface.h b/src/drivers/dshot/esc/ESCSettingsInterface.h
new file mode 100644
index 0000000000..fe5b6db371
--- /dev/null
+++ b/src/drivers/dshot/esc/ESCSettingsInterface.h
@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2026 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include
+
+enum class ESCType : uint8_t {
+ Unknown = 0,
+ AM32 = 1,
+};
+
+class ESCSettingsInterface
+{
+public:
+ virtual ~ESCSettingsInterface() = default;
+
+ virtual bool decodeInfoResponse(const uint8_t *buf, int size) = 0;
+ virtual int getExpectedResponseSize() = 0;
+ virtual void publish_latest() { /* no-op */};
+
+ // TODO: function to read data
+ // TODO: function to write data
+};
diff --git a/src/drivers/dshot/module.yaml b/src/drivers/dshot/module.yaml
index d25809f87c..3dda5f4dcf 100644
--- a/src/drivers/dshot/module.yaml
+++ b/src/drivers/dshot/module.yaml
@@ -8,6 +8,15 @@ serial_config:
parameters:
- group: DShot
definitions:
+ DSHOT_ESC_TYPE:
+ description:
+ short: ESC Type
+ long: The ESC firmware type
+ type: enum
+ values:
+ 0: Unknown
+ 1: AM32
+ default: 0
DSHOT_MIN:
description:
short: Minimum DShot Motor Output
@@ -33,13 +42,13 @@ parameters:
When mixer outputs 1000 or value inside DSHOT 3D deadband, DShot 0 is sent.
type: boolean
default: 0
- DSHOT_BIDIR_EN:
+ DSHOT_BIDIR_EDT:
description:
- short: Enable bidirectional DShot
+ short: Enable Extended DShot Telemetry
long: |
- This parameter enables bidirectional DShot which provides RPM feedback.
- Note that this requires ESCs that support bidirectional DSHot, e.g. BlHeli32.
- This is not the same as DShot telemetry which requires an additional serial connection.
+ This parameter enables Extended DShot Telemetry which allows transmission of
+ additional telemetry within the eRPM frame. The EDT data is interleaved with
+ the eRPM frames at a low rate.
type: boolean
default: 0
reboot_required: true
@@ -63,14 +72,18 @@ parameters:
min: 0
max: 1000
default: 1000
- MOT_POLE_COUNT: # only used by dshot so far, so keep it under the dshot group
+ DSHOT_MOT_POL${i}:
description:
- short: Number of magnetic poles of the motors
+ short: Number of magnetic poles of motor ${i}
long: |
- Specify the number of magnetic poles of the motors.
- It is required to compute the RPM value from the eRPM returned with the ESC telemetry.
-
- Either get the number from the motor spec sheet or count the magnets on the bell of the motor (not the stator magnets).
+ Number of magnetic poles for motor ${i}.
+ Required to compute RPM from the eRPM returned by ESC telemetry.
+ Either get the number from the motor spec sheet or count the magnets
+ on the bell of the motor (not the stator magnets).
Typical motors for 5 inch props have 14 poles.
type: int32
default: 14
+ min: 2
+ max: 400
+ num_instances: 12
+ instance_start: 1
diff --git a/src/drivers/pwm_out/module.yaml b/src/drivers/pwm_out/module.yaml
index c788296e3d..836efc043b 100644
--- a/src/drivers/pwm_out/module.yaml
+++ b/src/drivers/pwm_out/module.yaml
@@ -21,6 +21,9 @@ actuator_output:
type: enum
default: 400
values:
+ -8: BDShot150
+ -7: BDShot300
+ -6: BDShot600
-5: DShot150
-4: DShot300
-3: DShot600
diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp
index b19468ff54..a40e842606 100644
--- a/src/drivers/uavcan/actuators/esc.cpp
+++ b/src/drivers/uavcan/actuators/esc.cpp
@@ -153,9 +153,9 @@ void UavcanEscController::esc_status_extended_sub_cb(const uavcan::ReceivedDataS
}
}
-uint8_t UavcanEscController::check_escs_status()
+uint16_t UavcanEscController::check_escs_status()
{
- int esc_status_flags = 0;
+ uint16_t esc_status_flags = 0;
const hrt_abstime now = hrt_absolute_time();
for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp
index 8adcafe1b1..27e136e498 100644
--- a/src/drivers/uavcan/actuators/esc.hpp
+++ b/src/drivers/uavcan/actuators/esc.hpp
@@ -99,7 +99,7 @@ private:
/**
* Checks all the ESCs freshness based on timestamp, if an ESC exceeds the timeout then is flagged offline.
*/
- uint8_t check_escs_status();
+ uint16_t check_escs_status();
/**
* Gets failure flags for a specific ESC
diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp
index 08e1a58e1c..8f98df5e65 100644
--- a/src/lib/mixer_module/mixer_module.hpp
+++ b/src/lib/mixer_module/mixer_module.hpp
@@ -140,6 +140,8 @@ public:
OutputFunction outputFunction(int index) const { return _function_assignment[index]; }
+ bool isMotor(int index) const { return isFunctionSet(index) && (_function_assignment[index] >= OutputFunction::Motor1) && (_function_assignment[index] <= OutputFunction::Motor12); }
+
/**
* Call this regularly from Run(). It will call interface.updateOutputs().
* @return true if outputs were updated
diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp
index 837a2d68ba..7f1275b3f5 100644
--- a/src/lib/parameters/param_translation.cpp
+++ b/src/lib/parameters/param_translation.cpp
@@ -34,6 +34,7 @@
#include "param_translation.h"
+#include
#include
#include
#include
@@ -177,5 +178,19 @@ param_modify_on_import_ret param_modify_on_import(bson_node_t node)
}
}
+ // 2026-03-11: translate MOT_POLE_COUNT to per-motor DSHOT_MOT_POL1-12
+ {
+ if ((node->type == bson_type_t::BSON_INT32) && (strcmp("MOT_POLE_COUNT", node->name) == 0)) {
+ for (int i = 1; i <= 12; i++) {
+ char name[20];
+ snprintf(name, sizeof(name), "DSHOT_MOT_POL%d", i);
+ param_set(param_find(name), &node->i32);
+ }
+
+ PX4_INFO("migrating %s -> DSHOT_MOT_POL1-12 (value=%" PRId32 ")", "MOT_POLE_COUNT", node->i32);
+ return param_modify_on_import_ret::PARAM_SKIP_IMPORT;
+ }
+ }
+
return param_modify_on_import_ret::PARAM_NOT_MODIFIED;
}
diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 0a7a05172c..bd1ac421bc 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -1603,6 +1603,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR:
+ case vehicle_command_s::VEHICLE_CMD_ESC_REQUEST_EEPROM:
case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE:
case vehicle_command_s::VEHICLE_CMD_DO_WINCH:
case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER:
diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp
index 0810dfc9a1..4d9017f178 100644
--- a/src/modules/logger/logged_topics.cpp
+++ b/src/modules/logger/logged_topics.cpp
@@ -64,7 +64,7 @@ void LoggedTopics::add_default_topics()
add_optional_topic("external_ins_attitude");
add_optional_topic("external_ins_global_position");
add_optional_topic("external_ins_local_position");
- add_topic("esc_status");
+ add_topic("esc_status", 100);
add_topic("failure_detector_status", 100);
add_topic("failsafe_flags");
add_optional_topic("follow_target", 500);
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 719cb9160d..802bc40858 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1462,6 +1462,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("EFI_STATUS", 2.0f);
configure_stream_local("ESC_INFO", 1.0f);
configure_stream_local("ESC_STATUS", 1.0f);
+#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
+ configure_stream_local("ESC_EEPROM", unlimited_rate);
+#endif
configure_stream_local("ESTIMATOR_STATUS", 0.5f);
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 5.0f);
@@ -1526,6 +1529,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("DISTANCE_SENSOR", 10.0f);
configure_stream_local("ESC_INFO", 10.0f);
configure_stream_local("ESC_STATUS", 10.0f);
+#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
+ configure_stream_local("ESC_EEPROM", unlimited_rate);
+#endif
configure_stream_local("MOUNT_ORIENTATION", 10.0f);
configure_stream_local("OBSTACLE_DISTANCE", 10.0f);
configure_stream_local("ODOMETRY", 30.0f);
@@ -1704,6 +1710,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("EFI_STATUS", 10.0f);
configure_stream_local("ESC_INFO", 10.0f);
configure_stream_local("ESC_STATUS", 10.0f);
+#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
+ configure_stream_local("ESC_EEPROM", unlimited_rate);
+#endif
configure_stream_local("ESTIMATOR_STATUS", 5.0f);
configure_stream_local("EXTENDED_SYS_STATE", 2.0f);
configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
@@ -1799,6 +1808,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
configure_stream_local("ESC_INFO", 1.0f);
configure_stream_local("ESC_STATUS", 5.0f);
+#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
+ configure_stream_local("ESC_EEPROM", unlimited_rate);
+#endif
configure_stream_local("ADSB_VEHICLE", 5.f);
configure_stream_local("ATTITUDE_TARGET", 2.0f);
@@ -1865,6 +1877,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 2.0f);
configure_stream_local("ESC_INFO", 1.0f);
configure_stream_local("ESC_STATUS", 2.0f);
+#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
+ configure_stream_local("ESC_EEPROM", unlimited_rate);
+#endif
configure_stream_local("ADSB_VEHICLE", 1.0f);
configure_stream_local("ATTITUDE_TARGET", 0.5f);
configure_stream_local("AVAILABLE_MODES", 0.3f);
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 558b3a0e3f..e05cfa99c5 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -135,6 +135,10 @@
#include "streams/CURRENT_MODE.hpp"
#endif
+#ifdef MAVLINK_MSG_ID_ESC_EEPROM // Only defined if development.xml is used
+#include "streams/ESC_EEPROM.hpp"
+#endif
+
#if !defined(CONSTRAINED_FLASH)
# include "streams/ADSB_VEHICLE.hpp"
# include "streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp"
@@ -466,6 +470,9 @@ static const StreamListItem streams_list[] = {
#if defined(ESC_STATUS_HPP)
create_stream_list_item(),
#endif // ESC_STATUS_HPP
+#if defined(ESC_EEPROM_HPP)
+ create_stream_list_item(),
+#endif // ESC_EEPROM_HPP
#if defined(AUTOPILOT_VERSION_HPP)
create_stream_list_item(),
#endif // AUTOPILOT_VERSION_HPP
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index ad5cd15069..151a091ef2 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -333,6 +333,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
case MAVLINK_MSG_ID_SET_VELOCITY_LIMITS:
handle_message_set_velocity_limits(msg);
break;
+
+#endif
+
+#if defined(MAVLINK_MSG_ID_ESC_EEPROM) // For now only defined if development.xml is used
+
+ case MAVLINK_MSG_ID_ESC_EEPROM:
+ handle_message_esc_eeprom(msg);
+ break;
#endif
default:
@@ -598,14 +606,24 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
uint16_t message_id = (uint16_t)roundf(vehicle_command.param1);
- if (message_id == MAVLINK_MSG_ID_MESSAGE_INTERVAL) {
- get_message_interval((int)(cmd_mavlink.param2 + 0.5f));
+#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
- } else {
- result = handle_request_message_command(message_id,
- vehicle_command.param2, vehicle_command.param3, vehicle_command.param4,
- vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
- }
+ // Translate ESC_EEPROM request into the PX4 internal command so the DShot driver handles it
+ if (message_id == MAVLINK_MSG_ID_ESC_EEPROM) {
+ vehicle_command_s eeprom_cmd = vehicle_command;
+ eeprom_cmd.command = vehicle_command_s::VEHICLE_CMD_ESC_REQUEST_EEPROM;
+ _cmd_pub.publish(eeprom_cmd);
+
+ } else
+#endif
+ if (message_id == MAVLINK_MSG_ID_MESSAGE_INTERVAL) {
+ get_message_interval((int)(cmd_mavlink.param2 + 0.5f));
+
+ } else {
+ result = handle_request_message_command(message_id,
+ vehicle_command.param2, vehicle_command.param3, vehicle_command.param4,
+ vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
+ }
} else if (cmd_mavlink.command == MAV_CMD_INJECT_FAILURE) {
if (_mavlink.failure_injection_enabled()) {
@@ -1308,6 +1326,48 @@ void MavlinkReceiver::handle_message_set_velocity_limits(mavlink_message_t *msg)
}
#endif // MAVLINK_MSG_ID_SET_VELOCITY_LIMITS
+#if defined(MAVLINK_MSG_ID_ESC_EEPROM) // For now only defined if development.xml is used
+void
+MavlinkReceiver::handle_message_esc_eeprom(mavlink_message_t *msg)
+{
+ mavlink_esc_eeprom_t message;
+ mavlink_msg_esc_eeprom_decode(msg, &message);
+
+ esc_eeprom_write_s eeprom{};
+ eeprom.timestamp = hrt_absolute_time();
+ eeprom.firmware = message.firmware;
+ eeprom.index = message.esc_index;
+
+ if (eeprom.index != 255 && eeprom.index >= esc_status_s::CONNECTED_ESC_MAX) {
+ PX4_ERR("ESC EEPROM: invalid esc_index %u", eeprom.index);
+ return;
+ }
+
+ uint8_t min_length = sizeof(eeprom.data);
+ int length = message.length;
+
+ if (length > min_length) {
+ length = min_length;
+ }
+
+ for (int i = 0; i < length && i < min_length; i++) {
+ int mask_index = i / 32; // Which uint32_t in the write_mask array
+ int bit_index = i % 32; // Which bit within that uint32_t
+
+ if (message.write_mask[mask_index] & (1U << bit_index)) {
+ eeprom.data[i] = message.data[i];
+ }
+ }
+
+ eeprom.length = length;
+ memcpy(eeprom.write_mask, message.write_mask, sizeof(eeprom.write_mask));
+
+ PX4_DEBUG("ESC EEPROM write request for ESC%d", eeprom.index + 1);
+
+ _esc_eeprom_write_pub.publish(eeprom);
+}
+#endif // MAVLINK_MSG_ID_ESC_EEPROM
+
void
MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
{
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index a21a9f313c..da10db0c69 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -64,6 +64,10 @@
#include
#include
#include
+#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
+#include
+#endif
+#include
#include
#include
#include
@@ -203,6 +207,9 @@ private:
void handle_message_utm_global_position(mavlink_message_t *msg);
#if defined(MAVLINK_MSG_ID_SET_VELOCITY_LIMITS) // For now only defined if development.xml is used
void handle_message_set_velocity_limits(mavlink_message_t *msg);
+#endif
+#if defined(MAVLINK_MSG_ID_ESC_EEPROM) // For now only defined if development.xml is used
+ void handle_message_esc_eeprom(mavlink_message_t *msg);
#endif
void handle_message_vision_position_estimate(mavlink_message_t *msg);
void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg);
@@ -330,6 +337,11 @@ private:
uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
uORB::Publication _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
+
+#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
+ uORB::Publication _esc_eeprom_write_pub {ORB_ID(esc_eeprom_write)};
+#endif
+
#if !defined(CONSTRAINED_FLASH)
uORB::Publication _debug_array_pub {ORB_ID(debug_array)};
uORB::Publication _debug_key_value_pub{ORB_ID(debug_key_value)};
diff --git a/src/modules/mavlink/streams/ESC_EEPROM.hpp b/src/modules/mavlink/streams/ESC_EEPROM.hpp
new file mode 100644
index 0000000000..9f078887c7
--- /dev/null
+++ b/src/modules/mavlink/streams/ESC_EEPROM.hpp
@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2026 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.
+ *
+ ****************************************************************************/
+
+#ifndef ESC_EEPROM_HPP
+#define ESC_EEPROM_HPP
+
+#include
+
+class MavlinkStreamEscEeprom : public MavlinkStream
+{
+public:
+ static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamEscEeprom(mavlink); }
+
+ static constexpr const char *get_name_static() { return "ESC_EEPROM"; }
+ static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ESC_EEPROM; }
+
+ const char *get_name() const override { return get_name_static(); }
+ uint16_t get_id() override { return get_id_static(); }
+
+ unsigned get_size() override
+ {
+ return _esc_eeprom_read_sub.advertised() ? MAVLINK_MSG_ID_ESC_EEPROM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
+ }
+
+private:
+ explicit MavlinkStreamEscEeprom(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+ uORB::Subscription _esc_eeprom_read_sub{ORB_ID(esc_eeprom_read)};
+
+ bool emit_message(bool force)
+ {
+ esc_eeprom_read_s eeprom = {};
+
+ if (_esc_eeprom_read_sub.update(&eeprom) || force) {
+ mavlink_esc_eeprom_t msg = {};
+ msg.firmware = eeprom.firmware;
+ msg.esc_index = eeprom.index;
+ msg.msg_index = 0;
+ msg.msg_count = 1;
+ size_t copy_len = eeprom.length < sizeof(eeprom.data) ? eeprom.length : sizeof(eeprom.data);
+ memcpy(msg.data, eeprom.data, copy_len);
+ msg.length = copy_len;
+
+ mavlink_msg_esc_eeprom_send_struct(_mavlink->get_channel(), &msg);
+
+ return true;
+ }
+
+ return false;
+ }
+
+ bool send() override
+ {
+ return emit_message(false);
+ }
+
+};
+
+#endif // ESC_EEPROM_HPP
diff --git a/src/modules/mavlink/streams/ESC_INFO.hpp b/src/modules/mavlink/streams/ESC_INFO.hpp
index 8cdfbecdaa..57dda6dbcd 100644
--- a/src/modules/mavlink/streams/ESC_INFO.hpp
+++ b/src/modules/mavlink/streams/ESC_INFO.hpp
@@ -52,7 +52,7 @@ public:
unsigned get_size() override
{
static constexpr unsigned message_size = MAVLINK_MSG_ID_ESC_INFO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
- return _esc_status_subs.advertised_count() * message_size;
+ return MAX_NUM_MSGS * message_size;
}
private:
@@ -66,106 +66,101 @@ private:
static constexpr hrt_abstime ESC_TIMEOUT = 100000;
- struct EscOutputInterfaceInfo {
- uint16_t counter;
- uint8_t esc_count;
- uint8_t esc_connectiontype;
- uint8_t esc_online_flags;
- };
-
struct EscInfo {
hrt_abstime timestamp;
uint16_t failure_flags;
uint32_t error_count;
int16_t temperature;
+ uint8_t connectiontype;
bool online;
};
- int _total_esc_count = {};
- EscOutputInterfaceInfo _interface[MAX_NUM_MSGS] = {};
+ uint16_t _total_counter = {};
+ uint8_t _total_esc_count = {};
EscInfo _escs[MAX_ESC_OUTPUTS] = {};
+ uint16_t _instance_counter[ORB_MULTI_MAX_INSTANCES] = {};
+ uint8_t _instance_esc_count[ORB_MULTI_MAX_INSTANCES] = {};
+
void update_data() override
{
- int subscriber_count = math::min(_esc_status_subs.size(), MAX_NUM_MSGS);
-
- for (int i = 0; i < subscriber_count; i++) {
+ for (int i = 0; i < _esc_status_subs.size(); i++) {
esc_status_s esc = {};
if (_esc_status_subs[i].update(&esc)) {
- _interface[i].counter = esc.counter;
- _interface[i].esc_count = esc.esc_count;
- _interface[i].esc_connectiontype = esc.esc_connectiontype;
+ _instance_counter[i] = esc.counter;
+ _instance_esc_count[i] = esc.esc_count;
- // Capture online_flags, we will map from index to motor number
- uint8_t online_flags = esc.esc_online_flags;
- _interface[i].esc_online_flags = 0;
+ uint16_t online_flags = esc.esc_online_flags;
for (int j = 0; j < esc_status_s::CONNECTED_ESC_MAX; j++) {
bool is_motor = math::isInRange(esc.esc[j].actuator_function,
esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX);
if (is_motor) {
- // Map OutputFunction number to index
int index = (int)esc.esc[j].actuator_function - esc_report_s::ACTUATOR_FUNCTION_MOTOR1;
- _escs[index].online = online_flags & (1 << j);
- _escs[index].failure_flags = esc.esc[j].failures;
- _escs[index].error_count = esc.esc[j].esc_errorcount;
- _escs[index].timestamp = esc.esc[j].timestamp;
- _escs[index].temperature = esc.esc[j].esc_temperature * 100.f;
+
+ if (index >= 0 && index < MAX_ESC_OUTPUTS) {
+ _escs[index].online = online_flags & (1 << j);
+ _escs[index].failure_flags = esc.esc[j].failures;
+ _escs[index].error_count = esc.esc[j].esc_errorcount;
+ _escs[index].timestamp = esc.esc[j].timestamp;
+ _escs[index].temperature = esc.esc[j].esc_temperature * 100.f;
+ _escs[index].connectiontype = esc.esc_connectiontype;
+ }
}
}
}
}
- int count = 0;
+ _total_counter = 0;
+ _total_esc_count = 0;
- for (int i = 0; i < MAX_NUM_MSGS; i++) {
- count += _interface[i].esc_count;
+ for (int i = 0; i < _esc_status_subs.size(); i++) {
+ _total_counter += _instance_counter[i];
+ _total_esc_count += _instance_esc_count[i];
}
-
- _total_esc_count = count;
}
bool send() override
{
- bool updated = false;
+ if (_total_esc_count == 0) {
+ return false;
+ }
- for (int i = 0; i < MAX_NUM_MSGS; i++) {
+ const int num_msgs = math::min((_total_esc_count + ESCS_PER_MSG - 1) / ESCS_PER_MSG, (int)MAX_NUM_MSGS);
+ const hrt_abstime now = hrt_absolute_time();
- hrt_abstime now = hrt_absolute_time();
+ for (int i = 0; i < num_msgs; i++) {
mavlink_esc_info_t msg = {};
msg.index = i * ESCS_PER_MSG;
msg.time_usec = now;
- msg.counter = _interface[i].counter;
+ msg.counter = _total_counter;
msg.count = _total_esc_count;
- msg.connection_type = _interface[i].esc_connectiontype;
- msg.info = _interface[i].esc_online_flags;
-
- bool atleast_one_esc_updated = false;
+ msg.connection_type = 0;
+ msg.info = 0;
for (int j = 0; j < ESCS_PER_MSG; j++) {
EscInfo &esc = _escs[i * ESCS_PER_MSG + j];
- msg.info |= (esc.online << j);
-
if ((esc.timestamp != 0) && (esc.timestamp + ESC_TIMEOUT) > now) {
+ msg.info |= (esc.online << j);
msg.failure_flags[j] = esc.failure_flags;
msg.error_count[j] = esc.error_count;
msg.temperature[j] = esc.temperature;
- atleast_one_esc_updated = true;
+
+ if (msg.connection_type == 0) {
+ msg.connection_type = esc.connectiontype;
+ }
}
}
- if (atleast_one_esc_updated) {
- mavlink_msg_esc_info_send_struct(_mavlink->get_channel(), &msg);
- updated = true;
- }
+ mavlink_msg_esc_info_send_struct(_mavlink->get_channel(), &msg);
}
- return updated;
+ return true;
}
};
diff --git a/src/modules/mavlink/streams/ESC_STATUS.hpp b/src/modules/mavlink/streams/ESC_STATUS.hpp
index e316946a91..e5ff74ba29 100644
--- a/src/modules/mavlink/streams/ESC_STATUS.hpp
+++ b/src/modules/mavlink/streams/ESC_STATUS.hpp
@@ -52,7 +52,7 @@ public:
unsigned get_size() override
{
static constexpr unsigned message_size = MAVLINK_MSG_ID_ESC_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
- return _esc_status_subs.advertised_count() * message_size;
+ return MAX_NUM_MSGS * message_size;
}
private:
@@ -74,14 +74,17 @@ private:
EscStatus _escs[MAX_ESC_OUTPUTS] = {};
+ uint8_t _esc_count = {};
+ uint8_t _instance_esc_count[ORB_MULTI_MAX_INSTANCES] = {};
+
void update_data() override
{
- int subscriber_count = math::min(_esc_status_subs.size(), MAX_NUM_MSGS);
-
- for (int i = 0; i < subscriber_count; i++) {
+ for (int i = 0; i < _esc_status_subs.size(); i++) {
esc_status_s esc = {};
if (_esc_status_subs[i].update(&esc)) {
+ _instance_esc_count[i] = esc.esc_count;
+
for (int j = 0; j < esc_status_s::CONNECTED_ESC_MAX; j++) {
const bool is_motor = math::isInRange(esc.esc[j].actuator_function,
@@ -90,30 +93,40 @@ private:
if (is_motor) {
// Map OutputFunction number to index
int index = (int)esc.esc[j].actuator_function - esc_report_s::ACTUATOR_FUNCTION_MOTOR1;
- _escs[index].timestamp = esc.esc[j].timestamp;
- _escs[index].rpm = esc.esc[j].esc_rpm;
- _escs[index].voltage = esc.esc[j].esc_voltage;
- _escs[index].current = esc.esc[j].esc_current;
+
+ if (index >= 0 && index < MAX_ESC_OUTPUTS) {
+ _escs[index].timestamp = esc.esc[j].timestamp;
+ _escs[index].rpm = esc.esc[j].esc_rpm;
+ _escs[index].voltage = esc.esc[j].esc_voltage;
+ _escs[index].current = esc.esc[j].esc_current;
+ }
}
}
}
}
+
+ _esc_count = 0;
+
+ for (int i = 0; i < _esc_status_subs.size(); i++) {
+ _esc_count += _instance_esc_count[i];
+ }
}
bool send() override
{
- bool updated = false;
+ if (_esc_count == 0) {
+ return false;
+ }
- for (int i = 0; i < MAX_NUM_MSGS; i++) {
+ const int num_msgs = math::min((_esc_count + ESCS_PER_MSG - 1) / ESCS_PER_MSG, (int)MAX_NUM_MSGS);
+ const hrt_abstime now = hrt_absolute_time();
- hrt_abstime now = hrt_absolute_time();
+ for (int i = 0; i < num_msgs; i++) {
mavlink_esc_status_t msg = {};
msg.index = i * ESCS_PER_MSG;
msg.time_usec = now;
- bool atleast_one_esc_updated = false;
-
for (int j = 0; j < ESCS_PER_MSG; j++) {
EscStatus &esc = _escs[i * ESCS_PER_MSG + j];
@@ -122,17 +135,13 @@ private:
msg.rpm[j] = esc.rpm;
msg.voltage[j] = esc.voltage;
msg.current[j] = esc.current;
- atleast_one_esc_updated = true;
}
}
- if (atleast_one_esc_updated) {
- mavlink_msg_esc_status_send_struct(_mavlink->get_channel(), &msg);
- updated = true;
- }
+ mavlink_msg_esc_status_send_struct(_mavlink->get_channel(), &msg);
}
- return updated;
+ return true;
}
};