mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
feat(dshot): Extended Telemetry and EEPROM support
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
This commit is contained in:
Jacob Dahl
GitHub
parent
c9ee06ff17
commit
f00e46f618
1
boards/ark/fmu-v6x/mavlink-dev.px4board
Normal file
1
boards/ark/fmu-v6x/mavlink-dev.px4board
Normal file
@ -0,0 +1 @@
|
||||
CONFIG_MAVLINK_DIALECT="development"
|
||||
@ -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
|
||||
|
||||
19
boards/ark/fpv/init/rc.board_airframes
Normal file
19
boards/ark/fpv/init/rc.board_airframes
Normal file
@ -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
|
||||
1
boards/ark/fpv/mavlink-dev.px4board
Normal file
1
boards/ark/fpv/mavlink-dev.px4board
Normal file
@ -0,0 +1 @@
|
||||
CONFIG_MAVLINK_DIALECT="development"
|
||||
@ -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
|
||||
|
||||
1
boards/ark/pi6x/mavlink-dev.px4board
Normal file
1
boards/ark/pi6x/mavlink-dev.px4board
Normal file
@ -0,0 +1 @@
|
||||
CONFIG_MAVLINK_DIALECT="development"
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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}),
|
||||
|
||||
@ -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:
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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)
|
||||
|
||||
<Badge type="tip" text="PX4 v1.16" />
|
||||
|
||||
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.
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -69,6 +69,8 @@ set(msg_files
|
||||
DistanceSensorModeChangeRequest.msg
|
||||
DronecanNodeStatus.msg
|
||||
Ekf2Timestamps.msg
|
||||
EscEepromRead.msg
|
||||
EscEepromWrite.msg
|
||||
EscReport.msg
|
||||
EscStatus.msg
|
||||
EstimatorAidSource1d.msg
|
||||
|
||||
7
msg/EscEepromRead.msg
Normal file
7
msg/EscEepromRead.msg
Normal file
@ -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
|
||||
8
msg/EscEepromWrite.msg
Normal file
8
msg/EscEepromWrite.msg
Normal file
@ -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
|
||||
@ -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!
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -83,7 +83,6 @@ void VertiqTelemetryManager::StartPublishing(uORB::Publication<esc_status_s> *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
|
||||
|
||||
@ -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);
|
||||
*/
|
||||
}
|
||||
}
|
||||
|
||||
@ -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 <px4_platform_common/defines.h>
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -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 <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/esc_eeprom_write.h>
|
||||
|
||||
#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_s> 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<Command *> _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<vehicle_command_ack_s> _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_s> _esc_status_pub{ORB_ID(esc_status)};
|
||||
uORB::Publication<vehicle_command_ack_s> _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<px4::params::DSHOT_ESC_TYPE>) _param_dshot_esc_type,
|
||||
(ParamFloat<px4::params::DSHOT_MIN>) _param_dshot_min,
|
||||
(ParamBool<px4::params::DSHOT_3D_ENABLE>) _param_dshot_3d_enable,
|
||||
(ParamInt<px4::params::DSHOT_3D_DEAD_H>) _param_dshot_3d_dead_h,
|
||||
(ParamInt<px4::params::DSHOT_3D_DEAD_L>) _param_dshot_3d_dead_l,
|
||||
(ParamInt<px4::params::MOT_POLE_COUNT>) _param_mot_pole_count,
|
||||
(ParamBool<px4::params::DSHOT_BIDIR_EN>) _param_bidirectional_enable
|
||||
(ParamBool<px4::params::DSHOT_BIDIR_EDT>) _param_dshot_bidir_edt
|
||||
)
|
||||
};
|
||||
|
||||
93
src/drivers/dshot/DShotCommon.h
Normal file
93
src/drivers/dshot/DShotCommon.h
Normal file
@ -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 <drivers/drv_hrt.h>
|
||||
#include <board_config.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
@ -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 <px4_platform_common/log.h>
|
||||
#include <drivers/drv_dshot.h>
|
||||
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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 <px4_platform_common/Serial.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#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};
|
||||
};
|
||||
|
||||
85
src/drivers/dshot/esc/AM32Settings.cpp
Normal file
85
src/drivers/dshot/esc/AM32Settings.cpp
Normal file
@ -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 <px4_platform_common/log.h>
|
||||
|
||||
static constexpr int RESPONSE_SIZE = EEPROM_SIZE + 1; // 48B data + 1B CRC
|
||||
|
||||
uORB::Publication<esc_eeprom_read_s> 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;
|
||||
}
|
||||
57
src/drivers/dshot/esc/AM32Settings.h
Normal file
57
src/drivers/dshot/esc/AM32Settings.h
Normal file
@ -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 <uORB/Publication.hpp>
|
||||
#include <uORB/topics/esc_eeprom_read.h>
|
||||
|
||||
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_s> _esc_eeprom_read_pub;
|
||||
};
|
||||
54
src/drivers/dshot/esc/ESCSettingsInterface.h
Normal file
54
src/drivers/dshot/esc/ESCSettingsInterface.h
Normal file
@ -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 <cstdint>
|
||||
|
||||
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
|
||||
};
|
||||
@ -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
|
||||
|
||||
@ -21,6 +21,9 @@ actuator_output:
|
||||
type: enum
|
||||
default: 400
|
||||
values:
|
||||
-8: BDShot150
|
||||
-7: BDShot300
|
||||
-6: BDShot600
|
||||
-5: DShot150
|
||||
-4: DShot300
|
||||
-3: DShot600
|
||||
|
||||
@ -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++) {
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -34,6 +34,7 @@
|
||||
#include "param_translation.h"
|
||||
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <lib/drivers/device/Device.hpp>
|
||||
#include <drivers/drv_sensor.h>
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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<MavlinkStreamESCStatus>(),
|
||||
#endif // ESC_STATUS_HPP
|
||||
#if defined(ESC_EEPROM_HPP)
|
||||
create_stream_list_item<MavlinkStreamEscEeprom>(),
|
||||
#endif // ESC_EEPROM_HPP
|
||||
#if defined(AUTOPILOT_VERSION_HPP)
|
||||
create_stream_list_item<MavlinkStreamAutopilotVersion>(),
|
||||
#endif // AUTOPILOT_VERSION_HPP
|
||||
|
||||
@ -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)
|
||||
{
|
||||
|
||||
@ -64,6 +64,10 @@
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/autotune_attitude_control_status.h>
|
||||
#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
|
||||
#include <uORB/topics/esc_eeprom_write.h>
|
||||
#endif
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/camera_status.h>
|
||||
#include <uORB/topics/cellular_status.h>
|
||||
@ -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<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
|
||||
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
|
||||
uORB::Publication<vehicle_rates_setpoint_s> _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
|
||||
|
||||
#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
|
||||
uORB::Publication<esc_eeprom_write_s> _esc_eeprom_write_pub {ORB_ID(esc_eeprom_write)};
|
||||
#endif
|
||||
|
||||
#if !defined(CONSTRAINED_FLASH)
|
||||
uORB::Publication<debug_array_s> _debug_array_pub {ORB_ID(debug_array)};
|
||||
uORB::Publication<debug_key_value_s> _debug_key_value_pub{ORB_ID(debug_key_value)};
|
||||
|
||||
89
src/modules/mavlink/streams/ESC_EEPROM.hpp
Normal file
89
src/modules/mavlink/streams/ESC_EEPROM.hpp
Normal file
@ -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 <uORB/topics/esc_eeprom_read.h>
|
||||
|
||||
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
|
||||
@ -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;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user