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 5b501850668a..57550fe1cf08 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -1,7 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2019-2021 PX4 Development Team. All rights reserved. + * Copyright (C) 2019-2023 PX4 Development Team. All rights reserved. * Author: Igor Misic + * Author: Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,12 +40,17 @@ #include #include +#include #include #include #include #include #include +#include +#include +#include + #define MOTOR_PWM_BIT_1 14u #define MOTOR_PWM_BIT_0 7u @@ -63,8 +69,10 @@ #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) +#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) + typedef struct dshot_handler_t { - bool init; DMA_HANDLE dma_handle; uint32_t dma_size; } dshot_handler_t; @@ -82,36 +90,199 @@ 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] = {}; -int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) +static uint16_t dshot_capture_buffer[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 int dshot_output_timer_init(unsigned channel); +static int dshot_output_timer_deinit(unsigned channel); + +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 bool bidirectional_dshot_enabled = 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 int32_t _erpms[4] = {}; + +static void(*_erpm_callback)(int32_t[], size_t, void *) = NULL; +static void *_erpm_callback_context = NULL; + +uint8_t nibbles_from_mapped(uint8_t mapped) { - unsigned buffer_offset = 0; + 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; - for (int timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) { - dshot_handler[timer_index].init = false; + 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; } +} - for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { - if (io_timers[timer].base == 0) { // no more timers configured - break; +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; } - // 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); + 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; - if (buffer_offset > sizeof(dshot_burst_buffer_array)) { - return -EINVAL; // something is wrong with the board configuration or some other logic + // 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; +} + +int dshot_output_timer_deinit(unsigned channel) +{ + return io_timer_unallocate_channel(channel); +} + +int dshot_output_timer_init(unsigned channel) +{ + int ret = io_timer_channel_init(channel, + bidirectional_dshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, NULL, NULL); + + if (ret == -EBUSY) { + // either timer or channel already used - this is not fatal + return OK; + + } else { + return ret; } +} - /* Init channels */ - int ret_val = OK; - int channels_init_mask = 0; +int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot) +{ + bidirectional_dshot_enabled = enable_bidirectional_dshot; + _dshot_frequency = dshot_pwm_freq; - 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; @@ -119,49 +290,113 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) continue; } - ret_val = io_timer_channel_init(channel, 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 < DSHOT_TIMERS) && (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_pwm_freq, - io_timers_channel_mapping.element[timer_index].channel_count_including_gaps); + 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 } } } - return ret_val == OK ? channels_init_mask : ret_val; + return _channels_init_mask; } void up_dshot_trigger(void) { - for (uint8_t timer = 0; (timer < DSHOT_TIMERS); timer++) { - if (true == dshot_handler[timer].init) { + for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (_channels_init_mask & (1 << channel)) { + + // For bidirectional dshot we need to re-initialize the timers every time here. + // In normal mode, we just do it once. + int ret = OK; + + if (bidirectional_dshot_enabled) { + dshot_output_timer_deinit(channel); + ret = dshot_output_timer_init(channel); + } + + if (ret != OK) { + // TODO: what to do here? + return; + } + } + } + + io_timer_set_enable(true, bidirectional_dshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, + IO_TIMER_ALL_MODES_CHANNELS); + + 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, @@ -172,11 +407,130 @@ void up_dshot_trigger(void) // Clean UDE flag before DMA is started io_timer_update_dma_req(timer, false); // Trigger DMA (DShot Outputs) - stm32_dmastart(dshot_handler[timer].dma_handle, NULL, NULL, false); + stm32_dmastart(dshot_handler[timer].dma_handle, bidirectional_dshot_enabled ? do_capture : NULL, NULL, false); io_timer_update_dma_req(timer, true); + } + } +} + +void do_capture(DMA_HANDLE handle, uint8_t status, void *arg) +{ + (void)handle; + (void)status; + (void)arg; + + 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; + } + + // 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); + + // Instead of using the UP DMA channel, we need to use the CH1-4 channels. + // It turns out that we can infer the DMA channel index by starting from the UP channel -5. + unsigned timer_channel = timer_io_channels[capture_channel].timer_channel; + + switch (timer_channel) { + case 1: + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap - 5 + 1); + break; + + case 2: + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap - 5 + 2); + break; + + case 3: + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap - 5 + 3); + break; + + case 4: + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap - 5 + 4); + 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_BIDIRECTIONAL_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); + } + } + + // 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); +} + +void process_capture_results(void *arg) +{ + (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_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)); + + const unsigned period = calculate_period(); + if (period == 0) { + // If the parsing failed, we get 0. + _erpms[_motor_to_capture] = 0; + + } else if (period == 65408) { + // For still, we get this magic 65408 value. + _erpms[_motor_to_capture] = 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); + } + } + + 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; } /** @@ -202,9 +556,20 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet csum_data >>= NIBBLES_SIZE; } - packet |= (checksum & 0x0F); + if (bidirectional_dshot_enabled) { + packet |= ((~checksum) & 0x0F); + + } else { + packet |= ((checksum) & 0x0F); + } 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; @@ -219,7 +584,20 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet int up_dshot_arm(bool armed) { - return io_timer_set_enable(armed, IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS); + return io_timer_set_enable(armed, bidirectional_dshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, + IO_TIMER_ALL_MODES_CHANNELS); +} + +void up_dshot_set_erpm_callback(void(*callback)(int32_t[], size_t, void *), void *context) +{ + _erpm_callback = callback; + _erpm_callback_context = context; +} + +void print_driver_stats() +{ + PX4_INFO("dshot driver stats: %lu read, %lu failed nibble, %lu failed CRC, %lu invalid/zero", + read_ok, read_fail_nibble, read_fail_crc, read_fail_zero); } #endif diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h index 31d6e9b1c7e4..5ac86c8586ee 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2023 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 @@ -80,6 +80,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_LED = 7, IOTimerChanMode_PPS = 8, IOTimerChanMode_Other = 9, + IOTimerChanMode_DshotInverted = 10, + IOTimerChanMode_CaptureDMA = 11, IOTimerChanModeSize } io_timer_channel_mode_t; @@ -159,6 +161,9 @@ __EXPORT int io_timer_get_channel_mode(unsigned channel); __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); __EXPORT extern void io_timer_trigger(unsigned channels_mask); __EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable); +__EXPORT void io_timer_capture_update_dma_req(uint8_t timer, bool enable); +__EXPORT int io_timer_set_enable_capture_dma(bool state, io_timer_channel_allocation_t masks); +__EXPORT int io_timer_set_capture_mode(uint8_t timer, unsigned dshot_pwm_rate, unsigned channel); /** * Reserve a timer diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c index 61fd70ee886c..342020bd62de 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2023 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 @@ -365,8 +365,10 @@ int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge) rv = -ENXIO; /* Any pins in capture mode */ + int mode = io_timer_get_channel_mode(channel); - if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + if (mode == IOTimerChanMode_Capture || + mode == IOTimerChanMode_CaptureDMA) { uint16_t edge_bits = 0xffff; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index 5a72cc9f5e30..700ee5f2c4e9 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2023 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 @@ -131,6 +131,7 @@ static int io_timer_handler7(int irq, void *context, void *arg); (GTIM_CCMR_ICF_NOFILT << GTIM_CCMR1_IC1F_SHIFT) #define CCMR_C1_PWMOUT_INIT (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE +#define CCMR_C1_PWMOUT_INVERTED_INIT (GTIM_CCMR_MODE_PWM2 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE #define CCMR_C1_PWMIN_INIT 0 // TBD @@ -519,6 +520,18 @@ void io_timer_update_dma_req(uint8_t timer, bool enable) } } +void io_timer_capture_update_dma_req(uint8_t timer, bool enable) +{ + if (enable) { + rDIER(timer) |= ATIM_DIER_CC1DE | ATIM_DIER_CC2DE | ATIM_DIER_CC3DE | ATIM_DIER_CC4DE; + rEGR(timer) |= (ATIM_EGR_UG | ATIM_EGR_CC1G | ATIM_EGR_CC2G | ATIM_EGR_CC3G | ATIM_EGR_CC4G); + + } else { + rEGR(timer) &= ~(ATIM_EGR_UG | ATIM_EGR_CC1G | ATIM_EGR_CC2G | ATIM_EGR_CC3G | ATIM_EGR_CC4G); + rDIER(timer) &= ~(ATIM_DIER_CC1DE | ATIM_DIER_CC2DE | ATIM_DIER_CC3DE | ATIM_DIER_CC4DE); + } +} + int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_burst_length) { int ret_val = OK; @@ -545,7 +558,7 @@ int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_ rPSC(timer) = ((int)(io_timers[timer].clock_freq / dshot_pwm_freq) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1; rEGR(timer) = ATIM_EGR_UG; - // find the lowest channel index for the timer (they are not necesarily in ascending order) + // find the lowest channel index for the timer (they are not necessarily in ascending order) unsigned lowest_timer_channel = 4; uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index; uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count; @@ -574,6 +587,54 @@ int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_ return ret_val; } +int io_timer_set_capture_mode(uint8_t timer, unsigned dshot_pwm_freq, unsigned channel) +{ + rARR(timer) = -1; + rEGR(timer) = ATIM_EGR_UG | GTIM_EGR_CC1G | GTIM_EGR_CC2G | GTIM_EGR_CC3G | GTIM_EGR_CC4G; + + rPSC(timer) = ((int)(io_timers[timer].clock_freq / (dshot_pwm_freq * 5 / 4)) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1; + + + + switch (timer_io_channels[channel].timer_channel) { + case 1: + // We need to disable CC1E before we can switch to CC1S to input + rCCER(timer) &= ~(GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP); + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT); + rCR1(timer) |= GTIM_CR1_CEN; + rCCER(timer) |= (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP); +// We need to pass the offset of the register to read by DMA divided by 4. + rDCR(timer) = 0xD; // 0x34 / 4, offset for CC1 + break; + + case 2: + rCCER(timer) &= ~(GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP); + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC2S_SHIFT); + rCR1(timer) |= GTIM_CR1_CEN; + rCCER(timer) |= (GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP); + rDCR(timer) = 0xE; // 0x38 / 4, offset for CC2 + break; + + case 3: + rCCER(timer) &= ~(GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP); + rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC3S_SHIFT); + rCR1(timer) |= GTIM_CR1_CEN; + rCCER(timer) |= (GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP); + rDCR(timer) = 0xF; // 0x3C / 4, offset for CC3 + break; + + case 4: + rCCER(timer) &= ~(GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP); + rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC4S_SHIFT); + rCR1(timer) |= GTIM_CR1_CEN; + rCCER(timer) |= (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP); + rDCR(timer) = 0x10; // 0x40 / 4, offset for CC4 + break; + } + + return 0; +} + static inline void io_timer_set_PWM_mode(unsigned timer) { rPSC(timer) = (io_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1; @@ -773,6 +834,12 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, setbits = CCMR_C1_PWMOUT_INIT; break; + case IOTimerChanMode_DshotInverted: + ccer_setbits = 0; + dier_setbits = 0; + setbits = CCMR_C1_PWMOUT_INVERTED_INIT; + break; + case IOTimerChanMode_PWMIn: setbits = CCMR_C1_PWMIN_INIT; gpio = timer_io_channels[channel].gpio_in; @@ -781,6 +848,7 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, #if !defined(BOARD_HAS_NO_CAPTURE) case IOTimerChanMode_Capture: + case IOTimerChanMode_CaptureDMA: setbits = CCMR_C1_CAPTURE_INIT; gpio = timer_io_channels[channel].gpio_in; break; @@ -805,6 +873,13 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, rv = io_timer_init_timer(timer, mode); + if (rv == -16) { + // FIXME: I don't understand why exactly this is the way it is. + // With this hack I'm able to to toggle the dshot pins from output + // to input without problem. But there should be a nicer way. + rv = 0; + } + if (rv != 0 && previous_mode == IOTimerChanMode_NotUsed) { /* free the channel if it was not used before */ io_timer_unallocate_channel(channel); @@ -897,12 +972,14 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann break; case IOTimerChanMode_Dshot: + case IOTimerChanMode_DshotInverted: dier_bit = 0; cr1_bit = state ? GTIM_CR1_CEN : 0; break; case IOTimerChanMode_PWMIn: case IOTimerChanMode_Capture: + case IOTimerChanMode_CaptureDMA: break; default: @@ -944,6 +1021,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann (mode == IOTimerChanMode_PWMOut || mode == IOTimerChanMode_OneShot || mode == IOTimerChanMode_Dshot || + mode == IOTimerChanMode_DshotInverted || mode == IOTimerChanMode_Trigger))) { action_cache[timer].gpio[shifts] = timer_io_channels[chan_index].gpio_out; } @@ -1004,6 +1082,7 @@ int io_timer_set_ccr(unsigned channel, uint16_t value) if ((mode != IOTimerChanMode_PWMOut) && (mode != IOTimerChanMode_OneShot) && (mode != IOTimerChanMode_Dshot) && + (mode != IOTimerChanMode_DshotInverted) && (mode != IOTimerChanMode_Trigger)) { rv = -EIO; diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index 2e8dab2d6db9..0c03a1b043d4 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2023 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 @@ -88,10 +88,11 @@ typedef enum { * @param channel_mask Bitmask of channels (LSB = channel 0) to enable. * This allows some of the channels to remain configured * as GPIOs or as another function. Already used channels/timers will not be configured as DShot - * @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200 + * @param dshot_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200 + * @param enable_bidirectional_dshot Whether to use bidirectional dshot * @return <0 on error, the initialized channels mask. */ -__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq); +__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_freq, bool enable_bidirectional_dshot); /** * Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method) @@ -137,4 +138,10 @@ __EXPORT extern void up_dshot_trigger(void); */ __EXPORT extern int up_dshot_arm(bool armed); +__EXPORT extern bool up_dshot_get_periods(uint32_t periods[], size_t num_periods); + +__EXPORT extern void up_dshot_set_erpm_callback(void(*callback)(int32_t[], size_t, void *), void *context); + +__EXPORT extern void print_driver_stats(void); + __END_DECLS diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 5ae0150efba9..f799aed8a854 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 @@ -67,6 +67,13 @@ int DShot::init() // Getting initial parameter values update_params(); + // We can't advertise in interrupt context later. + _esc_status_pub.advertise(); + esc_status_s &esc_status = _esc_status_pub.get(); + esc_status = {}; + _esc_status_pub.update(); + up_dshot_set_erpm_callback(&DShot::erpm_trampoline, this); + ScheduleNow(); return OK; @@ -144,7 +151,7 @@ void DShot::enable_dshot_outputs(const bool enabled) } } - int ret = up_dshot_init(_output_mask, dshot_frequency); + int ret = up_dshot_init(_output_mask, dshot_frequency, _param_bidirectional_enable.get()); if (ret < 0) { PX4_ERR("up_dshot_init failed (%i)", ret); @@ -607,6 +614,30 @@ void DShot::update_params() } } +void DShot::erpm_trampoline(int32_t erpms[], size_t num_erpms, void *context) +{ + DShot *self = static_cast(context); + self->erpm(erpms, num_erpms); +} + +void DShot::erpm(int32_t erpms[], size_t num_erpms) +{ + esc_status_s &esc_status = _esc_status_pub.get(); + esc_status = {}; + esc_status.timestamp = hrt_absolute_time(); + esc_status.counter = _esc_status_counter++; + esc_status.esc_count = num_erpms; + esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; + esc_status.esc_armed_flags = _outputs_on; + + for (unsigned i = 0; i < num_erpms && i < esc_status_s::CONNECTED_ESC_MAX; ++i) { + esc_status.esc[i].timestamp = hrt_absolute_time(); + esc_status.esc[i].esc_rpm = erpms[i] / (_param_mot_pole_count.get() / 2); + } + + _esc_status_pub.update(); +} + int DShot::custom_command(int argc, char *argv[]) { const char *verb = argv[0]; @@ -707,6 +738,7 @@ int DShot::print_status() PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no"); perf_print_counter(_cycle_perf); _mixing_output.printStatus(); + print_driver_stats(); if (_telemetry) { PX4_INFO("telemetry on: %s", _telemetry_device); diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 4f314cebb75f..07f12bdbd5e1 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 @@ -143,6 +143,9 @@ class DShot final : public ModuleBase, public OutputModuleInterface void handle_vehicle_commands(); + static void erpm_trampoline(int32_t erpms[], size_t num_erpms, void *context); + void erpm(int32_t erpms[], size_t num_erpms); + MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; uint32_t _reversible_outputs{}; @@ -170,11 +173,15 @@ class DShot final : public ModuleBase, public OutputModuleInterface uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::PublicationData _esc_status_pub{ORB_ID(esc_status)}; + uint16_t _esc_status_counter{0}; + DEFINE_PARAMETERS( (ParamFloat) _param_dshot_min, (ParamBool) _param_dshot_3d_enable, (ParamInt) _param_dshot_3d_dead_h, (ParamInt) _param_dshot_3d_dead_l, - (ParamInt) _param_mot_pole_count + (ParamInt) _param_mot_pole_count, + (ParamBool) _param_bidirectional_enable ) }; diff --git a/src/drivers/dshot/module.yaml b/src/drivers/dshot/module.yaml index edd38edd23f1..e2dcf8bc97e8 100644 --- a/src/drivers/dshot/module.yaml +++ b/src/drivers/dshot/module.yaml @@ -33,6 +33,16 @@ parameters: When mixer outputs 1000 or value inside DSHOT 3D deadband, DShot 0 is sent. type: boolean default: 0 + DSHOT_BIDIR_EN: + description: + short: Enable bidirectional DShot + long: | + This parameter enables bidirectional DShot which provides RPM feedback. + Note that this requires ESCs that support bidirectional DSHot, e.g. BlHeli32. + This is not the same as DShot telemetry which requires an additional serial connection. + type: boolean + default: 0 + reboot_required: true DSHOT_3D_DEAD_H: description: short: DSHOT 3D deadband high