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 ad100c47d246..d6bf3253afad 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -48,28 +48,23 @@ #include #include #include -#include #define MOTOR_PWM_BIT_1 14u #define MOTOR_PWM_BIT_0 7u -#define DSHOT_TIMERS MAX_IO_TIMERS -#define MOTORS_NUMBER DIRECT_PWM_OUTPUT_CHANNELS #define ONE_MOTOR_DATA_SIZE 16u -#define CHANNEL_OUTPUT_BUFF_SIZE 17u -#define ALL_MOTORS_BUF_SIZE (MOTORS_NUMBER * CHANNEL_OUTPUT_BUFF_SIZE) +#define CHANNEL_OUTPUT_BUFF_SIZE 17u #define DSHOT_THROTTLE_POSITION 5u #define DSHOT_TELEMETRY_POSITION 4u #define NIBBLES_SIZE 4u #define DSHOT_NUMBER_OF_NIBBLES 3u -#define DSHOT_END_OF_STREAM 16u #define MAX_NUM_CHANNELS_PER_TIMER 4u // CCR1-CCR4 #define DSHOT_DMA_SCR (DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | \ - DMA_SCR_DIR_M2P | DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) + DMA_SCR_DIR_M2P | DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) -// TODO: why 16bit? +// 16-bit because not all of the General Purpose Timers support 32-bit #define DSHOT_BIDIRECTIONAL_DMA_SCR (DMA_SCR_PRIHI | DMA_SCR_MSIZE_16BITS | DMA_SCR_PSIZE_16BITS | DMA_SCR_MINC | \ - DMA_SCR_DIR_P2M | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) + DMA_SCR_DIR_P2M | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) typedef struct dshot_handler_t { DMA_HANDLE dma_up_handle; // DMA stream for DMA update @@ -84,182 +79,42 @@ typedef struct dshot_handler_t { #else #define DMA_ALIGN_UP(n) (n) #endif -#define DSHOT_BURST_BUFFER_SIZE(channel_count) (DMA_ALIGN_UP(sizeof(uint32_t)*CHANNEL_OUTPUT_BUFF_SIZE*channel_count)) +#define DSHOT_OUTPUT_BUFFER_SIZE(channel_count) (DMA_ALIGN_UP(sizeof(uint32_t)*CHANNEL_OUTPUT_BUFF_SIZE*channel_count)) -static dshot_handler_t dshot_handler[DSHOT_TIMERS] = {}; -static uint8_t dshot_burst_buffer_array[DSHOT_TIMERS * DSHOT_BURST_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)] +static dshot_handler_t dshot_handler[MAX_IO_TIMERS] = {}; +static uint8_t dshot_burst_buffer_array[MAX_IO_TIMERS * DSHOT_OUTPUT_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)] px4_cache_aligned_data() = {}; -static uint32_t *dshot_burst_buffer[DSHOT_TIMERS] = {}; +static uint32_t *dshot_output_buffer[MAX_IO_TIMERS] = {}; -static uint16_t dshot_capture_buffer[32] px4_cache_aligned_data() = {}; +// static uint16_t dshot_capture_buffer[32] px4_cache_aligned_data() = {}; +static uint16_t dshot_capture_buffer[MAX_TIMER_IO_CHANNELS][32] px4_cache_aligned_data() = {}; -// TODO: audit local variables -static struct hrt_call _call; -static void dma_callback_begin_capture(DMA_HANDLE handle, uint8_t status, void *arg); +static void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg); static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, void *arg); -static void reinitialize_timers_callback(void *arg); -static void process_capture_results(void *arg); -static unsigned calculate_period(void); +static void process_capture_results(int8_t output_channel); + +static unsigned calculate_period_from_erpm_frame(uint8_t* buffer, size_t buffer_size); static uint32_t read_ok = 0; static uint32_t read_fail_nibble = 0; static uint32_t read_fail_crc = 0; static uint32_t read_fail_zero = 0; -static uint32_t jakechannelcounter[8] = {}; +static int32_t jakechannelcounter[8] = { -7, -7, -7, -7, -7, -7, -7 , -7 }; +static uint32_t jakecounter = 0; +// We need local permanent memory for indices, channels, and callbacks for each output +static uint8_t timer_index_array[MAX_IO_TIMERS] = {}; +static uint8_t output_channel_array[MAX_TIMER_IO_CHANNELS] = {}; static bool _birectional = false; 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 int32_t _erpms[16] = {}; - - -uint8_t nibbles_from_mapped(uint8_t mapped) -{ - switch (mapped) { - case 0x19: - return 0x00; - - case 0x1B: - return 0x01; - - case 0x12: - return 0x02; - - case 0x13: - return 0x03; - - 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; - - default: - // Unknown mapped - return 0xFF; - } -} - -unsigned calculate_period(void) -{ - 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) { - - // We can ignore the very first data point as it's the pulse before it starts. - if (i > 1) { - - if (dshot_capture_buffer[i] == 0) { - // Once we get zeros we're through. - break; - } - - // 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[i] - previous + 5) / 20; - - for (unsigned bit = 0; bit < bits; ++bit) { - value = value << 1; - value |= high; - ++shifted; - } - - // The next edge toggles. - high = !high; - } - - previous = dshot_capture_buffer[i]; - } - - if (shifted == 0) { - // no data yet, or this time - ++read_fail_zero; - return 0; - } - - // We need to make sure we shifted 21 times. We might have missed some low "pulses" at the very end. - value = value << (21 - shifted); - - // Note: At 0 throttle, the value is 0x1AD6AE, so 0b110101101011010101110 - - // From GCR to eRPM according to: - // https://brushlesswhoop.com/dshot-and-bidirectional-dshot/#erpm-transmission - unsigned gcr = (value ^ (value >> 1)); - - uint32_t data = 0; - - // 20bits -> 5 mapped -> 4 nibbles - for (unsigned i = 0; i < 4; ++i) { - uint32_t nibble = nibbles_from_mapped(gcr & (0x1F)) << (4 * i); - - if (nibble == 0xff) { - ++read_fail_nibble; - return 0; - } - - data |= nibble; - gcr = gcr >> 5; - } - - unsigned shift = (data & 0xE000) >> 13; - unsigned period = ((data & 0x1FF0) >> 4) << shift; - unsigned crc = (data & 0xf); - - unsigned payload = (data & 0xFFF0) >> 4; - unsigned calculated_crc = (~(payload ^ (payload >> 4) ^ (payload >> 8))) & 0x0F; - - if (crc != calculated_crc) { - ++read_fail_crc; - return 0; - } - - ++read_ok; - return period; -} +static int32_t _erpms[MAX_TIMER_IO_CHANNELS] = {}; int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot) { @@ -316,7 +171,6 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi // This was just for testing. We will initialize again at run time. stm32_dmafree(dshot_handler[timer_index].dma_up_handle); dshot_handler[timer_index].dma_up_handle = NULL; - PX4_INFO("allocated and freed DMA for timer %u UP", timer_index); } } } @@ -331,7 +185,6 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi DMA_HANDLE* dma_handle = &dshot_handler[timer_index].dma_ch_handle[timer_channel_index]; uint32_t dma_map_ch = io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]; if (dma_map_ch) { - PX4_INFO("allocating timer %u DMA CH%u handle, output_channel %u", timer_index, timer_channel_index, output_channel); *dma_handle = stm32_dmachannel(dma_map_ch); if (*dma_handle == NULL) { PX4_INFO("could not allocate timer %u DMA CH%u handle, output_channel %u", timer_index, timer_channel_index, output_channel); @@ -339,20 +192,16 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi } // This was just for testing. We will initialize again at run time. - PX4_INFO("allocated timer %u DMA CH%u handle, output_channel %u", timer_index, timer_channel_index, output_channel); stm32_dmafree(*dma_handle); *dma_handle = NULL; - PX4_INFO("FREEEEE"); } } } - px4_usleep(500000); - // TODO: what are we doing here and why can't this be defined at compile time? unsigned buffer_offset = 0; - for (unsigned timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) { + for (unsigned timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { if (_timers_init_mask & (1 << timer_index)) { if (io_timers[timer_index].base == 0) { // no more timers configured break; @@ -361,10 +210,10 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi // 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_index] = (uint32_t *) &dshot_burst_buffer_array[buffer_offset]; + dshot_output_buffer[timer_index] = (uint32_t *) &dshot_burst_buffer_array[buffer_offset]; #pragma GCC diagnostic pop uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; - buffer_offset += DSHOT_BURST_BUFFER_SIZE(channel_count); + buffer_offset += DSHOT_OUTPUT_BUFFER_SIZE(channel_count); if (buffer_offset > sizeof(dshot_burst_buffer_array)) { return -EINVAL; // something is wrong with the board configuration or some other logic @@ -395,18 +244,10 @@ void up_dshot_trigger(void) // Enable all timers configured as dshot (weird way of doing this...) io_timer_set_enable(true, _birectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, - IO_TIMER_ALL_MODES_CHANNELS); - - // Clear capture_buffer cache - if (_birectional) { - uintptr_t buf = (uintptr_t)dshot_capture_buffer; - size_t size = sizeof(dshot_capture_buffer); - memset(dshot_capture_buffer, 0, size); - up_clean_dcache(buf, buf + size); - } + IO_TIMER_ALL_MODES_CHANNELS); // For each timer, begin DMA transmit - for (uint8_t timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) { + for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { if (_timers_init_mask & (1 << timer_index)) { uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; @@ -415,8 +256,6 @@ void up_dshot_trigger(void) if (dshot_handler[timer_index].dma_up_handle == NULL) { dshot_handler[timer_index].dma_size = channel_count * CHANNEL_OUTPUT_BUFF_SIZE; - io_timer_set_dshot_mode(timer_index, _dshot_frequency, channel_count); - dshot_handler[timer_index].dma_up_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up); if (dshot_handler[timer_index].dma_up_handle == NULL) { @@ -425,14 +264,17 @@ void up_dshot_trigger(void) } } + // Set timer in dshot mode + io_timer_set_dshot_mode(timer_index, _dshot_frequency, channel_count); + // Flush cache so DMA sees the data - up_clean_dcache((uintptr_t) dshot_burst_buffer[timer_index], - (uintptr_t) dshot_burst_buffer[timer_index] + - DSHOT_BURST_BUFFER_SIZE(channel_count)); + up_clean_dcache((uintptr_t) dshot_output_buffer[timer_index], + (uintptr_t) dshot_output_buffer[timer_index] + + DSHOT_OUTPUT_BUFFER_SIZE(channel_count)); px4_stm32_dmasetup(dshot_handler[timer_index].dma_up_handle, io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET, - (uint32_t)(dshot_burst_buffer[timer_index]), + (uint32_t)(dshot_output_buffer[timer_index]), dshot_handler[timer_index].dma_size, DSHOT_DMA_SCR); @@ -440,7 +282,8 @@ void up_dshot_trigger(void) io_timer_disable_update_dma_req(timer_index); // Trigger DMA (DShot Outputs) - stm32_dmastart(dshot_handler[timer_index].dma_up_handle, _birectional ? dma_callback_begin_capture : NULL, &timer_index, false); + timer_index_array[timer_index] = timer_index; + stm32_dmastart(dshot_handler[timer_index].dma_up_handle, _birectional ? dma_callback_capture_start : NULL, &timer_index_array[timer_index], false); // Enable DMA update request io_timer_enable_update_dma_req(timer_index); @@ -448,39 +291,56 @@ void up_dshot_trigger(void) } } -void dma_callback_begin_capture(DMA_HANDLE handle, uint8_t status, void *arg) +void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) { - (void)handle; (void)status; uint8_t timer_index = *((uint8_t*)arg); - // Clear DMA UP configuration - if (dshot_handler[timer_index].dma_up_handle != NULL) { - stm32_dmastop(dshot_handler[timer_index].dma_up_handle); - stm32_dmafree(dshot_handler[timer_index].dma_up_handle); - dshot_handler[timer_index].dma_up_handle = NULL; - } + // // Clear DMA UP configuration + stm32_dmastop(handle); + stm32_dmafree(handle); + + // NOTE: The dma_up_handle is now invalid // Disable DMA capture request io_timer_disable_capture_dma_req(timer_index); // Allocate DMA for all channels on this timer - for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; bool channel_enabled = _channels_init_mask & (1 << output_channel); + // if ((output_channel != 1) && (output_channel != 2) && (output_channel != 3)) { + // continue; + // } + if (is_this_timer && channel_enabled) { - uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel -1; - dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]); + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + + // Allocate DMA + if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] == NULL) { + dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]); + } + + // If DMA handler is valid, start DMA + if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] == NULL) { + PX4_INFO("failed to allocate dma for timer %u channel %u", timer_index, timer_channel_index); + continue; + } + + // Flush cache so DMA sees the data + memset(dshot_capture_buffer[output_channel], 0, sizeof(dshot_capture_buffer[output_channel])); + up_clean_dcache((uintptr_t)dshot_capture_buffer[output_channel], + (uintptr_t)dshot_capture_buffer[output_channel] + + sizeof(dshot_capture_buffer[output_channel])); // Setup DMA for this channel px4_stm32_dmasetup(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET, // DMA address for burst mode (16-bit, TIM2-5 only) - (uint32_t) dshot_capture_buffer, - sizeof(dshot_capture_buffer), + (uint32_t) dshot_capture_buffer[output_channel], + sizeof(dshot_capture_buffer[output_channel]), DSHOT_BIDIRECTIONAL_DMA_SCR); // Reset / Enable timer channel @@ -495,7 +355,8 @@ void dma_callback_begin_capture(DMA_HANDLE handle, uint8_t status, void *arg) io_timer_set_dshot_capture_mode(timer_index, _dshot_frequency, output_channel); // Start DMA with callback if bidi dshot is enabled - stm32_dmastart(dshot_handler[timer_index].dma_up_handle, dma_callback_capture_complete, &output_channel, false); + output_channel_array[output_channel] = output_channel; + stm32_dmastart(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], dma_callback_capture_complete, &output_channel_array[output_channel], false); } } @@ -505,77 +366,42 @@ void dma_callback_begin_capture(DMA_HANDLE handle, uint8_t status, void *arg) static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, void *arg) { - unsigned output_channel = *((unsigned*)arg); - - jakechannelcounter[output_channel]++; + // TODO: capture complete callback firing too early? Data is zero... + uint8_t output_channel = *((uint8_t*)arg); - hrt_call_after(&_call, 1, reinitialize_timers_callback, &output_channel); -} - -void reinitialize_timers_callback(void *arg) -{ - unsigned output_channel = *((unsigned*)arg); + stm32_dmastop(handle); + stm32_dmafree(handle); - uint8_t timer_index = timer_io_channels[output_channel].timer_index; - uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + up_invalidate_dcache((uintptr_t)dshot_capture_buffer[output_channel], + (uintptr_t)dshot_capture_buffer[output_channel] + + sizeof(dshot_capture_buffer[output_channel])); - stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); - stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); - dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; + process_capture_results(output_channel); + // TODO: do we need to unallocate / reinit timers here? It's done at the start.. io_timer_unallocate_channel(output_channel); io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); } -void process_capture_results(void *arg) +void process_capture_results(int8_t output_channel) { - (void)arg; - - // 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_up_handle != NULL) { - stm32_dmastop(dshot_handler[timer].dma_up_handle); - stm32_dmafree(dshot_handler[timer].dma_up_handle); - dshot_handler[timer].dma_up_handle = NULL; - } - } - } - - up_invalidate_dcache((uintptr_t)dshot_capture_buffer, - (uintptr_t)dshot_capture_buffer + - sizeof(dshot_capture_buffer)); + jakechannelcounter[output_channel] = output_channel; + jakecounter++; - const unsigned period = calculate_period(); + const unsigned period = calculate_period_from_erpm_frame((uint8_t*)dshot_capture_buffer[output_channel], sizeof(dshot_capture_buffer[output_channel])); if (period == 0) { // If the parsing failed, we get 0. - _erpms[_motor_to_capture] = 0; + _erpms[output_channel] = 0; } else if (period == 65408) { // For still, we get this magic 65408 value. - _erpms[_motor_to_capture] = 0; + _erpms[output_channel] = 0; } else { // from period in us to eRPM - _erpms[_motor_to_capture] = 1000000 * 60 / period; - } - - 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, IOTimerChanMode_DshotInverted, NULL, NULL); - } + _erpms[output_channel] = 1000000 * 60 / period; } - - // if (_erpm_callback != NULL) { - // // Only publish every 4th time once all measurements have come in. - // if (_motor_to_capture == 3) { - // _erpm_callback(_erpms, 4, _erpm_callback_context); - // } - // } - - _motor_to_capture = (_motor_to_capture + 1) % 4; } /** @@ -615,7 +441,7 @@ void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) unsigned timer = timer_io_channels[channel].timer_index; - uint32_t *buffer = dshot_burst_buffer[timer]; + uint32_t *buffer = dshot_output_buffer[timer]; const io_timers_channel_mapping_element_t *mapping = &io_timers_channel_mapping.element[timer]; unsigned num_motors = mapping->channel_count_including_gaps; unsigned timer_channel = timer_io_channels[channel].timer_channel - mapping->lowest_timer_channel; @@ -658,7 +484,151 @@ void up_bdshot_status(void) PX4_INFO("dshot driver stats: read %lu, failed nibble %lu, failed CRC %lu, invalid/zero %lu", read_ok, read_fail_nibble, read_fail_crc, read_fail_zero); - PX4_INFO("ch0 %lu, ch1 %lu, ch2 %lu, ch3 %lu,", jakechannelcounter[0], jakechannelcounter[1], jakechannelcounter[2], jakechannelcounter[3]); + // PX4_INFO("ch0 %ld, ch1 %ld, ch2 %ld, ch3 %ld,", _erpms[0], _erpms[1], _erpms[2], _erpms[3]); + + int32_t* jc = jakechannelcounter; + PX4_INFO("ch0 %ld, ch1 %ld, ch2 %ld, ch3 %ld, ch4 %ld, ch5 %ld, ch6 %ld, ch7 %ld,", jc[0], jc[1], jc[2], jc[3], jc[4], jc[5], jc[6], jc[7]); + PX4_INFO("jakecounter %lu", jakecounter); +} + +uint8_t nibbles_from_mapped(uint8_t mapped) +{ + switch (mapped) { + case 0x19: + return 0x00; + + case 0x1B: + return 0x01; + + case 0x12: + return 0x02; + + case 0x13: + return 0x03; + + 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; + + default: + // Unknown mapped + return 0xFF; + } +} + +unsigned calculate_period_from_erpm_frame(uint8_t* buffer, size_t buffer_size) +{ + uint32_t value = 0; + + // We start off with high + uint32_t high = 1; + + unsigned shifted = 0; + unsigned previous = 0; + + for (unsigned i = 1; i < buffer_size; ++i) { + + // We can ignore the very first data point as it's the pulse before it starts. + if (i > 1) { + + if (buffer[i] == 0) { + // Once we get zeros we're through. + break; + } + + // 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 = (buffer[i] - previous + 5) / 20; + + for (unsigned bit = 0; bit < bits; ++bit) { + value = value << 1; + value |= high; + ++shifted; + } + + // The next edge toggles. + high = !high; + } + + previous = buffer[i]; + } + + if (shifted == 0) { + // no data yet, or this time + ++read_fail_zero; + return 0; + } + + // We need to make sure we shifted 21 times. We might have missed some low "pulses" at the very end. + value = value << (21 - shifted); + + // Note: At 0 throttle, the value is 0x1AD6AE, so 0b110101101011010101110 + + // From GCR to eRPM according to: + // https://brushlesswhoop.com/dshot-and-bidirectional-dshot/#erpm-transmission + unsigned gcr = (value ^ (value >> 1)); + + uint32_t data = 0; + + // 20bits -> 5 mapped -> 4 nibbles + for (unsigned i = 0; i < 4; ++i) { + uint32_t nibble = nibbles_from_mapped(gcr & (0x1F)) << (4 * i); + + if (nibble == 0xff) { + ++read_fail_nibble; + return 0; + } + + data |= nibble; + gcr = gcr >> 5; + } + + unsigned shift = (data & 0xE000) >> 13; + unsigned period = ((data & 0x1FF0) >> 4) << shift; + unsigned crc = (data & 0xf); + + unsigned payload = (data & 0xFFF0) >> 4; + unsigned calculated_crc = (~(payload ^ (payload >> 4) ^ (payload >> 8))) & 0x0F; + + if (crc != calculated_crc) { + ++read_fail_crc; + return 0; + } + + ++read_ok; + return period; } #endif diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 9e9d91c905e7..940019d9bf04 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -534,8 +534,6 @@ void DShot::Run() if (_outputs_on != outputs_on) { enable_dshot_outputs(outputs_on); - PX4_INFO("DSHOT OUTPUTS ENABLED"); - px4_usleep(100'000); } if (_telemetry) {