forked from Archive/PX4-Autopilot
dshot: cleanup, reorg, however still limited to 4
This commit is contained in:
Julian Oes
parent
58a2334424
commit
8526f27e88
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue