Skip to content

Commit

Permalink
dshot: read eRPM in parallel for 4 channels
Browse files Browse the repository at this point in the history 
Instead of reading the RPMs in round robin style, we now read out all 4
channels of timer 5 (that's on Pixhawk 6X) in parallel. This requires
disabling PX4IO as well as the IMU on SPI2.
  • Loading branch information
@julianoes @dagar
julianoes authored and dagar committed Dec 12, 2022
1 parent 5cd70c9 commit e57526a
Show file tree
Hide file tree
Showing 7 changed files with 142 additions and 90 deletions.
2 changes: 1 addition & 1 deletion boards/px4/fmu-v6x/default.px4board
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
CONFIG_DRIVERS_POWER_MONITOR_INA228=y
CONFIG_DRIVERS_POWER_MONITOR_INA238=y
CONFIG_DRIVERS_PWM_OUT=y
CONFIG_DRIVERS_PX4IO=y
#CONFIG_DRIVERS_PX4IO=y
CONFIG_DRIVERS_RC_INPUT=y
CONFIG_DRIVERS_SAFETY_BUTTON=y
CONFIG_DRIVERS_TONE_ALARM=y
Expand Down
2 changes: 1 addition & 1 deletion boards/px4/fmu-v6x/init/rc.board_sensors
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ else
fi

# Internal SPI bus ICM42688p
icm42688p -R 6 -s start
#icm42688p -R 6 -s start # disabled for DMA for Dshot

if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004
then
Expand Down
16 changes: 8 additions & 8 deletions boards/px4/fmu-v6x/nuttx-config/include/board_dma_map.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,28 +39,28 @@
#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* 1 DMA1:37 ICM-20649 */
#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* 2 DMA1:38 ICM-20649 */

#define DMAMAP_SPI2_RX DMAMAP_DMA12_SPI2RX_0 /* 3 DMA1:39 ICM-42688-P */
#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* 4 DMA1:40 ICM-42688-P */
//#define DMAMAP_SPI2_RX DMAMAP_DMA12_SPI2RX_0 /* 3 DMA1:39 ICM-42688-P */
//#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* 4 DMA1:40 ICM-42688-P */

//#define DMAMAP_USART1_RX DMAMAP_DMA12_USART1RX_0 /* DMA1:41 GPS1 */
//#define DMAMAP_USART1_TX DMAMAP_DMA12_USART1TX_0 /* DMA1:42 GPS1 */

//#define DMAMAP_USART2_RX DMAMAP_DMA12_USART2RX_0 /* DMA1:43 Telem3 */
//#define DMAMAP_USART2_TX DMAMAP_DMA12_USART2TX_0 /* DMA1:44 Telem3 */

//#define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_0 /* DMA1:45 DEBUG */
//#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_0 /* DMA1:46 DEBUG */
#define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_0 /* DMA1:45 DEBUG */
#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_0 /* DMA1:46 DEBUG */

//#define DMAMAP_UART4_RX DMAMAP_DMA12_UART4RX_0 /* DMA1:63 EXT2 */
//#define DMAMAP_UART4_TX DMAMAP_DMA12_UART4TX_0 /* DMA1:64 EXT2 */

#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_0 /* 5 DMA1:71 PX4IO */
#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_0 /* 6 DMA1:72 PX4IO */
//#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_0 /* 5 DMA1:71 PX4IO --> not used */
//#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_0 /* 6 DMA1:72 PX4IO --> not used */

// Assigned in timer_config.cpp

// Timer 4 /* 7 DMA1:32 TIM4UP */
// Timer 5 /* 8 DMA1:50 TIM5UP */
// Timer 4 /* 7 DMA1:32 TIM4UP --> uses up to 4 channels */
// Timer 5 /* 8 DMA1:50 TIM5UP --> not configured */

// DMAMUX2 Using at most 8 Channels on DMA2 -------- Assigned
// V
Expand Down
2 changes: 0 additions & 2 deletions boards/px4/fmu-v6x/nuttx-config/nsh/defconfig
View file
Original file line number Diff line number Diff line change
Expand Up @@ -254,8 +254,6 @@ CONFIG_STM32H7_SPI1=y
CONFIG_STM32H7_SPI1_DMA=y
CONFIG_STM32H7_SPI1_DMA_BUFFER=1024
CONFIG_STM32H7_SPI2=y
CONFIG_STM32H7_SPI2_DMA=y
CONFIG_STM32H7_SPI2_DMA_BUFFER=4096
CONFIG_STM32H7_SPI3=y
CONFIG_STM32H7_SPI3_DMA=y
CONFIG_STM32H7_SPI3_DMA_BUFFER=1024
Expand Down
2 changes: 1 addition & 1 deletion boards/px4/fmu-v6x/src/timer_config.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@

constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
initIOTimer(Timer::Timer5, DMA{DMA::Index1}),
initIOTimer(Timer::Timer4, DMA{DMA::Index1}),
initIOTimer(Timer::Timer4),
initIOTimer(Timer::Timer12),
initIOTimer(Timer::Timer1),
initIOTimer(Timer::Timer2),
Expand Down
202 changes: 130 additions & 72 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 @@ -90,13 +90,14 @@ static uint8_t dshot_burst_buffer_array[DSHOT_TIMERS * DSHOT_BURST_BUFFER_SIZE(M
px4_cache_aligned_data() = {};
static uint32_t *dshot_burst_buffer[DSHOT_TIMERS] = {};

static uint16_t dshot_capture_buffer[32] px4_cache_aligned_data() = {};
static dshot_handler_t dshot_capture_handlers[4] = {};
static uint16_t dshot_capture_buffers[4][32] px4_cache_aligned_data() = {};

static struct hrt_call _call;

static void do_capture(DMA_HANDLE handle, uint8_t status, void *arg);
static void process_capture_results(void *arg);
static unsigned calculate_period(void);
static unsigned calculate_period(unsigned channel);
static int dshot_output_timer_init(unsigned channel);

static uint32_t read_ok = 0;
Expand All @@ -111,12 +112,16 @@ static int _timers_init_mask = 0;
static int _channels_init_mask = 0;

// We only support capture on the first timer (usually 4 channels) for now.
static uint32_t _motor_to_capture = 0;
static uint32_t _erpms[4] = {};

static void(*_erpm_callback)(uint32_t[], size_t, void *) = NULL;
static void *_erpm_callback_context = NULL;

// We use this state to make sure the steps are never executed out of order which
// can lead to segfaults if a dma_handle is suddenly NULL.
enum State {None, Trigger, Capture, Collect};
static enum State _state = None;

uint8_t nibbles_from_mapped(uint8_t mapped)
{
switch (mapped) {
Expand Down Expand Up @@ -174,40 +179,46 @@ uint8_t nibbles_from_mapped(uint8_t mapped)
}
}

unsigned calculate_period(void)
unsigned calculate_period(unsigned channel)
{
uint32_t value = 0;

// We start off with high
uint32_t high = 1;

unsigned shifted = 0;
unsigned previous = 0;

for (unsigned i = 1; i < (32); ++i) {
for (unsigned i = 1; i < sizeof(dshot_capture_buffers[channel]) / sizeof(dshot_capture_buffers[channel][0]); ++i) {

// We can ignore the very first data point as it's the pulse before it starts.
if (i > 1) {
unsigned previous = dshot_capture_buffers[channel][i - 1];

if (dshot_capture_buffer[i] == 0) {
// Once we get zeros we're through.
break;
}
// TODO: Doesn't cope with DShot frequency other than 600
const uint32_t bits = (dshot_capture_buffers[channel][i] - previous + 5) / 20;

// TODO: Doesn't cope with DShot frequency other than 600
const uint32_t bits = (dshot_capture_buffer[i] - previous + 5) / 20;
if (shifted == 0 && (bits > 5 || bits == 0)) {
// Too long or too short, lets ignore the beginning until it's usable.
continue;
}

for (unsigned bit = 0; bit < bits; ++bit) {
value = value << 1;
value |= high;
++shifted;
}
if (dshot_capture_buffers[channel][i] == 0) {
// Once we get zeros we're through.
break;
}

// The next edge toggles.
high = !high;
if (bits > 10) {
// Something is not right, let's not waste cycles and get out of here.
++read_fail_zero;
return 0;
}

previous = dshot_capture_buffer[i];
for (unsigned bit = 0; bit < bits; ++bit) {
value = value << 1;
value |= high;
++shifted;
}

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

if (shifted == 0) {
Expand Down Expand Up @@ -344,6 +355,11 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)

void up_dshot_trigger(void)
{
if (_state != None) {
return;
}

_state = Trigger;

for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) {
if (_channels_init_mask & (1 << channel)) {
Expand All @@ -356,6 +372,9 @@ void up_dshot_trigger(void)
}
}

io_timer_set_enable(true, enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot,
IO_TIMER_ALL_MODES_CHANNELS);

for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) {
if (_timers_init_mask & (1 << timer)) {

Expand Down Expand Up @@ -390,12 +409,11 @@ void up_dshot_trigger(void)
io_timer_update_dma_req(timer, false);
// Trigger DMA (DShot Outputs)
stm32_dmastart(dshot_handler[timer].dma_handle, do_capture, NULL, false);

io_timer_update_dma_req(timer, true);
}
}

io_timer_set_enable(true, enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot,
IO_TIMER_ALL_MODES_CHANNELS);
}

void do_capture(DMA_HANDLE handle, uint8_t status, void *arg)
Expand All @@ -404,6 +422,12 @@ void do_capture(DMA_HANDLE handle, uint8_t status, void *arg)
(void)status;
(void)arg;

if (_state != Trigger) {
return;
}

_state = Capture;

for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) {
if (_timers_init_mask & (1 << timer)) {
if (dshot_handler[timer].dma_handle != NULL) {
Expand All @@ -412,54 +436,76 @@ void do_capture(DMA_HANDLE handle, uint8_t status, void *arg)
dshot_handler[timer].dma_handle = NULL;
}

// TODO: this doesn't scale to more than 4 motors yet
unsigned capture_channel = _motor_to_capture;
memset(dshot_capture_buffers, 0, sizeof(dshot_capture_buffers));
up_clean_dcache((uintptr_t) dshot_capture_buffers,
(uintptr_t) dshot_capture_buffers +
sizeof(dshot_capture_buffers));

dshot_handler[timer].dma_size = sizeof(dshot_capture_buffer);
io_timer_capture_update_dma_req(timer, false);

// TODO: We should probably do this at another level board specific.
// However, right now the dma handles are all hard-coded to the UP(date) source
// rather than the capture compare one.
unsigned timer_channel = timer_io_channels[capture_channel].timer_channel;
for (unsigned channel = 0; channel < 4; ++channel) {
dshot_capture_handlers[channel].dma_size = sizeof(dshot_capture_buffers[channel]);

switch (timer_channel) {
case 1:
dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH1_0);
break;
// TODO: We should probably do this at another level board specific.
// However, right now the dma handles are all hard-coded to the UP(date) source
// rather than the capture compare one.
unsigned timer_channel = timer_io_channels[channel].timer_channel;

case 2:
dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH2_0);
break;
switch (timer_channel) {
case 1:
dshot_capture_handlers[channel].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH1_0);
break;

case 3:
dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH3_0);
break;
case 2:
dshot_capture_handlers[channel].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH2_0);
break;

case 4:
dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH4_0);
break;
}
case 3:
dshot_capture_handlers[channel].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH3_0);
break;

memset(dshot_capture_buffer, 0, sizeof(dshot_capture_buffer));
up_clean_dcache((uintptr_t) dshot_capture_buffer,
(uintptr_t) dshot_capture_buffer +
sizeof(dshot_capture_buffer));
case 4:
dshot_capture_handlers[channel].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH4_0);
break;
}

