mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-20 17:13:34 +08:00
rebased
This commit is contained in:
Jacob Dahl
@@ -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);
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user