diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index 03efaef592..1823bf65f6 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -72,7 +72,6 @@ DMA_SCR_DIR_P2M | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) typedef struct dshot_handler_t { - bool init; DMA_HANDLE dma_handle; uint32_t dma_size; } dshot_handler_t; @@ -91,13 +90,13 @@ px4_cache_aligned_data() = {}; static uint32_t *dshot_burst_buffer[DSHOT_TIMERS] = {}; static uint16_t dshot_capture_buffer[32] px4_cache_aligned_data() = {}; -static size_t dshot_capture_buffer_size = sizeof(dshot_capture_buffer) * sizeof(dshot_capture_buffer[0]); 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 int dshot_output_timer_init(unsigned channel); static uint32_t read_ok = 0; static uint32_t read_fail_nibble = 0; @@ -107,9 +106,12 @@ static uint32_t read_fail_zero = 0; static bool enable_bidirectional_dshot = true; static uint32_t _dshot_frequency = 0; +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] = {0, 0, 0, 0}; +static uint32_t _erpms[4] = {}; static void(*_erpm_callback)(uint32_t[], size_t, void *) = NULL; static void *_erpm_callback_context = NULL; @@ -191,6 +193,7 @@ unsigned calculate_period(void) break; } + // TODO: Doesn't cope with DShot frequency other than 600 const uint32_t bits = (dshot_capture_buffer[i] - previous + 5) / 20; for (unsigned bit = 0; bit < bits; ++bit) { @@ -252,48 +255,26 @@ unsigned calculate_period(void) return period; } +int dshot_output_timer_init(unsigned channel) +{ + io_timer_unallocate_channel(channel); + int ret = io_timer_channel_init(channel, + enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, NULL, NULL); + + if (ret == -EBUSY) { + // either timer or channel already used - this is not fatal + return OK; + + } else { + return ret; + } +} int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) { _dshot_frequency = dshot_pwm_freq; - unsigned buffer_offset = 0; - - for (int timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) { - dshot_handler[timer_index].init = false; - } - - for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { - if (io_timers[timer].base == 0) { // no more timers configured - break; - } - - // we know the uint8_t* cast to uint32_t* is fine, since we're aligned to cache line size -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wcast-align" - dshot_burst_buffer[timer] = (uint32_t *)&dshot_burst_buffer_array[buffer_offset]; -#pragma GCC diagnostic pop - buffer_offset += DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count_including_gaps); - - if (buffer_offset > sizeof(dshot_burst_buffer_array)) { - return -EINVAL; // something is wrong with the board configuration or some other logic - } - } - - int channels_init_mask = 0xf; - int ret_val = 0; - - return ret_val == OK ? channels_init_mask : ret_val; -} - -void up_dshot_trigger(void) -{ - /* Init channels */ - int ret_val = OK; - int channel_mask = 0xf; - int channels_init_mask = 0; - - for (unsigned channel = 0; (channel_mask != 0) && (channel < MAX_TIMER_IO_CHANNELS) && (OK == ret_val); channel++) { + for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { if (channel_mask & (1 << channel)) { uint8_t timer = timer_io_channels[channel].timer_index; @@ -301,52 +282,102 @@ void up_dshot_trigger(void) continue; } - ret_val = io_timer_unallocate_channel(channel); - ret_val = io_timer_channel_init(channel, - enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, NULL, NULL); - channel_mask &= ~(1 << channel); + int ret = dshot_output_timer_init(channel); - if (OK == ret_val) { - dshot_handler[timer].init = true; - channels_init_mask |= 1 << channel; + if (ret != OK) { + return ret; + } - } else if (ret_val == -EBUSY) { - /* either timer or channel already used - this is not fatal */ - ret_val = 0; + _channels_init_mask |= (1 << channel); + _timers_init_mask |= (1 << timer); + } + } + + for (uint8_t timer = 0; timer < MAX_IO_TIMERS; ++timer) { + if (_timers_init_mask & (1 << timer)) { + if (dshot_handler[timer].dma_handle == NULL) { + dshot_handler[timer].dma_size = io_timers_channel_mapping.element[timer].channel_count_including_gaps * + ONE_MOTOR_BUFF_SIZE; + + io_timer_set_dshot_mode(timer, _dshot_frequency, + io_timers_channel_mapping.element[timer].channel_count_including_gaps); + + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap); + + if (NULL == dshot_handler[timer].dma_handle) { + // TODO: how to log this? + return -ENOSR; + } + + // We have tested this now but will anyway initialize it again during a trigger, so we can free it again. + stm32_dmafree(dshot_handler[timer].dma_handle); + dshot_handler[timer].dma_handle = NULL; } } } - for (uint8_t timer_index = 0; (timer_index < 1) && (OK == ret_val); timer_index++) { + unsigned buffer_offset = 0; - if (true == dshot_handler[timer_index].init) { - dshot_handler[timer_index].dma_size = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps * - ONE_MOTOR_BUFF_SIZE; - io_timer_set_dshot_mode(timer_index, _dshot_frequency, - io_timers_channel_mapping.element[timer_index].channel_count_including_gaps); - - - if (dshot_handler[timer_index].dma_handle != NULL) { - stm32_dmafree(dshot_handler[timer_index].dma_handle); + for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { + if (_timers_init_mask & (1 << timer)) { + if (io_timers[timer].base == 0) { // no more timers configured + break; } - dshot_handler[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dmamap); - } + // we know the uint8_t* cast to uint32_t* is fine, since we're aligned to cache line size +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-align" + dshot_burst_buffer[timer] = (uint32_t *) &dshot_burst_buffer_array[buffer_offset]; +#pragma GCC diagnostic pop + buffer_offset += DSHOT_BURST_BUFFER_SIZE( + io_timers_channel_mapping.element[timer].channel_count_including_gaps); - - if (NULL == dshot_handler[timer_index].dma_handle) { - ret_val = ERROR; + if (buffer_offset > sizeof(dshot_burst_buffer_array)) { + return -EINVAL; // something is wrong with the board configuration or some other logic + } } } - for (uint8_t timer = 0; (timer < 1); timer++) { + return _channels_init_mask; +} - if (true == dshot_handler[timer].init) { +void up_dshot_trigger(void) +{ + + for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (_channels_init_mask & (1 << channel)) { + int ret = dshot_output_timer_init(channel); + + if (ret != OK) { + // TODO: what to do here? + return; + } + } + } + + for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { + if (_timers_init_mask & (1 << timer)) { + + if (dshot_handler[timer].dma_handle == NULL) { + dshot_handler[timer].dma_size = io_timers_channel_mapping.element[timer].channel_count_including_gaps * + ONE_MOTOR_BUFF_SIZE; + + io_timer_set_dshot_mode(timer, _dshot_frequency, + io_timers_channel_mapping.element[timer].channel_count_including_gaps); + + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap); + + if (NULL == dshot_handler[timer].dma_handle) { + // TODO: how to log this? + return; + } + } // Flush cache so DMA sees the data - up_clean_dcache((uintptr_t)dshot_burst_buffer[timer], - (uintptr_t)dshot_burst_buffer[timer] + - DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count_including_gaps)); + up_clean_dcache((uintptr_t) dshot_burst_buffer[timer], + (uintptr_t) dshot_burst_buffer[timer] + + DSHOT_BURST_BUFFER_SIZE( + io_timers_channel_mapping.element[timer].channel_count_including_gaps)); px4_stm32_dmasetup(dshot_handler[timer].dma_handle, io_timers[timer].base + STM32_GTIM_DMAR_OFFSET, @@ -372,77 +403,66 @@ void do_capture(DMA_HANDLE handle, uint8_t status, void *arg) (void)status; (void)arg; - if (_motor_to_capture >= 4) { - // We only support the first 4 for now. - return; - } - - // TODO: this things are still somewhat hard-coded. And I'm probably confused - // regarding channels and indexes. - const int capture_timer = 0; - const int capture_channel = _motor_to_capture; - for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { - if (dshot_handler[timer].dma_handle != NULL) { - stm32_dmastop(dshot_handler[timer].dma_handle); + 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; + } + + // TODO: this doesn't scale to more than 4 motors yet + unsigned capture_channel = _motor_to_capture; + + dshot_handler[timer].dma_size = sizeof(dshot_capture_buffer); + + // 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; + + switch (timer_channel) { + case 1: + dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH1_0); + break; + + case 2: + dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH2_0); + break; + + case 3: + dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH3_0); + break; + + case 4: + dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH4_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)); + + 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(capture_channel); + io_timer_channel_init(capture_channel, IOTimerChanMode_CaptureDMA, NULL, NULL); + io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, 1 << capture_channel); + + up_input_capture_set(capture_channel, Both, 0, NULL, NULL); + + 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); } } - dshot_handler[capture_timer].dma_size = 32; - - if (dshot_handler[0].dma_handle != NULL) { - stm32_dmafree(dshot_handler[0].dma_handle); - } - - // 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. - switch (timer_io_channels[_motor_to_capture].timer_channel) { - case 1: - dshot_handler[capture_timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH1_0); - break; - - case 2: - dshot_handler[capture_timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH2_0); - break; - - case 3: - dshot_handler[capture_timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH3_0); - break; - - case 4: - dshot_handler[capture_timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH4_0); - break; - } - - memset(dshot_capture_buffer, 0, sizeof(dshot_capture_buffer)); - up_clean_dcache((uintptr_t)dshot_capture_buffer, - (uintptr_t)dshot_capture_buffer + - dshot_capture_buffer_size); - - up_invalidate_dcache((uintptr_t)dshot_capture_buffer, - (uintptr_t)dshot_capture_buffer + - dshot_capture_buffer_size); - - px4_stm32_dmasetup(dshot_handler[0].dma_handle, - io_timers[capture_timer].base + STM32_GTIM_DMAR_OFFSET, - (uint32_t)(&dshot_capture_buffer[0]), - 32, - DSHOT_TELEMETRY_DMA_SCR); - - // TODO: check retval? - 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); - - up_input_capture_set(capture_channel, Both, 0, NULL, NULL); - - io_timer_capture_update_dma_req(capture_timer, false); - io_timer_set_capture_mode(capture_timer, _dshot_frequency, capture_channel); - - stm32_dmastart(dshot_handler[capture_timer].dma_handle, NULL, NULL, false); - io_timer_capture_update_dma_req(capture_timer, true); - // It takes around 85 us for the ESC to respond, so we should have a result after 150 us, surely. hrt_call_after(&_call, 150, process_capture_results, NULL); } @@ -451,54 +471,37 @@ void process_capture_results(void *arg) { (void)arg; - up_invalidate_dcache((uintptr_t)dshot_capture_buffer, - (uintptr_t)dshot_capture_buffer + - dshot_capture_buffer_size); - - if (dshot_handler[0].dma_handle != NULL) { - - stm32_dmastop(dshot_handler[0].dma_handle); - } - - /* Init channels */ - int ret_val = OK; - int channel_mask = 0xf; - int channels_init_mask = 0; - - for (unsigned channel = 0; (channel_mask != 0) && (channel < MAX_TIMER_IO_CHANNELS) && (OK == ret_val); channel++) { - if (channel_mask & (1 << channel)) { - uint8_t timer = timer_io_channels[channel].timer_index; - - if (io_timers[timer].dshot.dma_base == 0) { // board does not configure dshot on this timer - continue; - } - - ret_val = io_timer_unallocate_channel(channel); - ret_val = io_timer_channel_init(channel, - enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, NULL, NULL); - channel_mask &= ~(1 << channel); - - if (OK == ret_val) { - dshot_handler[timer].init = true; - channels_init_mask |= 1 << channel; - - } else if (ret_val == -EBUSY) { - /* either timer or channel already used - this is not fatal */ - ret_val = 0; + // 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; } } } + 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(); - if (_motor_to_capture == 3) { - if (_erpm_callback != NULL) { - _erpm_callback(_erpms, 4, _erpm_callback_context); + for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (_channels_init_mask & (1 << channel)) { + io_timer_unallocate_channel(channel); + io_timer_channel_init(channel, + enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, NULL, NULL); } } + if (_erpm_callback != NULL) { + _erpm_callback(_erpms, 4, _erpm_callback_context); + } + _motor_to_capture = (_motor_to_capture + 1) % 4; } @@ -534,6 +537,12 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet } unsigned timer = timer_io_channels[motor_number].timer_index; + + // If this timer is not initialized, we give up here. + if (!(_timers_init_mask & (1 << timer))) { + return; + } + uint32_t *buffer = dshot_burst_buffer[timer]; const io_timers_channel_mapping_element_t *mapping = &io_timers_channel_mapping.element[timer]; unsigned num_motors = mapping->channel_count_including_gaps;