From 90814657b1637d9f7ea721ff793a157fa2abbdfe Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 24 Sep 2023 14:09:23 +0100 Subject: [PATCH] AP_HAL_ChibiOS: bdshot for f103 iofirmware add support to tell if shared DMA channel is actually shared avoid starting and stopping the timer peripheral with bdshot ensure that rcout DMA allocation and deallocation happens entirely within the lock increase rcout thread working area for bdshot fix mode mask that is sent to the iomcu ensure iomcu rcout thread gets timeouts for callbacks control bdshot input and output line levels on f103 use input capture channel pairs to read rising and falling edges of telemetry on f103 reset channel pairs together on iomcu generalize the bdshot input path to support suitable buffer sizes for iomcu generalize DMAR reading of CCR registers to read two at a time on iomcu enable bi-directional dshot channels on PWM1-4 on iomcu add methods to directly access erpm values from rcout update erpm mask and esc telemetry correctly for firmware supporting dshot add support for propagating bdmask to iomcu dshot commands to all channels need to be aware of iomcu ensure esc type is propagated to iomcu cope with iomcu channel numbering when using EDT ensure pwm driver is reset properly for dshot commands on iomcu correctly reset pwm for dshot commands correctly mask off bdshot bits going to iomcu don't reset GPIO modes on disabled lines don't reset pwm_started when sharing DMA channels set thread name on iomcu rcout and reduce stack size on iomcu ensure that bdshot pulses with no response are handled correctly correctly setup DMA for input capture on f103 deal with out of order captured bytes when decoding bdshot telemetry ensure DMA sharing on f103 does not pull lines low only disable the timer peripheral when switching DMA channels on iomcu add support for waiting for _UP to finish before proceeding with dshot re-order iomcu dshot channels to let TIM4_UP go first ensure that a cascading event will always come when expected on rcout allow timeouts when using cascading dshot always rotate telemetry channel after trying to capture input cater for both in order and out-of-order bdshot telemetry packets cope with reversed packets when decoding bdshot telemetry ensure UP DMA channel is fully free on iomcu before starting next dshot cycle --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 231 +++++----- libraries/AP_HAL_ChibiOS/RCOutput.h | 35 +- libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp | 403 ++++++++++++++++-- libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp | 22 +- .../hwdef/iomcu-dshot/hwdef.inc | 2 +- .../hwdef/iomcu-f103-dshot/hwdef.dat | 37 ++ .../hwdef/scripts/chibios_hwdef.py | 5 +- libraries/AP_HAL_ChibiOS/shared_dma.cpp | 5 + libraries/AP_HAL_ChibiOS/shared_dma.h | 1 + 9 files changed, 583 insertions(+), 158 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 533733c10831de..0c7c10d7233b3c 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -295,6 +295,27 @@ __RAMFUNC__ void RCOutput::dshot_update_tick(virtual_timer_t* vt, void* p) chSysUnlockFromISR(); } +// calculate how much time remains in the current cycle +sysinterval_t RCOutput::calc_ticks_remaining(pwm_group &group, uint64_t time_out_us, uint64_t cycle_length_us) +{ + uint64_t now = AP_HAL::micros64(); + const uint64_t pulse_elapsed_us = now - group.last_dmar_send_us; + uint32_t wait_us = 0; + if (now < time_out_us) { + wait_us = time_out_us - now; + } + if (pulse_elapsed_us < group.dshot_pulse_send_time_us) { + // better to let the burst write in progress complete rather than cancelling mid way through + wait_us = MAX(wait_us, group.dshot_pulse_send_time_us - pulse_elapsed_us); + } + + // waiting for a very short period of time can cause a + // timer wrap with ChibiOS timers. Use CH_CFG_ST_TIMEDELTA + // as minimum. Don't allow for a very long delay (over _dshot_period_us) + // to prevent bugs in handling timer wrap + return constrain_uint32(chTimeUS2I(wait_us), CH_CFG_ST_TIMEDELTA, chTimeUS2I(cycle_length_us)); +} + #if AP_HAL_SHARED_DMA_ENABLED // release locks on the groups that are pending in reverse order void RCOutput::dshot_collect_dma_locks(uint64_t time_out_us, bool led_thread) @@ -309,29 +330,11 @@ void RCOutput::dshot_collect_dma_locks(uint64_t time_out_us, bool led_thread) continue; } + // dma handle will only be unlocked if the send was aborted if (group.dma_handle != nullptr && group.dma_handle->is_locked()) { - // calculate how long we have left - uint64_t now = AP_HAL::micros64(); // if we have time left wait for the event - eventmask_t mask = 0; - const uint64_t pulse_elapsed_us = now - group.last_dmar_send_us; - uint32_t wait_us = 0; - if (now < time_out_us) { - wait_us = time_out_us - now; - } - if (pulse_elapsed_us < group.dshot_pulse_send_time_us) { - // better to let the burst write in progress complete rather than cancelling mid way through - wait_us = MAX(wait_us, group.dshot_pulse_send_time_us - pulse_elapsed_us); - } - - // waiting for a very short period of time can cause a - // timer wrap with ChibiOS timers. Use CH_CFG_ST_TIMEDELTA - // as minimum. Don't allow for a very long delay (over _dshot_period_us) - // to prevent bugs in handling timer wrap - const uint32_t max_delay_us = led_thread ? LED_OUTPUT_PERIOD_US : _dshot_period_us; - const uint32_t min_delay_us = 10; // matches our CH_CFG_ST_TIMEDELTA - wait_us = constrain_uint32(wait_us, min_delay_us, max_delay_us); - mask = chEvtWaitOneTimeout(group.dshot_event_mask, MIN(TIME_MAX_INTERVAL, chTimeUS2I(wait_us))); + const sysinterval_t wait_ticks = calc_ticks_remaining(group, time_out_us, led_thread ? LED_OUTPUT_PERIOD_US : _dshot_period_us); + eventmask_t mask = chEvtWaitOneTimeout(group.dshot_event_mask, MIN(TIME_MAX_INTERVAL, wait_ticks)); // no time left cancel and restart if (!mask) { @@ -341,13 +344,11 @@ void RCOutput::dshot_collect_dma_locks(uint64_t time_out_us, bool led_thread) #ifdef HAL_WITH_BIDIR_DSHOT // if using input capture DMA then clean up if (group.bdshot.enabled) { - // the channel index only moves on with success - const uint8_t chan = mask ? group.bdshot.prev_telem_chan - : group.bdshot.curr_telem_chan; // only unlock if not shared - if (group.bdshot.ic_dma_handle[chan] != nullptr - && group.bdshot.ic_dma_handle[chan] != group.dma_handle) { - group.bdshot.ic_dma_handle[chan]->unlock(); + if (group.bdshot.curr_ic_dma_handle != nullptr + && group.bdshot.curr_ic_dma_handle != group.dma_handle) { + group.bdshot.curr_ic_dma_handle->unlock(); + group.bdshot.curr_ic_dma_handle = nullptr; } } #endif @@ -584,6 +585,11 @@ void RCOutput::set_dshot_esc_type(DshotEscType dshot_esc_type) DSHOT_BIT_1_TICKS = DSHOT_BIT_1_TICKS_DEFAULT; break; } +#if HAL_WITH_IO_MCU + if (AP_BoardConfig::io_dshot()) { + iomcu.set_dshot_esc_type(dshot_esc_type); + } +#endif } #endif // #if HAL_DSHOT_ENABLED @@ -1177,13 +1183,14 @@ void RCOutput::set_output_mode(uint32_t mask, const enum output_mode mode) } } #if HAL_WITH_IO_MCU + const uint16_t iomcu_mask = (mask & ((1U<= MODE_PWM_DSHOT150 && mode <= MODE_PWM_DSHOT600)) && - (mask & ((1U<is_system_initialized()) { + while (!hal.scheduler->is_system_initialized() || _dshot_period_us == 0) { hal.scheduler->delay_microseconds(1000); } rcout_thread_ctx = chThdGetSelfX(); + chRegSetThreadNameX(rcout_thread_ctx, rcout_thread_name); + + uint64_t last_cycle_run_us = 0; while (true) { chEvtWaitOne(EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND); - + // start the clock + const uint64_t last_thread_run_us = AP_HAL::micros64(); // this is when the cycle is supposed to start if (_dshot_cycle == 0) { + last_cycle_run_us = AP_HAL::micros64(); // register a timer for the next tick if push() will not be providing it if (_dshot_rate != 1) { chVTSet(&_dshot_rate_timer, chTimeUS2I(_dshot_period_us), dshot_update_tick, this); } } - // main thread requested a new dshot send or we timed out - if we are not running - // as a multiple of loop rate then ignore EVT_PWM_SEND events to preserve periodicity - dshot_send_groups(0); + // if DMA sharing is in effect there can be quite a delay between the request to begin the cycle and + // actually sending out data - thus we need to work out how much time we have left to collect the locks + uint64_t time_out_us = (_dshot_cycle + 1) * _dshot_period_us + last_cycle_run_us; + if (!_dshot_rate) { + time_out_us = last_thread_run_us + _dshot_period_us; + } + + // DMA channel sharing on F10x is complicated. The allocations are + // TIM2_UP - (1,2) + // TIM4_UP - (1,7) + // TIM3_UP - (1,3) + // TIM2_CH2 - (1,7) - F103 only + // TIM4_CH3 - (1,5) - F103 only + // TIM3_CH4 - (1,3) - F103 only + // and (1,7) is also shared with USART2_TX + // locks have to be unlocked in reverse order, and shared CH locks do not need to be taken so the + // ordering that will work follows. This relies on recursive lock behaviour that allows us to relock + // a mutex without releasing it first: + // TIM4_UP - lock (shared) + // TIM4 - dshot send + // TIM4_CH3 - lock + // TIM2_UP - lock + // TIM2_CH2 - lock recursive (shared) + // TIM2 - dshot send + // TIM3_UP - lock + // [TIM3_CH4 - shared lock] + // TIM3 - dshot send + // [TIM3_CH4 - shared unlock] + // TIM3_UP - unlock + // TIM2_CH2 - unlock recursive (shared) + // TIM2_UP - unlock + // TIM4_CH3 - unlock + // TIM4_UP - unlock + + dshot_send_groups(time_out_us); #if AP_HAL_SHARED_DMA_ENABLED - dshot_collect_dma_locks(0); + dshot_collect_dma_locks(time_out_us); #endif if (_dshot_rate > 0) { _dshot_cycle = (_dshot_cycle + 1) % _dshot_rate; @@ -1506,14 +1549,25 @@ void RCOutput::dshot_send_groups(uint64_t time_out_us) } for (auto &group : pwm_group_list) { + bool pulse_sent; // send a dshot command if (is_dshot_protocol(group.current_mode) && dshot_command_is_active(group)) { command_sent = dshot_send_command(group, _dshot_current_command.command, _dshot_current_command.chan); + pulse_sent = true; // actually do a dshot send } else if (group.can_send_dshot_pulse()) { dshot_send(group, time_out_us); + pulse_sent = true; } +#ifdef HAL_WITH_BIDIR_DSHOT + // prevent the next send going out until the previous send has released its DMA channel + if (pulse_sent && group.shared_up_dma && group.bdshot.enabled) { + chEvtWaitOneTimeout(DSHOT_CASCADE, calc_ticks_remaining(group, time_out_us, _dshot_period_us)); + } +#else + (void)pulse_sent; +#endif } if (command_sent) { @@ -1537,16 +1591,17 @@ __RAMFUNC__ void RCOutput::dshot_send_next_group(void* p) #if AP_HAL_SHARED_DMA_ENABLED void RCOutput::dma_allocate(Shared_DMA *ctx) { + chSysLock(); for (auto &group : pwm_group_list) { if (group.dma_handle == ctx && group.dma == nullptr) { - chSysLock(); group.dma = dmaStreamAllocI(group.dma_up_stream_id, 10, dma_up_irq_callback, &group); #if defined(IOMCU_FW) - if (group.pwm_started) { - pwmStart(group.pwm_drv, &group.pwm_cfg); + if (group.pwm_started && group.dma_handle->is_shared()) { + /* Timer configured and started.*/ + group.pwm_drv->tim->CR1 = STM32_TIM_CR1_ARPE | STM32_TIM_CR1_URS | STM32_TIM_CR1_CEN; + group.pwm_drv->tim->DIER = group.pwm_drv->config->dier & ~STM32_TIM_DIER_IRQ_MASK; } #endif - chSysUnlock(); #if STM32_DMA_SUPPORTS_DMAMUX if (group.dma) { dmaSetRequestSource(group.dma, group.dma_up_channel); @@ -1554,6 +1609,7 @@ void RCOutput::dma_allocate(Shared_DMA *ctx) #endif } } + chSysUnlock(); } /* @@ -1561,21 +1617,23 @@ void RCOutput::dma_allocate(Shared_DMA *ctx) */ void RCOutput::dma_deallocate(Shared_DMA *ctx) { + chSysLock(); for (auto &group : pwm_group_list) { if (group.dma_handle == ctx && group.dma != nullptr) { - chSysLock(); #if defined(IOMCU_FW) // leaving the peripheral running on IOMCU plays havoc with the UART that is - // also sharing this channel - if (group.pwm_started) { - pwmStop(group.pwm_drv); + // also sharing this channel, we only turn it off rather than resetting so + // that we don't have to worry about line modes etc + if (group.pwm_started && group.dma_handle->is_shared()) { + group.pwm_drv->tim->CR1 = 0; + group.pwm_drv->tim->DIER = 0; } #endif dmaStreamFreeI(group.dma); group.dma = nullptr; - chSysUnlock(); } } + chSysUnlock(); } #endif // AP_HAL_SHARED_DMA_ENABLED @@ -1641,7 +1699,7 @@ void RCOutput::fill_DMA_buffer_dshot(dmar_uint_t *buffer, uint8_t stride, uint16 void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us) { #if HAL_DSHOT_ENABLED - if (soft_serial_waiting() || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) { + if (soft_serial_waiting() || !is_dshot_send_allowed(group.dshot_state)) { // doing serial output or DMAR input, don't send DShot pulses return; } @@ -1654,7 +1712,7 @@ void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us) // if we are sharing UP channels then it might have taken a long time to get here, // if there's not enough time to actually send a pulse then cancel #if AP_HAL_SHARED_DMA_ENABLED - if (time_out_us > 0 && AP_HAL::micros64() + group.dshot_pulse_time_us > time_out_us) { + if (AP_HAL::micros64() + group.dshot_pulse_time_us > time_out_us) { group.dma_handle->unlock(); return; } @@ -1663,50 +1721,7 @@ void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us) // only the timer thread releases the locks group.dshot_waiter = rcout_thread_ctx; #ifdef HAL_WITH_BIDIR_DSHOT - // assume that we won't be able to get the input capture lock - group.bdshot.enabled = false; - - uint32_t active_channels = group.ch_mask & group.en_mask; - // now grab the input capture lock if we are able, we can only enable bi-dir on a group basis - if (((_bdshot.mask & active_channels) == active_channels) && group.has_ic()) { - if (group.has_shared_ic_up_dma()) { - // no locking required - group.bdshot.enabled = true; - } else { - osalDbgAssert(!group.bdshot.ic_dma_handle[group.bdshot.curr_telem_chan]->is_locked(), "DMA handle is already locked"); - group.bdshot.ic_dma_handle[group.bdshot.curr_telem_chan]->lock(); - group.bdshot.enabled = true; - } - } - - // if the last transaction returned telemetry, decode it - if (group.dshot_state == DshotState::RECV_COMPLETE) { - uint8_t chan = group.chan[group.bdshot.prev_telem_chan]; - uint32_t now = AP_HAL::millis(); - if (bdshot_decode_dshot_telemetry(group, group.bdshot.prev_telem_chan)) { - _bdshot.erpm_clean_frames[chan]++; - _active_escs_mask |= (1< 5000) { - _bdshot.erpm_clean_frames[chan] = 0; - _bdshot.erpm_errors[chan] = 0; - _bdshot.erpm_last_stats_ms[chan] = now; - } - } - - if (group.bdshot.enabled) { - if (group.pwm_started) { - pwmStop(group.pwm_drv); - } - pwmStart(group.pwm_drv, &group.pwm_cfg); - group.pwm_started = true; - - // we can be more precise for capture timer - group.bdshot.telempsc = (uint16_t)(lrintf(((float)group.pwm_drv->clock / bdshot_get_output_rate_hz(group.current_mode) + 0.01f)/TELEM_IC_SAMPLE) - 1); - } + bdshot_prepare_for_next_pulse(group); #endif bool safety_on = hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED; #if !defined(IOMCU_FW) @@ -1777,7 +1792,7 @@ void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us) } } - chEvtGetAndClearEvents(group.dshot_event_mask); + chEvtGetAndClearEvents(group.dshot_event_mask | DSHOT_CASCADE); // start sending the pulses out send_pulses_DMAR(group, DSHOT_BUFFER_LENGTH); #endif // HAL_DSHOT_ENABLED @@ -1796,7 +1811,7 @@ bool RCOutput::serial_led_send(pwm_group &group) } #if HAL_DSHOT_ENABLED - if (soft_serial_waiting() || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) { + if (soft_serial_waiting() || !is_dshot_send_allowed(group.dshot_state)) { // doing serial output or DMAR input, don't send DShot pulses return false; } @@ -1816,7 +1831,7 @@ bool RCOutput::serial_led_send(pwm_group &group) group.dshot_waiter = led_thread_ctx; - chEvtGetAndClearEvents(group.dshot_event_mask); + chEvtGetAndClearEvents(group.dshot_event_mask | DSHOT_CASCADE); // start sending the pulses out send_pulses_DMAR(group, group.dma_buffer_len); @@ -1861,10 +1876,18 @@ void RCOutput::send_pulses_DMAR(pwm_group &group, uint32_t buffer_length) #endif dmaStreamSetMode(group.dma, STM32_DMA_CR_CHSEL(group.dma_up_channel) | - STM32_DMA_CR_DIR_M2P | STM32_DMA_CR_PSIZE_WORD | + STM32_DMA_CR_DIR_M2P | #if defined(IOMCU_FW) +#ifdef HAL_WITH_BIDIR_DSHOT + STM32_DMA_CR_PSIZE_HWORD | + STM32_DMA_CR_MSIZE_HWORD | +#else + STM32_DMA_CR_PSIZE_WORD | STM32_DMA_CR_MSIZE_BYTE | +#endif + #else + STM32_DMA_CR_PSIZE_WORD | STM32_DMA_CR_MSIZE_WORD | #endif STM32_DMA_CR_MINC | STM32_DMA_CR_PL(3) | @@ -1953,7 +1976,7 @@ void RCOutput::dma_cancel(pwm_group& group) } } chVTResetI(&group.dma_timeout); - chEvtGetAndClearEventsI(group.dshot_event_mask); + chEvtGetAndClearEventsI(group.dshot_event_mask | DSHOT_CASCADE); group.dshot_state = DshotState::IDLE; chSysUnlock(); diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 53630f626327c8..8ef6bd4095bcfe 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -26,9 +26,16 @@ #if HAL_USE_PWM == TRUE #if defined(IOMCU_FW) +#ifdef HAL_WITH_BIDIR_DSHOT +typedef uint16_t dmar_uint_t; // save memory to allow dshot on IOMCU +typedef int16_t dmar_int_t; +#else typedef uint8_t dmar_uint_t; // save memory to allow dshot on IOMCU +typedef int8_t dmar_int_t; +#endif #else typedef uint32_t dmar_uint_t; +typedef int32_t dmar_int_t; #endif #define RCOU_DSHOT_TIMING_DEBUG 0 @@ -59,6 +66,11 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput float get_erpm_error_rate(uint8_t chan) const override { return 100.0f * float(_bdshot.erpm_errors[chan]) / (1 + _bdshot.erpm_errors[chan] + _bdshot.erpm_clean_frames[chan]); } + /* + allow all erpm values to be read and for new updates to be detected - primarily for IOMCU + */ + bool new_erpm() override { return _bdshot.update_mask != 0; } + uint32_t read_erpm(uint16_t* erpm, uint8_t len) override; #endif void set_output_mode(uint32_t mask, const enum output_mode mode) override; enum output_mode get_output_mode(uint32_t& mask) override; @@ -277,7 +289,8 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput SEND_START = 1, SEND_COMPLETE = 2, RECV_START = 3, - RECV_COMPLETE = 4 + RECV_COMPLETE = 4, + RECV_FAILED = 5 }; struct PACKED SerialLed { @@ -297,7 +310,9 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput static const uint16_t MIN_GCR_BIT_LEN = 7; static const uint16_t MAX_GCR_BIT_LEN = 22; static const uint16_t GCR_TELEMETRY_BIT_LEN = MAX_GCR_BIT_LEN; - static const uint16_t GCR_TELEMETRY_BUFFER_LEN = GCR_TELEMETRY_BIT_LEN*sizeof(uint32_t); + // input capture is expecting TELEM_IC_SAMPLE (16) ticks per transition (22) so the maximum + // value of the counter in CCR registers is 16*22 == 352, so must be 16-bit + static const uint16_t GCR_TELEMETRY_BUFFER_LEN = GCR_TELEMETRY_BIT_LEN*sizeof(dmar_uint_t); struct pwm_group { // only advanced timers can do high clocks needed for more than 400Hz @@ -315,6 +330,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput uint8_t stream_id; uint8_t channel; } dma_ch[4]; + bool shared_up_dma; // do we need to wait for TIMx_UP DMA to be finished after use #endif uint8_t alt_functions[4]; ioline_t pal_lines[4]; @@ -380,8 +396,9 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput uint8_t telem_tim_ch[4]; uint8_t curr_telem_chan; uint8_t prev_telem_chan; + Shared_DMA *curr_ic_dma_handle; // a shortcut to avoid logic errors involving the wrong lock uint16_t telempsc; - uint32_t dma_buffer_copy[GCR_TELEMETRY_BUFFER_LEN]; + dmar_uint_t dma_buffer_copy[GCR_TELEMETRY_BUFFER_LEN]; #if RCOU_DSHOT_TIMING_DEBUG uint16_t telem_rate[4]; uint16_t telem_err_rate[4]; @@ -525,6 +542,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput struct { uint32_t mask; uint16_t erpm[max_channels]; + volatile uint32_t update_mask; #ifdef HAL_WITH_BIDIR_DSHOT uint16_t erpm_errors[max_channels]; uint16_t erpm_clean_frames[max_channels]; @@ -593,6 +611,10 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput bool is_bidir_dshot_enabled() const { return _bdshot.mask != 0; } + static bool is_dshot_send_allowed(DshotState state) { + return state == DshotState::IDLE || state == DshotState::RECV_COMPLETE || state == DshotState::RECV_FAILED; + } + // are all the ESCs returning data bool group_escs_active(const pwm_group& group) const { return group.en_mask > 0 && (group.en_mask & _active_escs_mask) == group.en_mask; @@ -644,12 +666,15 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput uint16_t create_dshot_packet(const uint16_t value, bool telem_request, bool bidir_telem); void fill_DMA_buffer_dshot(dmar_uint_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul); + // event to allow dshot cascading + static const eventmask_t DSHOT_CASCADE = EVENT_MASK(16); void dshot_send_groups(uint64_t time_out_us); void dshot_send(pwm_group &group, uint64_t time_out_us); bool dshot_send_command(pwm_group &group, uint8_t command, uint8_t chan); static void dshot_update_tick(virtual_timer_t*, void* p); static void dshot_send_next_group(void* p); // release locks on the groups that are pending in reverse order + sysinterval_t calc_ticks_remaining(pwm_group &group, uint64_t time_out_us, uint64_t cycle_length_us); void dshot_collect_dma_locks(uint64_t last_run_us, bool led_thread = false); static void dma_up_irq_callback(void *p, uint32_t flags); static void dma_unlock(virtual_timer_t*, void *p); @@ -668,14 +693,16 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput */ void bdshot_ic_dma_allocate(Shared_DMA *ctx); void bdshot_ic_dma_deallocate(Shared_DMA *ctx); - static uint32_t bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t count); + static uint32_t bdshot_decode_telemetry_packet(dmar_uint_t* buffer, uint32_t count, bool reversed); bool bdshot_decode_telemetry_from_erpm(uint16_t erpm, uint8_t chan); bool bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan); static uint8_t bdshot_find_next_ic_channel(const pwm_group& group); static void bdshot_dma_ic_irq_callback(void *p, uint32_t flags); static void bdshot_finish_dshot_gcr_transaction(virtual_timer_t* vt, void *p); bool bdshot_setup_group_ic_DMA(pwm_group &group); + void bdshot_prepare_for_next_pulse(pwm_group& group); static void bdshot_receive_pulses_DMAR(pwm_group* group); + static void bdshot_reset_pwm(pwm_group& group, uint8_t telem_channel); static void bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch); static uint32_t bdshot_get_output_rate_hz(const enum output_mode mode); diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp index 8ad2f28fb5bce8..e7d6a2fd9c7fca 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp @@ -21,9 +21,20 @@ #include "hwdef/common/stm32_util.h" #include #include +#include + +#if HAL_WITH_IO_MCU +#include +extern AP_IOMCU iomcu; +#endif #ifdef HAL_WITH_BIDIR_DSHOT +#if defined(IOMCU_FW) +#undef INTERNAL_ERROR +#define INTERNAL_ERROR(x) do {} while (0) +#endif + using namespace ChibiOS; extern const AP_HAL::HAL& hal; @@ -43,6 +54,12 @@ extern const AP_HAL::HAL& hal; */ void RCOutput::set_bidir_dshot_mask(uint32_t mask) { +#if HAL_WITH_IO_MCU + const uint32_t iomcu_mask = ((1U<> chan_offset); // we now need to reconfigure the DMA channels since they are affected by the value of the mask for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { @@ -75,6 +92,7 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group) // Return error return false; } + if (!group.bdshot.ic_dma_handle[i]) { // share up channel if required if (group.dma_ch[i].stream_id == group.dma_up_stream_id) { @@ -98,6 +116,11 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group) if (group.chan[i] != CHAN_DISABLED && _bdshot.mask & group.ch_mask) { // bi-directional dshot requires less than MID2 speed and PUSHPULL in order to avoid noise on the line // when switching from output to input +#if defined(IOMCU_FW) + // on F103 the line mode has to be managed manually + // PAL_MODE_STM32_ALTERNATE_PUSHPULL is 50Mhz, similar to the medieum speed on other MCUs + palSetLineMode(group.pal_lines[i], PAL_MODE_STM32_ALTERNATE_PUSHPULL); +#else palSetLineMode(group.pal_lines[i], PAL_MODE_ALTERNATE(group.alt_functions[i]) | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_PUPDR_PULLUP | #ifdef PAL_STM32_OSPEED_MID1 @@ -108,6 +131,7 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group) #error "Cannot set bdshot line speed" #endif ); +#endif } if (!group.is_chan_enabled(i) || !(_bdshot.mask & (1 << group.chan[i]))) { @@ -166,13 +190,12 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group) */ void RCOutput::bdshot_ic_dma_allocate(Shared_DMA *ctx) { + chSysLock(); for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { pwm_group &group = pwm_group_list[i]; for (uint8_t icuch = 0; icuch < 4; icuch++) { if (group.bdshot.ic_dma_handle[icuch] == ctx && group.bdshot.ic_dma[icuch] == nullptr) { - chSysLock(); group.bdshot.ic_dma[icuch] = dmaStreamAllocI(group.dma_ch[icuch].stream_id, 10, bdshot_dma_ic_irq_callback, &group); - chSysUnlock(); #if STM32_DMA_SUPPORTS_DMAMUX if (group.bdshot.ic_dma[icuch]) { dmaSetRequestSource(group.bdshot.ic_dma[icuch], group.dma_ch[icuch].channel); @@ -181,6 +204,7 @@ void RCOutput::bdshot_ic_dma_allocate(Shared_DMA *ctx) } } } + chSysUnlock(); } /* @@ -188,17 +212,151 @@ void RCOutput::bdshot_ic_dma_allocate(Shared_DMA *ctx) */ void RCOutput::bdshot_ic_dma_deallocate(Shared_DMA *ctx) { + chSysLock(); for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { pwm_group &group = pwm_group_list[i]; for (uint8_t icuch = 0; icuch < 4; icuch++) { if (group.bdshot.ic_dma_handle[icuch] == ctx && group.bdshot.ic_dma[icuch] != nullptr) { - chSysLock(); dmaStreamFreeI(group.bdshot.ic_dma[icuch]); group.bdshot.ic_dma[icuch] = nullptr; - chSysUnlock(); } } } + chSysUnlock(); +} + +// setup bdshot for sending and receiving the next pulse +void RCOutput::bdshot_prepare_for_next_pulse(pwm_group& group) +{ + // assume that we won't be able to get the input capture lock + group.bdshot.enabled = false; + + uint32_t active_channels = group.ch_mask & group.en_mask; + // now grab the input capture lock if we are able, we can only enable bi-dir on a group basis + if (((_bdshot.mask & active_channels) == active_channels) && group.has_ic()) { + if (group.has_shared_ic_up_dma()) { + // no locking required + group.bdshot.enabled = true; + } else { + osalDbgAssert(!group.bdshot.curr_ic_dma_handle, "IC DMA handle has not been released"); + group.bdshot.curr_ic_dma_handle = group.bdshot.ic_dma_handle[group.bdshot.curr_telem_chan]; + osalDbgAssert(group.shared_up_dma || !group.bdshot.curr_ic_dma_handle->is_locked(), "IC DMA handle is already locked"); + group.bdshot.curr_ic_dma_handle->lock(); + group.bdshot.enabled = true; + } + } + + // if the last transaction returned telemetry, decode it + if (group.dshot_state == DshotState::RECV_COMPLETE) { + uint8_t chan = group.chan[group.bdshot.prev_telem_chan]; + uint32_t now = AP_HAL::millis(); + if (bdshot_decode_dshot_telemetry(group, group.bdshot.prev_telem_chan)) { + _bdshot.erpm_clean_frames[chan]++; + _active_escs_mask |= (1< 5000) { + _bdshot.erpm_clean_frames[chan] = 0; + _bdshot.erpm_errors[chan] = 0; + _bdshot.erpm_last_stats_ms[chan] = now; + } + } else if (group.dshot_state == DshotState::RECV_FAILED) { + _bdshot.erpm_errors[group.bdshot.curr_telem_chan]++; + } + + if (group.bdshot.enabled) { + if (group.pwm_started) { + bdshot_reset_pwm(group, group.bdshot.prev_telem_chan); + } + else { + pwmStart(group.pwm_drv, &group.pwm_cfg); + group.pwm_started = true; + } + + // we can be more precise for capture timer + group.bdshot.telempsc = (uint16_t)(lrintf(((float)group.pwm_drv->clock / bdshot_get_output_rate_hz(group.current_mode) + 0.01f)/TELEM_IC_SAMPLE) - 1); + } +} + +// reset pwm driver to output mode without resetting the clock or the peripheral +// the code here is the equivalent of pwmStart()/pwmStop() +void RCOutput::bdshot_reset_pwm(pwm_group& group, uint8_t telem_channel) +{ +#if defined(IOMCU_FW) + osalSysLock(); + + stm32_tim_t* TIMx = group.pwm_drv->tim; + // pwmStop sets these + TIMx->CR1 = 0; /* Timer disabled. */ + TIMx->DIER = 0; /* All IRQs disabled. */ + TIMx->SR = 0; /* Clear eventual pending IRQs. */ + TIMx->CNT = 0; + TIMx->CCR[0] = 0; /* Comparator 1 disabled. */ + TIMx->CCR[1] = 0; /* Comparator 2 disabled. */ + TIMx->CCR[2] = 0; /* Comparator 3 disabled. */ + TIMx->CCR[3] = 0; /* Comparator 4 disabled. */ + // at the point this is called we will have done input capture on two CC channels + // we need to switch those channels back to output and the default settings + // all other channels will not have been modified + switch (group.bdshot.telem_tim_ch[telem_channel]) { + case 0: // CC1 + case 1: // CC2 + MODIFY_REG(TIMx->CCER, TIM_CCER_CC2E | TIM_CCER_CC1E, 0); // disable CC so that it can be modified + MODIFY_REG(TIMx->CCMR1, (TIM_CCMR1_CC1S | TIM_CCMR1_IC1F | TIM_CCMR1_IC1PSC), + STM32_TIM_CCMR1_OC1M(6) | STM32_TIM_CCMR1_OC1PE); + MODIFY_REG(TIMx->CCMR1, (TIM_CCMR1_CC2S | TIM_CCMR1_IC2F | TIM_CCMR1_IC2PSC), + STM32_TIM_CCMR1_OC2M(6) | STM32_TIM_CCMR1_OC2PE); + MODIFY_REG(TIMx->CCER, (TIM_CCER_CC1P | TIM_CCER_CC2P), + (TIM_CCER_CC1P | TIM_CCER_CC2P | TIM_CCER_CC1E | TIM_CCER_CC2E)); + break; + case 2: // CC3 + case 3: // CC4 + MODIFY_REG(TIMx->CCER, TIM_CCER_CC3E | TIM_CCER_CC4E, 0); // disable CC so that it can be modified + MODIFY_REG(TIMx->CCMR2, (TIM_CCMR2_CC3S | TIM_CCMR2_IC3F | TIM_CCMR2_IC3PSC), + STM32_TIM_CCMR2_OC3M(6) | STM32_TIM_CCMR2_OC3PE); + MODIFY_REG(TIMx->CCMR2, (TIM_CCMR2_CC4S | TIM_CCMR2_IC4F | TIM_CCMR2_IC4PSC), + STM32_TIM_CCMR2_OC4M(6) | STM32_TIM_CCMR2_OC4PE); + MODIFY_REG(TIMx->CCER, (TIM_CCER_CC3P | TIM_CCER_CC4P), + (TIM_CCER_CC3P | TIM_CCER_CC4P | TIM_CCER_CC3E | TIM_CCER_CC4E)); + break; + default: + break; + } + // pwmStart sets these + uint32_t psc = (group.pwm_drv->clock / group.pwm_drv->config->frequency) - 1; + TIMx->PSC = psc; + TIMx->ARR = group.pwm_drv->period - 1; + TIMx->CR2 = group.pwm_drv->config->cr2; + TIMx->EGR = STM32_TIM_EGR_UG; /* Update event. */ + TIMx->SR = 0; /* Clear pending IRQs. */ + TIMx->DIER = group.pwm_drv->config->dier & /* DMA-related DIER settings. */ + ~STM32_TIM_DIER_IRQ_MASK; + if (group.pwm_drv->has_bdtr) { + TIMx->BDTR = group.pwm_drv->config->bdtr | STM32_TIM_BDTR_MOE; + } + + // we need to switch every output on the same input channel to avoid + // spurious line changes + for (uint8_t i = 0; i<4; i++) { + if (group.chan[i] == CHAN_DISABLED) { + continue; + } + if (group.bdshot.telem_tim_ch[telem_channel] == group.bdshot.telem_tim_ch[i]) { + palSetLineMode(group.pal_lines[i], PAL_MODE_STM32_ALTERNATE_PUSHPULL); + } + } + + /* Timer configured and started.*/ + TIMx->CR1 = STM32_TIM_CR1_ARPE | STM32_TIM_CR1_URS | STM32_TIM_CR1_CEN; + + osalSysUnlock(); +#else + // on more capable MCUs we can do something very simple + pwmStop(group.pwm_drv); + pwmStart(group.pwm_drv, &group.pwm_cfg); +#endif } // see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625 @@ -212,15 +370,20 @@ void RCOutput::bdshot_receive_pulses_DMAR(pwm_group* group) // should be plenty chVTSetI(&group->dma_timeout, chTimeUS2I(group->dshot_pulse_send_time_us + 30U + 10U), bdshot_finish_dshot_gcr_transaction, group); - uint8_t curr_ch = group->bdshot.curr_telem_chan; group->pwm_drv->tim->CR1 = 0; - group->pwm_drv->tim->CCER = 0; // Configure Timer group->pwm_drv->tim->SR = 0; + // do NOT set CCER to 0 here - this pulls the line low on F103 (at least) + // and since we are already doing bdshot the relevant options that are set for output + // also apply to input and bdshot_config_icu_dshot() will disable any channels that need + // disabling +#if !defined(IOMCU_FW) + group->pwm_drv->tim->CCER = 0; group->pwm_drv->tim->CCMR1 = 0; group->pwm_drv->tim->CCMR2 = 0; +#endif group->pwm_drv->tim->DIER = 0; group->pwm_drv->tim->CR2 = 0; group->pwm_drv->tim->PSC = group->bdshot.telempsc; @@ -230,7 +393,20 @@ void RCOutput::bdshot_receive_pulses_DMAR(pwm_group* group) //TOGGLE_PIN_CH_DEBUG(54, curr_ch); group->pwm_drv->tim->ARR = 0xFFFF; // count forever group->pwm_drv->tim->CNT = 0; + uint8_t curr_ch = group->bdshot.curr_telem_chan; +#if defined(IOMCU_FW) + // we need to switch every input on the same input channel to allow + // the ESCs to drive the lines + for (uint8_t i = 0; i<4; i++) { + if (group->chan[i] == CHAN_DISABLED) { + continue; + } + if (group->bdshot.telem_tim_ch[curr_ch] == group->bdshot.telem_tim_ch[i]) { + palSetLineMode(group->pal_lines[i], PAL_MODE_INPUT_PULLUP); + } + } +#endif // Initialise ICU channels bdshot_config_icu_dshot(group->pwm_drv->tim, curr_ch, group->bdshot.telem_tim_ch[curr_ch]); @@ -252,14 +428,27 @@ void RCOutput::bdshot_receive_pulses_DMAR(pwm_group* group) #endif dmaStreamSetMode(ic_dma, STM32_DMA_CR_CHSEL(group->dma_ch[curr_ch].channel) | - STM32_DMA_CR_DIR_P2M | STM32_DMA_CR_PSIZE_WORD | STM32_DMA_CR_MSIZE_WORD | + STM32_DMA_CR_DIR_P2M | +#if defined(IOMCU_FW) + STM32_DMA_CR_PSIZE_HWORD | + STM32_DMA_CR_MSIZE_HWORD | +#else + STM32_DMA_CR_PSIZE_WORD | + STM32_DMA_CR_MSIZE_WORD | +#endif STM32_DMA_CR_MINC | STM32_DMA_CR_PL(3) | STM32_DMA_CR_TEIE | STM32_DMA_CR_TCIE); // setup for transfers. 0x0D is the register // address offset of the CCR registers in the timer peripheral +#if defined(IOMCU_FW) + uint8_t telem_ch_pair = group->bdshot.telem_tim_ch[curr_ch] & ~1U; // round to the lowest of the channel pair + const uint8_t ccr_ofs = offsetof(stm32_tim_t, CCR)/4 + telem_ch_pair; + group->pwm_drv->tim->DCR = STM32_TIM_DCR_DBA(ccr_ofs) | STM32_TIM_DCR_DBL(1); // read two registers at a time +#else const uint8_t ccr_ofs = offsetof(stm32_tim_t, CCR)/4 + group->bdshot.telem_tim_ch[curr_ch]; group->pwm_drv->tim->DCR = STM32_TIM_DCR_DBA(ccr_ofs) | STM32_TIM_DCR_DBL(0); +#endif // Start Timer group->pwm_drv->tim->EGR |= STM32_TIM_EGR_UG; @@ -271,6 +460,78 @@ void RCOutput::bdshot_receive_pulses_DMAR(pwm_group* group) void RCOutput::bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch) { +#if defined(IOMCU_FW) + // F103 does not support both edges input capture so we need to set up two channels + // both pointing at the same input to capture the data. The triggered channel + // needs to handle the second edge - so rising or falling - so that we get an + // even number of half-words in the DMA buffer + switch(ccr_ch) { + case 0: + case 1: { + // Disable the IC1 and IC2: Reset the CCxE Bit + MODIFY_REG(TIMx->CCER, TIM_CCER_CC1E | TIM_CCER_CC2E, 0); + // Select the Input and set the filter and the prescaler value + if (chan == 0) { // TI1 + MODIFY_REG(TIMx->CCMR1, + (TIM_CCMR1_CC1S | TIM_CCMR1_IC1F | TIM_CCMR1_IC1PSC), + (TIM_CCMR1_CC1S_0 | TIM_CCMR1_IC1F_1));// 4 samples per output transition + MODIFY_REG(TIMx->CCMR1, + (TIM_CCMR1_CC2S | TIM_CCMR1_IC2F | TIM_CCMR1_IC2PSC), + (TIM_CCMR1_CC2S_1 | TIM_CCMR1_IC2F_1)); + } else { // TI2 + MODIFY_REG(TIMx->CCMR1, + (TIM_CCMR1_CC1S | TIM_CCMR1_IC1F | TIM_CCMR1_IC1PSC), + (TIM_CCMR1_CC1S_1 | TIM_CCMR1_IC1F_1)); + MODIFY_REG(TIMx->CCMR1, + (TIM_CCMR1_CC2S | TIM_CCMR1_IC2F | TIM_CCMR1_IC2PSC), + (TIM_CCMR1_CC2S_0 | TIM_CCMR1_IC2F_1)); + } + if (ccr_ch == 0) { + // Select the Polarity as falling on IC2 and rising on IC1 + MODIFY_REG(TIMx->CCER, TIM_CCER_CC1P | TIM_CCER_CC2P, TIM_CCER_CC2P | TIM_CCER_CC1E | TIM_CCER_CC2E); + MODIFY_REG(TIMx->DIER, TIM_DIER_CC1DE | TIM_DIER_CC2DE, TIM_DIER_CC1DE); + } else { + // Select the Polarity as falling on IC1 and rising on IC2 + MODIFY_REG(TIMx->CCER, TIM_CCER_CC1P | TIM_CCER_CC2P, TIM_CCER_CC1P | TIM_CCER_CC1E | TIM_CCER_CC2E); + MODIFY_REG(TIMx->DIER, TIM_DIER_CC1DE | TIM_DIER_CC2DE, TIM_DIER_CC2DE); + } + break; + } + case 2: + case 3: { + MODIFY_REG(TIMx->CCER, TIM_CCER_CC3E | TIM_CCER_CC4E, 0); + // Select the Input and set the filter and the prescaler value + if (chan == 2) { // TI3 + MODIFY_REG(TIMx->CCMR2, + (TIM_CCMR2_CC3S | TIM_CCMR2_IC3F | TIM_CCMR2_IC3PSC), + (TIM_CCMR2_CC3S_0 | TIM_CCMR2_IC3F_1)); + MODIFY_REG(TIMx->CCMR2, + (TIM_CCMR2_CC4S | TIM_CCMR2_IC4F | TIM_CCMR2_IC4PSC), + (TIM_CCMR2_CC4S_1 | TIM_CCMR2_IC4F_1)); + } else { // TI4 + MODIFY_REG(TIMx->CCMR2, + (TIM_CCMR2_CC3S | TIM_CCMR2_IC3F | TIM_CCMR2_IC3PSC), + (TIM_CCMR2_CC3S_1 | TIM_CCMR2_IC3F_1)); + MODIFY_REG(TIMx->CCMR2, + (TIM_CCMR2_CC4S | TIM_CCMR2_IC4F | TIM_CCMR2_IC4PSC), + (TIM_CCMR2_CC4S_0 | TIM_CCMR2_IC4F_1)); + } + if (ccr_ch == 2) { + // Select the Polarity as falling on IC4 and rising on IC3 + MODIFY_REG(TIMx->CCER, TIM_CCER_CC3P | TIM_CCER_CC4P, TIM_CCER_CC4P | TIM_CCER_CC3E | TIM_CCER_CC4E); + MODIFY_REG(TIMx->DIER, TIM_DIER_CC3DE | TIM_DIER_CC4DE, TIM_DIER_CC3DE); + } else { + // Select the Polarity as falling on IC3 and rising on IC4 + MODIFY_REG(TIMx->CCER, TIM_CCER_CC3P | TIM_CCER_CC4P, TIM_CCER_CC3P | TIM_CCER_CC3E | TIM_CCER_CC4E); + MODIFY_REG(TIMx->DIER, TIM_DIER_CC3DE | TIM_DIER_CC4DE, TIM_DIER_CC4DE); + } + break; + + } + default: + break; + } +#else switch(ccr_ch) { case 0: { /* Disable the Channel 1: Reset the CC1E Bit */ @@ -367,6 +628,7 @@ void RCOutput::bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t default: break; } +#endif } /* @@ -385,14 +647,21 @@ __RAMFUNC__ void RCOutput::bdshot_finish_dshot_gcr_transaction(virtual_timer_t* // or the input channel buffer const stm32_dma_stream_t *dma = group->has_shared_ic_up_dma() ? group->dma : group->bdshot.ic_dma[curr_telem_chan]; + osalDbgAssert(dma, "No DMA channel"); // record the transaction size before the stream is released dmaStreamDisable(dma); group->bdshot.dma_tx_size = MIN(uint16_t(GCR_TELEMETRY_BIT_LEN), GCR_TELEMETRY_BIT_LEN - dmaStreamGetTransactionSize(dma)); + stm32_cacheBufferInvalidate(group->dma_buffer, group->bdshot.dma_tx_size); - memcpy(group->bdshot.dma_buffer_copy, group->dma_buffer, sizeof(uint32_t) * group->bdshot.dma_tx_size); + memcpy(group->bdshot.dma_buffer_copy, group->dma_buffer, sizeof(dmar_uint_t) * group->bdshot.dma_tx_size); - group->dshot_state = DshotState::RECV_COMPLETE; + // although it should be possible to start the next DMAR transaction concurrently with receiving + // telemetry, in practice it seems to interfere with the DMA engine + if (group->shared_up_dma && group->bdshot.enabled) { + // next dshot pulse can go out now + chEvtSignalI(group->dshot_waiter, DSHOT_CASCADE); + } // if using input capture DMA and sharing the UP and CH channels then clean up // by assigning the source back to UP @@ -402,9 +671,17 @@ __RAMFUNC__ void RCOutput::bdshot_finish_dshot_gcr_transaction(virtual_timer_t* } #endif - // rotate to the next input channel group->bdshot.prev_telem_chan = group->bdshot.curr_telem_chan; + // rotate to the next input channel, we have to rotate even on failure otherwise + // we will not get data from active channels group->bdshot.curr_telem_chan = bdshot_find_next_ic_channel(*group); + + if (group->bdshot.dma_tx_size > 0) { + group->dshot_state = DshotState::RECV_COMPLETE; + } else { + group->dshot_state = DshotState::RECV_FAILED; + } + // tell the waiting process we've done the DMA chEvtSignalI(group->dshot_waiter, group->dshot_event_mask); #ifdef HAL_GPIO_LINE_GPIO56 @@ -423,7 +700,11 @@ bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan) } // evaluate dshot telemetry - group.bdshot.erpm[chan] = bdshot_decode_telemetry_packet(group.bdshot.dma_buffer_copy, group.bdshot.dma_tx_size); + bool reversed = false; +#if defined(IOMCU_FW) + reversed = (group.bdshot.telem_tim_ch[chan] & 1U) == 0; +#endif + group.bdshot.erpm[chan] = bdshot_decode_telemetry_packet(group.bdshot.dma_buffer_copy, group.bdshot.dma_tx_size, reversed); group.dshot_state = DshotState::IDLE; @@ -440,7 +721,7 @@ bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan) TOGGLE_PIN_DEBUG(57); #endif } - +#if !defined(IOMCU_FW) uint64_t now = AP_HAL::micros64(); if (chan == DEBUG_CHANNEL && (now - group.bdshot.last_print) > 1000000) { hal.console->printf("TELEM: %d <%d Hz, %.1f%% err>", group.bdshot.erpm[chan], group.bdshot.telem_rate[chan], @@ -455,6 +736,7 @@ bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan) group.bdshot.telem_err_rate[chan] = 0; group.bdshot.last_print = now; } +#endif #endif return group.bdshot.erpm[chan] != 0xFFFF; } @@ -493,9 +775,11 @@ __RAMFUNC__ void RCOutput::dma_up_irq_callback(void *p, uint32_t flags) } dmaStreamDisable(group->dma); - if (group->in_serial_dma && irq.waiter) { + if (soft_serial_waiting()) { +#if HAL_SERIAL_ESC_COMM_ENABLED // tell the waiting process we've done the DMA chEvtSignalI(irq.waiter, serial_event_mask); +#endif } else if (!group->in_serial_dma && group->bdshot.enabled) { group->dshot_state = DshotState::SEND_COMPLETE; // sending is done, in 30us the ESC will send telemetry @@ -548,32 +832,65 @@ uint32_t RCOutput::bdshot_get_output_rate_hz(const enum output_mode mode) } } -#define INVALID_ERPM 0xffffU +#define INVALID_ERPM 0xfffU // decode a telemetry packet from a GCR encoded stride buffer, take from betaflight decodeTelemetryPacket // see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625 for a description of the protocol -uint32_t RCOutput::bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t count) +uint32_t RCOutput::bdshot_decode_telemetry_packet(dmar_uint_t* buffer, uint32_t count, bool reversed) { uint32_t value = 0; - uint32_t oldValue = buffer[0]; uint32_t bits = 0; uint32_t len; - for (uint32_t i = 1; i <= count; i++) { - if (i < count) { - int32_t diff = buffer[i] - oldValue; - if (bits >= 21U) { - break; + if (reversed) { + // on F103 we are reading one edge with ICn and the other with ICn+1, the DMA architecture only + // allows to trigger on a single register dictated by the DMA input capture channel being used. + // even though we are reading multiple registers per transfer we always cannot trigger on one or other + // of the registers and if the one we trigger on is the one that is numerically first each register + // pair that we read will be swapped in time. in this case we trigger on ICn and then read CCRn and CCRn+1 + // giving us the new value of ICn and the old value of ICn+1. in order to avoid reading garbage on the + // first read we trigger ICn on the rising edge. this gives us all the data but with each pair of bytes + // transposed. we thus need to untranspose as we decode + dmar_uint_t oldValue = buffer[1]; + + for (int32_t i = 0; i <= count+1; ) { + if (i < count) { + dmar_int_t diff = buffer[i] - oldValue; + if (bits >= 21U) { + break; + } + len = (diff + TELEM_IC_SAMPLE/2U) / TELEM_IC_SAMPLE; + } else { + len = 21U - bits; } - len = (diff + TELEM_IC_SAMPLE/2U) / TELEM_IC_SAMPLE; - } else { - len = 21U - bits; + + value <<= len; + value |= 1U << (len - 1U); + oldValue = buffer[i]; + bits += len; + + i += (i%2 ? -1 : 3); } + } + else { + dmar_uint_t oldValue = buffer[0]; + + for (uint32_t i = 1; i <= count; i++) { + if (i < count) { + dmar_int_t diff = buffer[i] - oldValue; + if (bits >= 21U) { + break; + } + len = (diff + TELEM_IC_SAMPLE/2U) / TELEM_IC_SAMPLE; + } else { + len = 21U - bits; + } - value <<= len; - value |= 1U << (len - 1U); - oldValue = buffer[i]; - bits += len; + value <<= len; + value |= 1U << (len - 1U); + oldValue = buffer[i]; + bits += len; + } } if (bits != 21U) { return INVALID_ERPM; @@ -609,8 +926,16 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c } // eRPM = m << e (see https://github.com/bird-sanctuary/extended-dshot-telemetry) - uint8_t expo = ((encodederpm & 0xfffffe00U) >> 9U) & 0xffU; - uint16_t value = (encodederpm & 0x000001ffU); + uint8_t expo = ((encodederpm & 0xfffffe00U) >> 9U) & 0xffU; // 3bits + uint16_t value = (encodederpm & 0x000001ffU); // 9bits +#if HAL_WITH_ESC_TELEM + uint8_t normalized_chan = chan; +#endif +#if HAL_WITH_IO_MCU + if (AP_BoardConfig::io_dshot()) { + normalized_chan = chan + chan_offset; + } +#endif if (!(value & 0x100U) && (_dshot_esc_type == DSHOT_ESC_BLHELI_EDT || _dshot_esc_type == DSHOT_ESC_BLHELI_EDT_S)) { switch (expo) { @@ -619,7 +944,7 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c TelemetryData t { .temperature_cdeg = int16_t(value * 100) }; - update_telem_data(chan, t, AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE); + update_telem_data(normalized_chan, t, AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE); #endif return false; } @@ -629,7 +954,7 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c TelemetryData t { .voltage = 0.25f * value }; - update_telem_data(chan, t, AP_ESC_Telem_Backend::TelemetryType::VOLTAGE); + update_telem_data(normalized_chan, t, AP_ESC_Telem_Backend::TelemetryType::VOLTAGE); #endif return false; } @@ -639,7 +964,7 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c TelemetryData t { .current = float(value) }; - update_telem_data(chan, t, AP_ESC_Telem_Backend::TelemetryType::CURRENT); + update_telem_data(normalized_chan, t, AP_ESC_Telem_Backend::TelemetryType::CURRENT); #endif return false; } @@ -670,11 +995,21 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c // update the ESC telemetry data if (erpm < INVALID_ERPM) { _bdshot.erpm[chan] = erpm; + _bdshot.update_mask |= 1U<