px4_stm32_dmasetup(dshot_handler[timer].dma_handle,
io_timers[timer].base + STM32_GTIM_DMAR_OFFSET,
(uint32_t) dshot_capture_buffer,
sizeof(dshot_capture_buffer),
DSHOT_TELEMETRY_DMA_SCR);
io_timer_unallocate_channel(channel);

io_timer_unallocate_channel(capture_channel);
io_timer_channel_init(capture_channel, IOTimerChanMode_CaptureDMA, NULL, NULL);
io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, 1 << capture_channel);
unsigned offset = 0;

up_input_capture_set(capture_channel, Both, 0, NULL, NULL);
switch (timer_io_channels[channel].timer_channel) {
case 1:
offset = STM32_GTIM_CCR1_OFFSET;
break;

case 2:
offset = STM32_GTIM_CCR2_OFFSET;
break;

case 3:
offset = STM32_GTIM_CCR3_OFFSET;
break;

case 4:
offset = STM32_GTIM_CCR4_OFFSET;
break;
}

px4_stm32_dmasetup(dshot_capture_handlers[channel].dma_handle,
io_timers[timer].base + offset,
(uint32_t) dshot_capture_buffers[channel],
sizeof(dshot_capture_buffers[channel]),
DSHOT_TELEMETRY_DMA_SCR);

