Skip to content

Commit

Permalink
round robin sampling for less than 4 DMA
Browse files Browse the repository at this point in the history
  • Loading branch information
@dakejahl
dakejahl committed Nov 18, 2024
1 parent 04fc3c9 commit 9a29a2d
Showing 1 changed file with 105 additions and 21 deletions.
126 changes: 105 additions & 21 deletions platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,9 @@
#include <stdio.h>
#include <drivers/drv_input_capture.h>

#define TEST_LIMITED_DMA 1
#define LIMIT_DMA_CHANNELS 1

// DShot protocol definitions
#define ONE_MOTOR_DATA_SIZE 16u
#define MOTOR_PWM_BIT_1 14u
Expand Down Expand Up @@ -84,6 +87,8 @@ static void init_timers_dma_up(void);
static void init_timers_dma_capt_comp(uint8_t timer_index);
static int32_t init_timer_channels(uint8_t timer_index);

static void configure_channels_round_robin(uint8_t timer_index);

static void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg);
static void capture_complete_callback(void *arg);

Expand All @@ -100,9 +105,12 @@ typedef struct timer_config_t {
bool initialized_channels[4]; // Timer channels initialized (successfully started)
bool bidirectional; // Timer in bidi (inverted) mode
bool captcomp_channels[4]; // Channels configured for CaptComp
bool round_robin_enabled;
uint8_t timer_index; // Timer index. Necessary to have memory for passing pointer to hrt callback
} timer_config_t;

static uint8_t _num_dma_available = 0;

static timer_config_t timer_configs[MAX_IO_TIMERS] = {};

// Output buffer of interleaved motor output bytes
Expand Down Expand Up @@ -252,6 +260,14 @@ static void init_timers_dma_capt_comp(uint8_t timer_index)
PX4_DEBUG("Allocated DMA CH Timer Index %u Channel %u", timer_index, timer_channel_index);
// Mark this timer channel as bidirectional
timer_configs[timer_index].captcomp_channels[timer_channel_index] = true;
_num_dma_available++;

#if defined(TEST_LIMITED_DMA)
if (_num_dma_available >= LIMIT_DMA_CHANNELS) {
PX4_INFO("Limiting DMA channels to %u", _num_dma_available);
break;
}
#endif
}
}

Expand Down Expand Up @@ -350,6 +366,11 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi
// Use first configured DShot timer (Timer index 0)
// TODO: BDSHOT_TIM param to select timer index?
init_timers_dma_capt_comp(_bidi_timer_index);

// Enable round robin if we have 1 - 3 DMA
if ((_num_dma_available < 4) && _num_dma_available > 0) {
timer_configs[_bidi_timer_index].round_robin_enabled = true;
}
}

int32_t channels_init_mask = 0;
Expand Down Expand Up @@ -439,6 +460,57 @@ void up_dshot_trigger()
}
}

static void configure_channels_round_robin(uint8_t timer_index)
{
switch (_num_dma_available) {
case 1: {
for (uint8_t i = 0; i < 4; i++) {
if (timer_configs[timer_index].captcomp_channels[i]) {
timer_configs[timer_index].captcomp_channels[i] = false;
if (i == 3) {
timer_configs[timer_index].captcomp_channels[0] = true;
} else {
timer_configs[timer_index].captcomp_channels[i + 1] = true;
}
break;
}
}
break;
}
case 2: {
if (timer_configs[timer_index].captcomp_channels[0]) {
timer_configs[timer_index].captcomp_channels[0] = false;
timer_configs[timer_index].captcomp_channels[1] = true;
timer_configs[timer_index].captcomp_channels[2] = false;
timer_configs[timer_index].captcomp_channels[3] = true;

} else {
timer_configs[timer_index].captcomp_channels[0] = true;
timer_configs[timer_index].captcomp_channels[1] = false;
timer_configs[timer_index].captcomp_channels[2] = true;
timer_configs[timer_index].captcomp_channels[3] = false;
}
break;
}
case 3: {
for (uint8_t i = 0; i < 4; i++) {
if (!timer_configs[timer_index].captcomp_channels[i]) {
timer_configs[timer_index].captcomp_channels[i] = true;
if (i == 3) {
timer_configs[timer_index].captcomp_channels[0] = false;
} else {
timer_configs[timer_index].captcomp_channels[i + 1] = false;
}
break;
}
}
break;
}
default:
break;
}
}

void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg)
{
uint8_t timer_index = *((uint8_t *)arg);
Expand All @@ -461,6 +533,11 @@ void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg)
up_clean_dcache((uintptr_t) dshot_capture_buffer,
(uintptr_t) dshot_capture_buffer + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER));

// If round robin is enabled reconfigure which channels we capture on
if (timer_configs[timer_index].round_robin_enabled) {
configure_channels_round_robin(timer_index);
}

// Allocate DMA for all enabled channels on this timer
for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) {

Expand Down Expand Up @@ -655,8 +732,9 @@ 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];
bool captcomp_enabled = timer_configs[timer_index].captcomp_channels[timer_channel_index];

if (channel_initialized) {
if (channel_initialized && captcomp_enabled) {
*erpm = _erpms[timer_channel_index];
return PX4_OK;
}
Expand All @@ -683,6 +761,14 @@ void up_bdshot_status(void)
{
PX4_INFO("dshot driver stats:");

if (_bidirectional) {
PX4_INFO("Bidirectional DShot enabled");
PX4_INFO("Available DMA: %u", _num_dma_available);
if (_num_dma_available < 4) {
PX4_INFO("Round robin 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++) {
Expand Down Expand Up @@ -761,33 +847,31 @@ unsigned calculate_period(uint8_t timer_index, uint8_t channel_index)
uint32_t value = 0;
uint32_t high = 1; // We start off with high
unsigned shifted = 0;
unsigned previous = 0;

// Loop through the capture buffer for the specified channel
for (unsigned i = 1; i < CHANNEL_CAPTURE_BUFF_SIZE; ++i) {
// We can ignore the very first data point as it's the pulse before it starts.
unsigned previous = dshot_capture_buffer[channel_index][1];

// We can ignore the very first data point as it's the pulse before it starts.
if (i > 1) {

if (dshot_capture_buffer[channel_index][i] == 0) {
// Once we get zeros we're through
break;
}
// Loop through the capture buffer for the specified channel
for (unsigned i = 2; i < CHANNEL_CAPTURE_BUFF_SIZE; ++i) {

// This seemss to work with dshot 150, 300, 600, 1200
// The values were found by trial and error to get the quantization just right.
const uint32_t bits = (dshot_capture_buffer[channel_index][i] - previous + 5) / 20;
if (dshot_capture_buffer[channel_index][i] == 0) {
// Once we get zeros we're through
break;
}

// Shift the bits into the value
for (unsigned bit = 0; bit < bits; ++bit) {
value = (value << 1) | high;
++shifted;
}
// This seemss to work with dshot 150, 300, 600, 1200
// The values were found by trial and error to get the quantization just right.
const uint32_t bits = (dshot_capture_buffer[channel_index][i] - previous + 5) / 20;

// The next edge toggles.
high = !high;
// Convert GCR encoded pulse train into value
for (unsigned bit = 0; bit < bits; ++bit) {
value = (value << 1) | high;
++shifted;
}

// The next edge toggles.
high = !high;

previous = dshot_capture_buffer[channel_index][i];
}

Expand Down

0 comments on commit 9a29a2d

Please sign in to comment.