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<