io_timer_channel_init(channel, IOTimerChanMode_CaptureDMA, NULL, NULL);
io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, 1 << channel);

up_input_capture_set(channel, Both, 0, NULL, NULL);

io_timer_set_capture_mode(timer, _dshot_frequency, channel);
stm32_dmastart(dshot_capture_handlers[channel].dma_handle, NULL, NULL, false);
}

io_timer_capture_update_dma_req(timer, false);
io_timer_set_capture_mode(timer, _dshot_frequency, capture_channel);
stm32_dmastart(dshot_handler[timer].dma_handle, NULL, NULL, false);
io_timer_capture_update_dma_req(timer, true);
}
}
Expand All @@ -472,26 +518,38 @@ void process_capture_results(void *arg)
{
(void)arg;

if (_state != Capture) {
return;
}

_state = Collect;


up_invalidate_dcache((uintptr_t)dshot_capture_buffers,
(uintptr_t)dshot_capture_buffers +
sizeof(dshot_capture_buffers));

// In case DMA is still set up from the last capture, we clear that.
for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) {
if (_timers_init_mask & (1 << timer)) {
if (dshot_handler[timer].dma_handle != NULL) {
stm32_dmastop(dshot_handler[timer].dma_handle);
stm32_dmafree(dshot_handler[timer].dma_handle);
dshot_handler[timer].dma_handle = NULL;

for (unsigned channel = 0; channel < 4; ++channel) {
if (dshot_capture_handlers[channel].dma_handle != NULL) {
stm32_dmastop(dshot_capture_handlers[channel].dma_handle);
stm32_dmafree(dshot_capture_handlers[channel].dma_handle);
dshot_capture_handlers[channel].dma_handle = NULL;
}
}
}
}

up_invalidate_dcache((uintptr_t)dshot_capture_buffer,
(uintptr_t)dshot_capture_buffer +
sizeof(dshot_capture_buffer));

// TODO: fix order
// TODO: convert from period to erpm
_erpms[_motor_to_capture] = calculate_period();

for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) {
for (unsigned channel = 0; channel < 4; channel++) {

_erpms[channel] = calculate_period(channel);

if (_channels_init_mask & (1 << channel)) {
io_timer_unallocate_channel(channel);
io_timer_channel_init(channel,
Expand All @@ -503,7 +561,7 @@ void process_capture_results(void *arg)
_erpm_callback(_erpms, 4, _erpm_callback_context);
}

_motor_to_capture = (_motor_to_capture + 1) % 4;
_state = None;
}


Expand Down
Loading

0 comments on commit e57526a

Please sign in to comment.