fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-11 07:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

rebased

Browse Source
This commit is contained in:
Jacob Dahl
2026-01-27 15:39:49 -09:00
parent 9be7585add
commit b93cb96c4f
21 changed files with 2142 additions and 1302 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/Am32EepromRead.msg
+5
View File
@@ -0,0 +1,5 @@
uint64 timestamp # time since system start (microseconds)
uint8 index # Index of the ESC (0 = ESC1, 1 = ESC2, etc.)
uint8[48] data # Raw AM32 EEPROM data
uint8 ORB_QUEUE_LENGTH = 8 # To support 8 queued up reponses
msg/Am32EepromWrite.msg
+6
View File
@@ -0,0 +1,6 @@
uint64 timestamp # time since system start (microseconds)
uint8 index # Index of the ESC (0 = ESC1, 1 = ESC2, etc, 255 = All)
uint8[48] data # Raw AM32 EEPROM data
uint32[2] write_mask # Bitmask indicating which bytes in the data array should be written (max 64 values, am32 is currently 48)
uint8 ORB_QUEUE_LENGTH = 8 # To support 8 queued up requests
msg/CMakeLists.txt
+2
View File
@@ -69,6 +69,8 @@ set(msg_files
DistanceSensorModeChangeRequest.msg
DronecanNodeStatus.msg
Ekf2Timestamps.msg
Am32EepromRead.msg
Am32EepromWrite.msg
EscReport.msg
EscStatus.msg
EstimatorAidSource1d.msg
msg/EscReport.msg
+24 -23
View File
@@ -1,15 +1,16 @@
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
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 # time since system start (microseconds)
uint8 esc_state # State of ESC - depend on Vendor
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
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
uint8 actuator_function # actuator output function (one of Motor1...MotorN)
uint8 esc_state # State of ESC - depend on Vendor
uint8 actuator_function # actuator output function (one of Motor1...MotorN)
uint8 ACTUATOR_FUNCTION_MOTOR1 = 101
uint8 ACTUATOR_FUNCTION_MOTOR2 = 102
@@ -24,17 +25,17 @@ uint8 ACTUATOR_FUNCTION_MOTOR10 = 110
uint8 ACTUATOR_FUNCTION_MOTOR11 = 111
uint8 ACTUATOR_FUNCTION_MOTOR12 = 112
uint16 failures # Bitmask to indicate the internal ESC faults
int8 esc_power # Applied power 0-100 in % (negative values reserved)
uint16 failures # Bitmask to indicate the internal ESC faults
int8 esc_power # Applied power 0-100 in % (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!
msg/EscStatus.msg
+13 -13
View File
@@ -1,19 +1,19 @@
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 # time since system start (microseconds)
uint8 CONNECTED_ESC_MAX = 8 # The number of ESCs supported. Current (Q2/2013) we support 8 ESCs
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 # how ESCs connected to the system
uint8 esc_online_flags # Bitmask indicating which ESC is online/offline
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
@@ -23,6 +23,6 @@ uint8 esc_online_flags # Bitmask indicating which ESC is online/offline
# esc_online_flags bit 6 : Set to 1 if ESC6 is online
# esc_online_flags bit 7 : Set to 1 if ESC7 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.
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.
EscReport[8] esc
msg/versioned/VehicleCommand.msg
+1
View File
@@ -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_AM32_REQUEST_EEPROM = 312 # Request EEPROM data from an AM32 ESC. |esc index|
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.
platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c
+17 -4
View File
@@ -321,8 +321,11 @@ 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, unsigned dshot_pwm_freq, bool bdshot_enable,
bool enable_extended_dshot_telemetry)
{
(void)enable_extended_dshot_telemetry; // 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;
@@ -497,7 +500,7 @@ void up_bdshot_erpm(void)
}
int up_bdshot_num_erpm_ready(void)
int up_bdshot_num_channels_ready(void)
{
int num_ready = 0;
@@ -512,6 +515,10 @@ int up_bdshot_num_erpm_ready(void)
return num_ready;
}
int up_bdshot_num_errors(uint8_t channel)
{
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)
{
@@ -523,7 +530,13 @@ 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_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);
@@ -538,7 +551,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);
platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
+430 -222
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2024 PX4 Development Team. All rights reserved.
* Author: Igor Misic <igy1000mb@gmail.com>
* 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
@@ -97,7 +96,8 @@ static void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void
static void capture_complete_callback(void *arg);
static void process_capture_results(uint8_t timer_index, uint8_t channel_index);
static unsigned calculate_period(uint8_t timer_index, uint8_t channel_index);
static uint32_t convert_edge_intervals_to_bitstream(uint8_t channel_index);
static void decode_dshot_telemetry(uint32_t payload, struct BDShotTelemetry *packet);
// Timer configuration struct
typedef struct timer_config_t {
@@ -122,24 +122,65 @@ static uint32_t *dshot_output_buffer[MAX_IO_TIMERS] = {};
static uint16_t dshot_capture_buffer[MAX_NUM_CHANNELS_PER_TIMER][CHANNEL_CAPTURE_BUFF_SIZE]
px4_cache_aligned_data() = {};
static bool _bidirectional = false;
static const uint32_t gcr_decode[32] = {
0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
0x0, 0x9, 0xA, 0xB, 0x0, 0xD, 0xE, 0xF,
0x0, 0x0, 0x2, 0x3, 0x0, 0x5, 0x6, 0x7,
0x0, 0x0, 0x8, 0x1, 0x0, 0x4, 0xC, 0x0
};
// Indicates when the bdshot capture cycle is finished. This is necessary since the captured data is
// processed after a fixed delay in an hrt callback. System jitter can delay the firing of the hrt callback
// and thus delay the processing of the data. This should never happen in a properly working system, as the
// jitter would have to be longer than the control allocator update interval. A warning is issued if this
// ever does occur.
static bool _bdshot_cycle_complete = true;
static bool _bdshot_enabled = false;
static bool _extended_dshot_telem = false;
static uint8_t _bidi_timer_index = 0; // TODO: BDSHOT_TIM param to select timer index?
static uint32_t _dshot_frequency = 0;
// eRPM data for channels on the singular timer
static int32_t _erpms[MAX_TIMER_IO_CHANNELS] = {};
static bool _erpms_ready[MAX_TIMER_IO_CHANNELS] = {};
// Online flags, set if ESC is reponding with valid BDShot frames
#define BDSHOT_OFFLINE_COUNT 200
static bool _bdshot_online[MAX_TIMER_IO_CHANNELS] = {};
static bool _bdshot_processed[MAX_TIMER_IO_CHANNELS] = {};
static int _consecutive_failures[MAX_TIMER_IO_CHANNELS] = {};
static int _consecutive_successes[MAX_TIMER_IO_CHANNELS] = {};
// ePRM data
typedef struct erpm_data_t {
int32_t erpm;
bool ready;
float rate_hz;
uint64_t last_timestamp;
} erpm_data_t;
erpm_data_t _erpms[MAX_TIMER_IO_CHANNELS] = {};
// EDT data
typedef struct edt_data_t {
uint8_t value;
bool ready;
float rate_hz;
uint64_t last_timestamp;
} edt_data_t;
edt_data_t _edt_temp[MAX_TIMER_IO_CHANNELS] = {};
edt_data_t _edt_volt[MAX_TIMER_IO_CHANNELS] = {};
edt_data_t _edt_curr[MAX_TIMER_IO_CHANNELS] = {};
static float calculate_rate_hz(uint64_t last_timestamp, float last_rate_hz, uint64_t timestamp);
// hrt callback handle for captcomp post dma processing
static struct hrt_call _cc_call;
// decoding status for each channel
static uint32_t read_ok[MAX_NUM_CHANNELS_PER_TIMER] = {};
static uint32_t read_fail_nibble[MAX_NUM_CHANNELS_PER_TIMER] = {};
static uint32_t read_fail_crc[MAX_NUM_CHANNELS_PER_TIMER] = {};
static uint32_t read_fail_zero[MAX_NUM_CHANNELS_PER_TIMER] = {};
static perf_counter_t hrt_callback_perf = NULL;
static perf_counter_t capture_cycle_perf = NULL;
static perf_counter_t capture_cycle_perf2 = NULL;
static void init_timer_config(uint32_t channel_mask)
{
@@ -163,7 +204,7 @@ static void init_timer_config(uint32_t channel_mask)
}
// NOTE: only 1 timer can be used if Bidirectional DShot is enabled
if (_bidirectional && (timer_index != _bidi_timer_index)) {
if (_bdshot_enabled && (timer_index != _bidi_timer_index)) {
continue;
}
@@ -175,7 +216,7 @@ static void init_timer_config(uint32_t channel_mask)
timer_configs[timer_index].enabled_channels[timer_channel_index] = true;
// Mark timer as bidirectional
if (_bidirectional && timer_index == _bidi_timer_index) {
if (_bdshot_enabled && timer_index == _bidi_timer_index) {
timer_configs[timer_index].bidirectional = true;
}
}
@@ -198,7 +239,7 @@ static void init_timers_dma_up(void)
}
// NOTE: only 1 timer can be used if Bidirectional DShot is enabled
if (_bidirectional && (timer_index != _bidi_timer_index)) {
if (_bdshot_enabled && (timer_index != _bidi_timer_index)) {
continue;
}
@@ -216,7 +257,7 @@ static void init_timers_dma_up(void)
// Bidirectional DShot will free/allocate DMA stream on every update event. This is required
// in order to reconfigure the DMA stream between Timer Burst and CaptureCompare.
if (_bidirectional) {
if (_bdshot_enabled) {
// Free the allocated DMA channels
for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
if (timer_configs[timer_index].dma_handle != NULL) {
@@ -276,14 +317,18 @@ static int32_t init_timer_channels(uint8_t timer_index)
return channels_init_mask;
}
int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot)
int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool bdshot_enable,
bool enable_extended_dshot_telemetry)
{
_dshot_frequency = dshot_pwm_freq;
_bidirectional = enable_bidirectional_dshot;
_bdshot_enabled = bdshot_enable;
_extended_dshot_telem = enable_extended_dshot_telemetry;
if (_bidirectional) {
if (_bdshot_enabled) {
PX4_INFO("Bidirectional DShot enabled, only one timer will be used");
hrt_callback_perf = perf_alloc(PC_ELAPSED, "dshot: callback perf");
capture_cycle_perf = perf_alloc(PC_INTERVAL, "dshot: cycle perf");
capture_cycle_perf2 = perf_alloc(PC_INTERVAL, "dshot: cycle perf2");
}
// NOTE: if bidirectional is enabled only 1 timer can be used. This is because Burst mode uses 1 DMA channel per timer
@@ -333,8 +378,18 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi
// Kicks off a DMA transmit for each configured timer and the associated channels
void up_dshot_trigger()
{
if (_bdshot_enabled) {
if (!_bdshot_cycle_complete) {
PX4_WARN("Cylce not complete! Check system jitter");
return;
}
_bdshot_cycle_complete = false;
}
// Enable DShot inverted on all channels
io_timer_set_enable(true, _bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot,
io_timer_set_enable(true, _bdshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot,
IO_TIMER_ALL_MODES_CHANNELS);
// For each timer, begin DMA transmit
@@ -345,7 +400,7 @@ void up_dshot_trigger()
io_timer_set_dshot_burst_mode(timer_index, _dshot_frequency, channel_count);
if (_bidirectional) {
if (_bdshot_enabled) {
// Deallocate DMA from previous transaction
if (timer_configs[timer_index].dma_handle != NULL) {
stm32_dmastop(timer_configs[timer_index].dma_handle);
@@ -376,14 +431,20 @@ void up_dshot_trigger()
// Clean UDE flag before DMA is started
io_timer_update_dma_req(timer_index, false);
// Trigger DMA (DShot Outputs)
if (timer_configs[timer_index].bidirectional) {
// Trigger DMA (DShot Outputs). Only capture compare afte the system has had time to boot.
if (timer_configs[timer_index].bidirectional && (hrt_absolute_time() > 3000000)) {
perf_begin(capture_cycle_perf);
stm32_dmastart(timer_configs[timer_index].dma_handle, dma_burst_finished_callback,
&timer_configs[timer_index].timer_index,
false);
} else {
stm32_dmastart(timer_configs[timer_index].dma_handle, NULL, NULL, false);
if (_bdshot_enabled) {
_bdshot_cycle_complete = true;
}
}
// Enable DMA update request
@@ -453,11 +514,21 @@ void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg)
// Unallocate timer channel for currently selected capture_channel
uint8_t capture_channel = timer_configs[timer_index].capture_channel_index;
uint8_t output_channel = output_channel_from_timer_channel(timer_index, capture_channel);
// Re-initialize output for CaptureDMA for next time
io_timer_unallocate_channel(output_channel);
io_timer_channel_init(output_channel, IOTimerChanMode_CaptureDMA, NULL, NULL);
// Re-initialize all output channels on this timer as CaptureDMA to ensure all lines idle high
for (uint8_t channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) {
bool is_this_timer = timer_index == timer_io_channels[channel].timer_index;
uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1;
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
if (is_this_timer && channel_initialized) {
io_timer_unallocate_channel(channel);
// Initialize back to DShotInverted to bring IO back to the expected idle state
io_timer_channel_init(channel, IOTimerChanMode_CaptureDMA, NULL, NULL);
}
}
// Select the next capture channel
select_next_capture_channel(timer_index);
@@ -493,14 +564,19 @@ void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg)
// Enable CaptureDMA and on all configured channels
io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, IO_TIMER_ALL_MODES_CHANNELS);
// 30us to switch regardless of DShot frequency + eRPM frame time + 10us for good measure
// Measuring the time it takes from when we start the DMA to when we enable CaptureDMA
perf_end(capture_cycle_perf);
perf_begin(capture_cycle_perf2);
// 30us to switch regardless of DShot frequency + eRPM frame time + 20us for good measure
hrt_abstime frame_us = (16 * 1000000) / _dshot_frequency; // 16 bits * us_per_s / bits_per_s
hrt_abstime delay = 30 + frame_us + 10;
hrt_abstime delay = 30 + frame_us + 20;
hrt_call_after(&_cc_call, delay, capture_complete_callback, arg);
}
static void capture_complete_callback(void *arg)
{
perf_end(capture_cycle_perf2);
perf_begin(hrt_callback_perf);
uint8_t timer_index = *((uint8_t *)arg);
@@ -524,7 +600,6 @@ static void capture_complete_callback(void *arg)
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
if (is_this_timer && channel_initialized) {
io_timer_unallocate_channel(output_channel);
// Initialize back to DShotInverted to bring IO back to the expected idle state
io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL);
@@ -542,213 +617,137 @@ static void capture_complete_callback(void *arg)
io_timer_set_enable(true, IOTimerChanMode_DshotInverted, IO_TIMER_ALL_MODES_CHANNELS);
perf_end(hrt_callback_perf);
_bdshot_cycle_complete = true;
}
void process_capture_results(uint8_t timer_index, uint8_t channel_index)
{
const unsigned period = calculate_period(timer_index, channel_index);
(void)timer_index; // NOTE: in the current implementation only 1 timer is used
uint8_t output_channel = output_channel_from_timer_channel(timer_index, channel_index);
uint32_t value = convert_edge_intervals_to_bitstream(channel_index);
// Decode RLL
value = (value ^ (value >> 1));
if (period == 0) {
// If the parsing failed, set the eRPM to 0
_erpms[output_channel] = 0;
// Decode GCR
uint32_t payload = gcr_decode[value & 0x1f];
payload |= gcr_decode[(value >> 5) & 0x1f] << 4;
payload |= gcr_decode[(value >> 10) & 0x1f] << 8;
payload |= gcr_decode[(value >> 15) & 0x1f] << 12;
} else if (period == 65408) {
// Special case for zero motion (e.g., stationary motor)
_erpms[output_channel] = 0;
// Calculate checksum
uint32_t checksum = payload;
checksum = checksum ^ (checksum >> 8);
checksum = checksum ^ (checksum >> NIBBLES_SIZE);
} else {
// Convert the period to eRPM
_erpms[output_channel] = (1000000 * 60 / 100 + period / 2) / period;
}
if ((checksum & 0xF) != 0xF) {
++read_fail_crc[output_channel];
// We set it ready anyway, not to hold up other channels when used in round robin.
_erpms_ready[output_channel] = true;
}
if (_consecutive_failures[output_channel]++ > BDSHOT_OFFLINE_COUNT) {
_consecutive_failures[output_channel] = BDSHOT_OFFLINE_COUNT;
_consecutive_successes[output_channel] = 0;
_bdshot_online[output_channel] = false;
}
/**
* bits 1-11 - throttle value (0-47 are reserved for commands, 48-2047 give 2000 steps of throttle resolution)
* bit 12 - dshot telemetry enable/disable
* bits 13-16 - XOR checksum
**/
void dshot_motor_data_set(unsigned channel, uint16_t data, bool telemetry)
{
uint8_t timer_index = timer_io_channels[channel].timer_index;
uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1;
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
if (!channel_initialized) {
_bdshot_processed[output_channel] = true;
return;
}
uint16_t packet = 0;
uint16_t checksum = 0;
++read_ok[output_channel];
packet |= data << DSHOT_THROTTLE_POSITION;
packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION;
uint16_t csum_data = packet;
/* XOR checksum calculation */
csum_data >>= NIBBLES_SIZE;
for (uint8_t i = 0; i < DSHOT_NUMBER_OF_NIBBLES; i++) {
checksum ^= (csum_data & 0x0F); // XOR data by nibbles
csum_data >>= NIBBLES_SIZE;
if (_consecutive_successes[output_channel]++ > BDSHOT_OFFLINE_COUNT) {
_consecutive_successes[output_channel] = BDSHOT_OFFLINE_COUNT;
_consecutive_failures[output_channel] = 0;
_bdshot_online[output_channel] = true;
}
if (_bidirectional) {
packet |= ((~checksum) & 0x0F);
// Convert payload into telem type/value
struct BDShotTelemetry packet = {};
payload = (payload >> 4) & 0xFFF;
decode_dshot_telemetry(payload, &packet);
} else {
packet |= ((checksum) & 0x0F);
}
hrt_abstime now = hrt_absolute_time();
switch (packet.type) {
case DSHOT_EDT_ERPM: {
_erpms[output_channel].erpm = packet.value;
_erpms[output_channel].ready = true;
const io_timers_channel_mapping_element_t *mapping = &io_timers_channel_mapping.element[timer_index];
uint8_t num_motors = mapping->channel_count_including_gaps;
uint8_t timer_channel = timer_io_channels[channel].timer_channel - mapping->lowest_timer_channel;
for (uint8_t motor_data_index = 0; motor_data_index < ONE_MOTOR_DATA_SIZE; motor_data_index++) {
dshot_output_buffer[timer_index][motor_data_index * num_motors + timer_channel] =
(packet & 0x8000) ? MOTOR_PWM_BIT_1 : MOTOR_PWM_BIT_0; // MSB first
packet <<= 1;
}
}
int up_dshot_arm(bool armed)
{
return io_timer_set_enable(armed, _bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot,
IO_TIMER_ALL_MODES_CHANNELS);
}
int up_bdshot_num_erpm_ready(void)
{
int num_ready = 0;
for (unsigned i = 0; i < MAX_TIMER_IO_CHANNELS; ++i) {
if (_erpms_ready[i]) {
++num_ready;
uint64_t last_timestamp = _erpms[output_channel].last_timestamp;
float last_rate_hz = _erpms[output_channel].rate_hz;
_erpms[output_channel].rate_hz = calculate_rate_hz(last_timestamp, last_rate_hz, now);
_erpms[output_channel].last_timestamp = now;
break;
}
}
return num_ready;
}
case DSHOT_EDT_TEMPERATURE: {
_edt_temp[output_channel].value = packet.value;
_edt_temp[output_channel].ready = true;
int up_bdshot_get_erpm(uint8_t output_channel, int *erpm)
{
uint8_t timer_index = timer_io_channels[output_channel].timer_index;
uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1;
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
if (channel_initialized) {
*erpm = _erpms[output_channel];
_erpms_ready[output_channel] = false;
return PX4_OK;
}
// this channel is not configured for dshot
return PX4_ERROR;
}
int up_bdshot_channel_status(uint8_t channel)
{
uint8_t timer_index = timer_io_channels[channel].timer_index;
uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1;
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
// TODO: track that each channel is communicating using the decode stats
if (channel_initialized) {
return 1;
}
return 0;
}
void up_bdshot_status(void)
{
PX4_INFO("dshot driver stats:");
if (_bidirectional) {
PX4_INFO("Bidirectional DShot enabled");
}
uint8_t timer_index = _bidi_timer_index;
for (uint8_t timer_channel_index = 0; timer_channel_index < MAX_NUM_CHANNELS_PER_TIMER; timer_channel_index++) {
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
if (channel_initialized) {
PX4_INFO("Timer %u, Channel %u: read %lu, failed nibble %lu, failed CRC %lu, invalid/zero %lu",
timer_index, timer_channel_index,
read_ok[timer_channel_index],
read_fail_nibble[timer_channel_index],
read_fail_crc[timer_channel_index],
read_fail_zero[timer_channel_index]);
uint64_t last_timestamp = _edt_temp[output_channel].last_timestamp;
float last_rate_hz = _edt_temp[output_channel].rate_hz;
_edt_temp[output_channel].rate_hz = calculate_rate_hz(last_timestamp, last_rate_hz, now);
_edt_temp[output_channel].last_timestamp = now;
break;
}
}
}
uint8_t nibbles_from_mapped(uint8_t mapped)
{
switch (mapped) {
case 0x19:
return 0x00;
case DSHOT_EDT_VOLTAGE: {
_edt_volt[output_channel].value = packet.value;
_edt_volt[output_channel].ready = true;
case 0x1B:
return 0x01;
uint64_t last_timestamp = _edt_volt[output_channel].last_timestamp;
float last_rate_hz = _edt_volt[output_channel].rate_hz;
_edt_volt[output_channel].rate_hz = calculate_rate_hz(last_timestamp, last_rate_hz, now);
_edt_volt[output_channel].last_timestamp = now;
break;
}
case 0x12:
return 0x02;
case DSHOT_EDT_CURRENT: {
_edt_curr[output_channel].value = packet.value;
_edt_curr[output_channel].ready = true;
case 0x13:
return 0x03;
uint64_t last_timestamp = _edt_curr[output_channel].last_timestamp;
float last_rate_hz = _edt_curr[output_channel].rate_hz;
_edt_curr[output_channel].rate_hz = calculate_rate_hz(last_timestamp, last_rate_hz, now);
_edt_curr[output_channel].last_timestamp = now;
break;
}
case 0x1D:
return 0x04;
case 0x15:
return 0x05;
case 0x16:
return 0x06;
case 0x17:
return 0x07;
case 0x1a:
return 0x08;
case 0x09:
return 0x09;
case 0x0A:
return 0x0A;
case 0x0B:
return 0x0B;
case 0x1E:
return 0x0C;
case 0x0D:
return 0x0D;
case 0x0E:
return 0x0E;
case 0x0F:
return 0x0F;
case DSHOT_EDT_STATE_EVENT:
// TODO: Handle these?
break;
default:
// Unknown mapped
return 0xFF;
PX4_WARN("unknown EDT type %d", packet.type);
break;
}
_bdshot_processed[output_channel] = true;
}
unsigned calculate_period(uint8_t timer_index, uint8_t channel_index)
float calculate_rate_hz(uint64_t last_timestamp, float last_rate_hz, uint64_t timestamp)
{
if (last_timestamp == 0 || timestamp <= last_timestamp) {
return last_rate_hz;
}
uint64_t dt_us = timestamp - last_timestamp;
float instant_rate = 1000000.0f / dt_us;
// Simple exponential moving average with fixed alpha
// Alpha = 0.125 (1/8) works well across all rates
float rate_hz = instant_rate * 0.125f + last_rate_hz * 0.875f;
return rate_hz;
}
// Converts captured edge timestamps into a raw bit stream.
// Measures the time intervals between signal edges to determine how many consecutive
// 1s or 0s to shift in, alternating the bit value with each edge transition.
// Returns a 20 bit raw value that still needs RLL and GCR decoding.
uint32_t convert_edge_intervals_to_bitstream(uint8_t channel_index)
{
uint32_t value = 0;
uint32_t high = 1; // We start off with high
@@ -783,46 +782,255 @@ unsigned calculate_period(uint8_t timer_index, uint8_t channel_index)
if (shifted == 0) {
// no data yet, or this time
++read_fail_zero[channel_index];
return 0;
}
// We need to make sure we shifted 21 times. We might have missed some low "pulses" at the very end.
value <<= (21 - shifted);
// From GCR to eRPM according to:
// https://brushlesswhoop.com/dshot-and-bidirectional-dshot/#erpm-transmission
unsigned gcr = (value ^ (value >> 1));
return value;
}
uint32_t data = 0;
void decode_dshot_telemetry(uint32_t payload, struct BDShotTelemetry *packet)
{
// Extended DShot Telemetry
bool edt_enabled = _extended_dshot_telem;
uint32_t mantissa = payload & 0x01FF;
bool is_telemetry = (mantissa & 0x0100) ==
0; // if the msb of the mantissa is zero, then this is an extended telemetry packet
// 20bits -> 5 mapped -> 4 nibbles
for (unsigned i = 0; i < 4; ++i) {
uint32_t nibble = nibbles_from_mapped(gcr & 0x1F) << (4 * i);
if (edt_enabled && is_telemetry) {
packet->type = (payload & 0x0F00) >> 8;
packet->value = payload & 0x00FF; // extended telemetry value is 8 bits wide
if (nibble == 0xFF) {
++read_fail_nibble[channel_index];;
return 0;
} else {
// otherwise it's an eRPM frame
uint8_t exponent = ((payload >> 9) & 0x7); // 3 bit: exponent
uint16_t period = (payload & 0x1FF); // 9 bit: period base
period = period << exponent; // Period in usec
packet->type = DSHOT_EDT_ERPM;
if (period == 65408) {
// Special case for zero motion (e.g., stationary motor)
packet->value = 0;
} else {
packet->value = (1000000 * 60 / 100 + period / 2) / period;
}
}
}
data |= nibble;
gcr >>= 5;
// bits 1-11 - throttle value (0-47 are reserved for commands, 48-2047 give 2000 steps of throttle resolution)
// bit 12 - dshot telemetry enable/disable
// bits 13-16 - XOR checksum
void dshot_motor_data_set(unsigned channel, uint16_t data, bool telemetry)
{
uint8_t timer_index = timer_io_channels[channel].timer_index;
uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1;
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
if (!channel_initialized) {
return;
}
unsigned shift = (data & 0xE000) >> 13;
unsigned period = ((data & 0x1FF0) >> 4) << shift;
unsigned crc = data & 0xF;
uint16_t packet = 0;
uint16_t checksum = 0;
unsigned payload = (data & 0xFFF0) >> 4;
unsigned calculated_crc = (~(payload ^ (payload >> 4) ^ (payload >> 8))) & 0x0F;
packet |= data << DSHOT_THROTTLE_POSITION;
packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION;
if (crc != calculated_crc) {
++read_fail_crc[channel_index];;
uint16_t csum_data = packet;
// XOR checksum calculation
csum_data >>= NIBBLES_SIZE;
for (uint8_t i = 0; i < DSHOT_NUMBER_OF_NIBBLES; i++) {
checksum ^= (csum_data & 0x0F); // XOR data by nibbles
csum_data >>= NIBBLES_SIZE;
}
if (_bdshot_enabled) {
packet |= ((~checksum) & 0x0F);
} else {
packet |= ((checksum) & 0x0F);
}
const io_timers_channel_mapping_element_t *mapping = &io_timers_channel_mapping.element[timer_index];
uint8_t num_motors = mapping->channel_count_including_gaps;
uint8_t timer_channel = timer_io_channels[channel].timer_channel - mapping->lowest_timer_channel;
for (uint8_t motor_data_index = 0; motor_data_index < ONE_MOTOR_DATA_SIZE; motor_data_index++) {
dshot_output_buffer[timer_index][motor_data_index * num_motors + timer_channel] =
(packet & 0x8000) ? MOTOR_PWM_BIT_1 : MOTOR_PWM_BIT_0; // MSB first
packet <<= 1;
}
}
int up_dshot_arm(bool armed)
{
return io_timer_set_enable(armed, _bdshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot,
IO_TIMER_ALL_MODES_CHANNELS);
}
int up_bdshot_num_channels_ready(void)
{
int num_ready = 0;
for (unsigned i = 0; i < MAX_TIMER_IO_CHANNELS; ++i) {
if (_bdshot_processed[i]) {
++num_ready;
}
}
return num_ready;
}
int up_bdshot_num_errors(uint8_t channel)
{
return read_fail_crc[channel];
}
int up_bdshot_get_erpm(uint8_t channel, int *erpm)
{
uint8_t timer_index = timer_io_channels[channel].timer_index;
uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1;
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
int status = PX4_ERROR;
if (channel_initialized && _erpms[channel].ready) {
*erpm = _erpms[channel].erpm;
status = PX4_OK;
}
// Mark sample read
_bdshot_processed[channel] = false;
return status;
}
int up_bdshot_get_extended_telemetry(uint8_t channel, int type, uint8_t *value)
{
int result = PX4_ERROR;
switch (type) {
case DSHOT_EDT_TEMPERATURE:
if (_edt_temp[channel].ready) {
*value = _edt_temp[channel].value;
_edt_temp[channel].ready = false;
result = PX4_OK;
}
break;
case DSHOT_EDT_VOLTAGE:
if (_edt_volt[channel].ready) {
*value = _edt_volt[channel].value;
_edt_volt[channel].ready = false;
result = PX4_OK;
}
break;
case DSHOT_EDT_CURRENT:
if (_edt_curr[channel].ready) {
*value = _edt_curr[channel].value;
_edt_curr[channel].ready = false;
result = PX4_OK;
}
break;
default:
break;
}
return result;
}
int up_bdshot_get_extended_telemetry_rate(uint8_t channel, int type, int *value)
{
int result = PX4_ERROR;
switch (type) {
case DSHOT_EDT_TEMPERATURE:
if (_bdshot_online[channel]) {
*value = 0;
result = PX4_OK;
}
break;
case DSHOT_EDT_VOLTAGE:
if (_bdshot_online[channel]) {
*value = 0;
result = PX4_OK;
}
break;
case DSHOT_EDT_CURRENT:
if (_bdshot_online[channel]) {
*value = 0;
result = PX4_OK;
}
break;
default:
break;
}
return result;
}
int up_bdshot_channel_online(uint8_t channel)
{
if (channel >= MAX_TIMER_IO_CHANNELS) {
return 0;
}
++read_ok[channel_index];;
return period;
return _bdshot_online[channel];
}
void up_bdshot_status(void)
{
PX4_INFO("dshot driver stats:");
if (_bdshot_enabled) {
PX4_INFO("BDShot enabled");
}
if (_extended_dshot_telem) {
PX4_INFO("BDShot EDT rates");
for (int i = 0; i < MAX_TIMER_IO_CHANNELS; i++) {
if (_bdshot_online[i]) {
PX4_INFO("Ch%d: eRPM: %dHz Temp: %.2fHz Volt: %.2fHz Curr: %.2fHz",
i,
(int)_erpms[i].rate_hz,
(double)_edt_temp[i].rate_hz,
(double)_edt_volt[i].rate_hz,
(double)_edt_curr[i].rate_hz);
}
}
}
uint8_t timer_index = _bidi_timer_index;
for (uint8_t timer_channel_index = 0; timer_channel_index < MAX_NUM_CHANNELS_PER_TIMER; timer_channel_index++) {
bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
if (channel_initialized) {
PX4_INFO("Timer %u, Channel %u: read %lu, failed CRC %lu",
timer_index, timer_channel_index,
read_ok[timer_channel_index],
read_fail_crc[timer_channel_index]);
}
}
}
#endif
src/drivers/drv_dshot.h
+55 -44
View File
@@ -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,7 +87,8 @@ 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, unsigned dshot_pwm_freq, bool bdshot_enable,
bool enable_extended_dshot_telemetry);
/**
* Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method)
@@ -107,7 +104,7 @@ __EXPORT extern void dshot_motor_data_set(unsigned channel, uint16_t throttle, b
*/
static inline void up_dshot_motor_data_set(unsigned 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,7 +139,6 @@ __EXPORT extern int up_dshot_arm(bool armed);
*/
__EXPORT extern void up_bdshot_status(void);
/**
* Get how many bidirectional erpm channels are ready
*
@@ -151,8 +147,14 @@ __EXPORT extern void up_bdshot_status(void);
*
* @return <0 on error, OK on succes
*/
__EXPORT extern int up_bdshot_num_erpm_ready(void);
__EXPORT extern int up_bdshot_num_channels_ready(void);
/**
* 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_errors(uint8_t channel);
/**
* Get bidrectional dshot erpm for a channel
@@ -162,6 +164,16 @@ __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);
__EXPORT extern int up_bdshot_get_extended_telemetry_rate(uint8_t channel, int type, int *value);
/**
* Get bidrectional dshot status for a channel
@@ -169,7 +181,6 @@ __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);
__END_DECLS
src/drivers/dshot/CMakeLists.txt
+2
View File
@@ -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
src/drivers/dshot/DShot.cpp
+877 -574
View File
File diff suppressed because it is too large Load Diff
src/drivers/dshot/DShot.h
+128 -83
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019-2022 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
@@ -36,10 +36,11 @@
#include <lib/mixer_module/mixer_module.hpp>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/am32_eeprom_write.h>
#include "DShotCommon.h"
#include "DShotTelemetry.h"
using namespace time_literals;
@@ -48,14 +49,24 @@ using namespace time_literals;
# error "board_config.h needs to define DIRECT_PWM_OUTPUT_CHANNELS"
#endif
static constexpr hrt_abstime ESC_INIT_TELEM_WAIT_TIME = 3_s;
/** Dshot PWM frequency, Hz */
static constexpr unsigned int DSHOT150 = 150000u;
static constexpr unsigned int DSHOT300 = 300000u;
static constexpr unsigned int DSHOT600 = 600000u;
static constexpr int DSHOT_DISARM_VALUE = 0;
static constexpr int DSHOT_MIN_THROTTLE = 1;
static constexpr int DSHOT_MAX_THROTTLE = 1999;
static constexpr uint16_t DSHOT_DISARM_VALUE = 0;
static constexpr uint16_t DSHOT_MIN_THROTTLE = 1;
static constexpr uint16_t DSHOT_MAX_THROTTLE = 1999;
// We do this to avoid bringing in mavlink.h
// #include <mavlink/common/mavlink.h>
#define ACTUATOR_CONFIGURATION_BEEP 1
#define ACTUATOR_CONFIGURATION_3D_MODE_OFF 2
#define ACTUATOR_CONFIGURATION_3D_MODE_ON 3
#define ACTUATOR_CONFIGURATION_SPIN_DIRECTION1 4
#define ACTUATOR_CONFIGURATION_SPIN_DIRECTION2 5
class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
{
@@ -63,122 +74,156 @@ 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(uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
bool updateOutputs(uint16_t *outputs, unsigned num_outputs, unsigned num_control_groups_updated) override;
private:
enum class State {
Disarmed,
Armed
} _state = State::Disarmed;
/** 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);
uint8_t esc_armed_mask(uint16_t *outputs, int num_outputs);
void publish_esc_status(void);
void update_motor_outputs(uint16_t *outputs, int num_outputs);
void update_motor_commands(int num_outputs);
void select_next_command();
int handle_new_bdshot_erpm(void);
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 Run() override;
void update_params();
void update_num_motors();
void handle_vehicle_commands();
void consume_esc_data(const EscData &data, TelemetrySource source);
uint16_t calculate_output_value(uint16_t raw, int index);
uint16_t convert_output_to_3d_scaling(uint16_t output);
void Run() override;
void update_params();
// Mavlink command handlers
void handle_vehicle_commands();
void handle_configure_actuator(const vehicle_command_s &command);
void handle_am32_request_eeprom(const vehicle_command_s &command);
// Mixer
MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
uint32_t _reversible_outputs{};
uint32_t _output_mask{0}; // Configured outputs for this (shouldn't this live in OutputModuleInterface?)
DShotTelemetry *_telemetry{nullptr};
// uORB
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _am32_eeprom_write_sub{ORB_ID(am32_eeprom_write)};
uORB::PublicationMultiData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
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_MAXIMUM_CHANNELS] = {};
uint32_t _bdshot_telem_errors[DSHOT_MAXIMUM_CHANNELS] = {};
uint8_t _bdshot_edt_requested_mask = 0;
uint8_t _settings_requested_mask = 0;
// Array of timestamps indicating when the telemetry came online
hrt_abstime _serial_telem_online_timestamps[DSHOT_MAXIMUM_CHANNELS] = {};
hrt_abstime _bdshot_telem_online_timestamps[DSHOT_MAXIMUM_CHANNELS] = {};
// Serial Telemetry
DShotTelemetry _telemetry;
static char _telemetry_device[20];
static bool _telemetry_swap_rxtx;
static px4::atomic_bool _request_telemetry_init;
int _telemetry_motor_index = 0;
uint32_t _telemetry_requested_mask = 0;
hrt_abstime _telem_delay_until = ESC_INIT_TELEM_WAIT_TIME;
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};
uint32_t _output_mask{0};
int _num_motors{0};
// Perf counters
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")};
perf_counter_t _bdshot_success_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot success")};
perf_counter_t _bdshot_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot error")};
perf_counter_t _bdshot_timeout_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot timeout")};
perf_counter_t _telem_success_perf{perf_alloc(PC_COUNT, MODULE_NAME": telem success")};
perf_counter_t _telem_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": telem error")};
perf_counter_t _telem_timeout_perf{perf_alloc(PC_COUNT, MODULE_NAME": telem timeout")};
perf_counter_t _telem_allsampled_perf{perf_alloc(PC_COUNT, MODULE_NAME": telem all sampled")};
Command _current_command{};
// Commands
struct DShotCommand {
uint16_t command{};
int num_repetitions{0};
uint8_t motor_mask{0xff};
bool save{false};
bool expect_response{false};
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};
bool finished() const { return num_repetitions == 0; }
void clear()
{
command = 0;
num_repetitions = 0;
motor_mask = 0;
save = 0;
expect_response = 0;
}
};
DShotCommand _current_command{};
// DShot Programming Mode
enum class ProgrammingState {
Idle,
EnterMode,
SendAddress,
SendValue,
ExitMode
};
am32_eeprom_write_s _am32_eeprom_write{};
bool _dshot_programming_active = {};
uint32_t _settings_written_mask[2] = {};
ProgrammingState _programming_state{ProgrammingState::Idle};
uint16_t _programming_address{};
uint16_t _programming_value{};
// 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_EN>) _param_dshot_bidir_en,
(ParamBool<px4::params::DSHOT_BIDIR_EDT>) _param_dshot_bidir_edt,
(ParamBool<px4::params::DSHOT_TEL_CFG>) _param_dshot_tel_cfg
)
};
src/drivers/dshot/DShotCommon.h
+96
View File
@@ -0,0 +1,96 @@
/****************************************************************************
*
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <drivers/drv_hrt.h>
#include <uORB/topics/esc_status.h>
static constexpr int DSHOT_MAXIMUM_CHANNELS = esc_status_s::CONNECTED_ESC_MAX;
enum class TelemetrySource {
Serial = 0,
BDShot = 1,
};
struct EscData {
int motor_index; // Motors 0-7
hrt_abstime timestamp; // Sample time
TelemetrySource source;
float temperature; // [deg C]
float voltage; // [0.01V]
float current; // [0.01A]
int16_t erpm; // [100ERPM]
};
enum class TelemetryStatus {
NotStarted = 0,
NotReady = 1,
Ready = 2,
Timeout = 3,
ParseError = 4,
};
inline int count_set_bits(int mask)
{
int count = 0;
while (mask) {
mask &= mask - 1;
count++;
}
return count;
}
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;
}
src/drivers/dshot/DShotTelemetry.cpp
+187 -273
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019 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
@@ -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_MAXIMUM_CHANNELS; i++) {
if (_settings_handlers[i]) {
delete _settings_handlers[i];
_settings_handlers[i] = nullptr;
}
}
}
int DShotTelemetry::init(const char *port, bool swap_rxtx)
@@ -76,105 +85,204 @@ int DShotTelemetry::init(const char *port, bool swap_rxtx)
return PX4_OK;
}
int DShotTelemetry::update(int num_motors)
void DShotTelemetry::initSettingsHandlers(ESCType esc_type, uint8_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;
if (_settings_initialized) {
return;
}
_esc_type = esc_type;
for (uint8_t i = 0; i < DSHOT_MAXIMUM_CHANNELS; i++) {
bool output_enabled = (1 << i) & output_mask;
if (!output_enabled) {
continue;
}
ESCSettingsInterface *interface = nullptr;
switch (esc_type) {
case ESCType::AM32:
interface = new AM32Settings(i);
break;
default:
PX4_WARN("Unsupported ESC type for settings: %d", (int)esc_type);
break;
}
if (interface) {
_settings_handlers[i] = interface;
}
}
_settings_initialized = true;
}
int DShotTelemetry::parseCommandResponse()
{
if (hrt_elapsed_time(&_command_response_start) > 1_s) {
PX4_WARN("Command response timed out: %d bytes received", _command_response_position);
_command_response_motor_index = -1;
_command_response_start = 0;
_command_response_position = 0;
return -1;
}
if (_uart.bytesAvailable() <= 0) {
return -1;
}
uint8_t buf[COMMAND_RESPONSE_MAX_SIZE];
int bytes = _uart.read(buf, sizeof(buf));
// Add bytes to buffer
for (int i = 0; i < bytes; i++) {
_command_response_buffer[_command_response_position++] = buf[i];
}
int index = -1;
switch (_command_response_command) {
case DSHOT_CMD_ESC_INFO: {
auto handler = _settings_handlers[_command_response_motor_index];
if (handler && _command_response_position == handler->getExpectedResponseSize()) {
if (handler->decodeInfoResponse(_command_response_buffer, _command_response_position)) {
index = _command_response_motor_index;
}
// Reset command state
_command_response_position = 0;
_command_response_start = 0;
_command_response_motor_index = -1;
}
break;
}
default:
break;
}
return index;
}
TelemetryStatus DShotTelemetry::parseTelemetryPacket(EscData *esc_data)
{
if (telemetryResponseFinished()) {
return TelemetryStatus::NotStarted;
}
// read from the uart. This must be non-blocking, so check first if there is data available
int bytes_available = _uart.bytesAvailable();
if (_uart.bytesAvailable() <= 0) {
if (hrt_elapsed_time(&_telemetry_request_start) > 30_ms) {
// NOTE: this happens when sending commands, there's a window after an ESC receives
// a command where it will not respond to any telemetry requests
// PX4_INFO("ESC telemetry timeout: %d", esc_data->motor_index);
++_num_timeouts;
if (bytes_available <= 0) {
// no data available. Check for a timeout
const hrt_abstime now = hrt_absolute_time();
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;
} 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;
// Mark telemetry request as finished
_telemetry_request_start = 0;
_frame_position = 0;
return TelemetryStatus::Timeout;
}
return -1;
return TelemetryStatus::NotReady;
}
uint8_t buf[ESC_FRAME_SIZE];
uint8_t buf[TELEMETRY_FRAME_SIZE];
int bytes = _uart.read(buf, sizeof(buf));
int ret = -1;
for (int i = 0; i < bytes && ret == -1; ++i) {
if (_redirect_output) {
_redirect_output->buffer[_redirect_output->buf_pos++] = buf[i];
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);
}
}
}
return ret;
return decodeTelemetryResponse(buf, bytes, esc_data);
}
bool DShotTelemetry::decodeByte(uint8_t byte, bool &successful_decoding)
TelemetryStatus DShotTelemetry::decodeTelemetryResponse(uint8_t *buffer, int length, EscData *esc_data)
{
_frame_buffer[_frame_position++] = byte;
successful_decoding = false;
auto status = TelemetryStatus::NotReady;
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];
for (int i = 0; i < length; i++) {
_frame_buffer[_frame_position++] = buffer[i];
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;
/*
* 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
*/
} else {
++_num_checksum_errors;
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];
int16_t voltage = (_frame_buffer[1] << 8) | _frame_buffer[2];
int16_t current = (_frame_buffer[3] << 8) | _frame_buffer[4];
// int16_t consumption = (_frame_buffer[5]) << 8 | _frame_buffer[6];
int16_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;
_uart.flush();
} else {
++_num_checksum_errors;
status = TelemetryStatus::ParseError;
}
// Mark telemetry request as finished
_telemetry_request_start = 0;
_frame_position = 0;
}
return true;
}
return false;
return status;
}
void DShotTelemetry::publish_esc_settings()
{
for (int i = 0; i < DSHOT_MAXIMUM_CHANNELS; i++) {
if (_settings_handlers[i]) {
_settings_handlers[i]->publish_latest();
}
}
}
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;
}
void DShotTelemetry::startTelemetryRequest()
{
_telemetry_request_start = hrt_absolute_time();
}
bool DShotTelemetry::telemetryResponseFinished()
{
return _telemetry_request_start == 0;
}
void DShotTelemetry::printStatus() const
@@ -183,197 +291,3 @@ void DShotTelemetry::printStatus() const
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");
}
}
}
src/drivers/dshot/DShotTelemetry.h
+35 -66
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019 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
@@ -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.
int parseCommandResponse();
bool commandResponseFinished();
void setExpectCommandResponse(int motor_index, uint16_t command);
void initSettingsHandlers(ESCType esc_type, uint8_t output_mask);
void publish_esc_settings();
private:
static constexpr int ESC_FRAME_SIZE = 10;
static constexpr int COMMAND_RESPONSE_MAX_SIZE = 128;
static constexpr int COMMAND_RESPONSE_SETTINGS_SIZE = 49; // 48B for EEPROM + 1B for CRC
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
EscData _latest_data{};
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_MAXIMUM_CHANNELS] = {nullptr};
ESCType _esc_type{ESCType::Unknown};
bool _settings_initialized{false};
};
src/drivers/dshot/esc/AM32Settings.cpp
+85
View File
@@ -0,0 +1,85 @@
/****************************************************************************
*
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "AM32Settings.h"
#include "../DShotCommon.h"
#include <px4_platform_common/log.h>
static constexpr int EEPROM_SIZE = 48; // AM32 sends raw eeprom data
static constexpr int RESPONSE_SIZE = 49; // 48B data + 1B CRC
uORB::Publication<am32_eeprom_read_s> AM32Settings::_am32_eeprom_read_pub{ORB_ID(am32_eeprom_read)};
AM32Settings::AM32Settings(int index)
: _esc_index(index)
{}
int AM32Settings::getExpectedResponseSize()
{
return RESPONSE_SIZE;
}
void AM32Settings::publish_latest()
{
// PX4_INFO("publish_latest()");
am32_eeprom_read_s data = {};
data.timestamp = hrt_absolute_time();
data.index = _esc_index;
memcpy(data.data, &_eeprom_data, sizeof(data.data));
_am32_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_INFO("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 immedietly
publish_latest();
return true;
}
src/drivers/dshot/esc/AM32Settings.h
+103
View File
@@ -0,0 +1,103 @@
/****************************************************************************
*
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "ESCSettingsInterface.h"
#include <uORB/Publication.hpp>
#include <uORB/topics/am32_eeprom_read.h>
class AM32Settings : public ESCSettingsInterface
{
public:
AM32Settings(int index);
struct EEPROMData {
uint8_t eeprom_start; // 0: must be 1
uint8_t eeprom_version; // 1: version 0-255
uint8_t bootloader_version; // 2: bootloader version 0-255
uint8_t firmware_major; // 3: firmware version major
uint8_t firmware_minor; // 4: firmware version minor
uint8_t max_ramp_speed; // 5: value/10 percent per ms (default 160 = 16%/ms)
uint8_t min_duty_cycle; // 6: value/2 (default 4 = 2%)
uint8_t stick_calibration; // 7: disable stick calibration (default 0)
uint8_t voltage_cutoff; // 8: absolute voltage cutoff (default 10)
uint8_t current_pid_p; // 9: P value x2 (default 100 = 200)
uint8_t current_pid_i; // 10: I value (default 0)
uint8_t current_pid_d; // 11: D value x10 (default 50 = 500)
uint8_t active_brake_power; // 12: active brake power
uint8_t reserved[4]; // 13-16: reserved bytes
uint8_t direction_reversed; // 17: direction reversed
uint8_t bidirectional_mode; // 18: bidirectional mode (1=on, 0=off)
uint8_t sinusoidal_startup; // 19: sinusoidal startup
uint8_t complementary_pwm; // 20: complementary PWM
uint8_t variable_pwm_freq; // 21: variable PWM frequency
uint8_t stuck_rotor_protection; // 22: stuck rotor protection
uint8_t timing_advance; // 23: timing advance x0.9375 (16 = 15 degrees)
uint8_t pwm_frequency; // 24: PWM freq in kHz (default 24)
uint8_t startup_power; // 25: startup power 50-150% (default 100)
uint8_t motor_kv; // 26: KV in increments of 40 (55 = 2200kv)
uint8_t motor_poles; // 27: motor poles (default 14)
uint8_t brake_on_stop; // 28: brake on stop (default 0)
uint8_t anti_stall; // 29: anti-stall protection
uint8_t beep_volume; // 30: beep volume 0-11 (default 5)
uint8_t telemetry_30ms; // 31: 30ms telemetry output (0 or 1)
uint8_t servo_low; // 32: servo low (value*2)+750us
uint8_t servo_high; // 33: servo high (value*2)+1750us
uint8_t servo_neutral; // 34: servo neutral 1374+value us (128=1500us)
uint8_t servo_deadband; // 35: servo deadband 0-100
uint8_t low_voltage_cutoff; // 36: low voltage cutoff
uint8_t low_voltage_threshold; // 37: threshold value+250/10V (50=3.0V)
uint8_t rc_car_reversing; // 38: RC car type reversing (default 0)
uint8_t hall_sensors; // 39: hall sensor options
uint8_t sine_mode_range; // 40: sine mode range 5-25% (default 15)
uint8_t drag_brake_strength; // 41: drag brake 1-10 (default 10)
uint8_t running_brake_amount; // 42: brake when running (default 10)
uint8_t temperature_limit; // 43: temp limit 70-140C (141=disabled)
uint8_t current_protection; // 44: current limit value x2 (102=disabled)
uint8_t sine_mode_strength; // 45: sine mode strength 1-10 (default 6)
uint8_t input_type; // 46: input type selector
uint8_t auto_timing; // 47: auto timing
} __attribute__((packed));
int getExpectedResponseSize() override;
bool decodeInfoResponse(const uint8_t *buf, int size) override;
void publish_latest() override;
private:
int _esc_index{};
EEPROMData _eeprom_data{};
static uORB::Publication<am32_eeprom_read_s> _am32_eeprom_read_pub;
};
src/drivers/dshot/esc/ESCSettingsInterface.h
+53
View File
@@ -0,0 +1,53 @@
/****************************************************************************
*
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
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
};
src/drivers/dshot/module.yaml
+19
View File
@@ -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
2: TODO Check Ardupilot
DSHOT_MIN:
description:
short: Minimum DShot Motor Output
@@ -43,6 +52,16 @@ parameters:
type: boolean
default: 0
reboot_required: true
DSHOT_BIDIR_EDT:
description:
short: Enable Extended DShot Telemetry
long: |
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
DSHOT_3D_DEAD_H:
description:
short: DSHOT 3D deadband high
src/lib/mixer_module/mixer_module.hpp
+2
View File
@@ -141,6 +141,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
src/modules/logger/logged_topics.cpp
+2
View File
@@ -38,6 +38,7 @@
#include <px4_platform_common/log.h>
#include <px4_platform_common/px4_config.h>
#include <uORB/topics/uORBTopics.hpp>
#include <uORB/topics/am32_eeprom_read.h> // TODO: debugging
#include <string.h>
@@ -45,6 +46,7 @@ using namespace px4::logger;
void LoggedTopics::add_default_topics()
{
add_topic("am32_eeprom_read"); // TODO: debugging
add_topic("action_request");
add_topic("actuator_armed");
add_optional_topic("actuator_controls_status_0", 300);