mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 08:58:29 -04:00
401e5c2073
add support for sampling GPIO pins using timer don't restart pwm group when not doing bi-dir fix hwdef generation preproc for TIM DMA decode telemetry at the start of the dshot cycle calculate dshot pulse separation correctly and ensure we output rapidly enough calculate dshot min periods and timeouts correctly refactor dshot_send() into dshot_send_groups() use bi-dir dshot channel mask selectively enable bi-dir RC Channels process bi-dir mask correctly when allocating DMA channels allow UP and CH DMA channel sharing optionally enable bidir vars in hwdef. enable bi-dir dshot in KakuteF7Mini enable bi-dir dshot in OmnibusF4Pro enable bi-dir dshot in OmnibusNanoV6 enable bi-dir dshot in MatekF405 enable bi-dir dshot in fmuv5 enable bi-dir dshot in fmuv3 enable bi-dir dshot in OmnibusF7V2 enable bi-dir dshot in OmnibusNanoV6 enable bi-dir dshot in CubeOrange enable bi-dir dshot in Pixracer enable bi-dir dshot in mRoPixracerPro Co-authored-by: bugobliterator <siddharthbharatpurohit@gmail.com>
576 lines
20 KiB
C++
576 lines
20 KiB
C++
/*
|
|
* This file is free software: you can redistribute it and/or modify it
|
|
* under the terms of the GNU General Public License as published by the
|
|
* Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This file is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
|
* See the GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*
|
|
* Bi-directional dshot based on Betaflight, code by Andy Piper and Siddharth Bharat Purohit
|
|
*/
|
|
#include "RCOutput.h"
|
|
#include <AP_Math/AP_Math.h>
|
|
#include "hwdef/common/stm32_util.h"
|
|
#include <AP_InternalError/AP_InternalError.h>
|
|
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
|
|
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
|
|
|
using namespace ChibiOS;
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#if RCOU_DSHOT_TIMING_DEBUG
|
|
#define DEBUG_CHANNEL 1
|
|
#define TOGGLE_PIN_CH_DEBUG(pin, channel) do { if (channel == DEBUG_CHANNEL) palToggleLine(HAL_GPIO_LINE_GPIO ## pin); } while (0)
|
|
#define TOGGLE_PIN_DEBUG(pin) do { palToggleLine(HAL_GPIO_LINE_GPIO ## pin); } while (0)
|
|
#else
|
|
#define TOGGLE_PIN_CH_DEBUG(pin, channel) do {} while (0)
|
|
#define TOGGLE_PIN_DEBUG(pin) do {} while (0)
|
|
#endif
|
|
|
|
#define TELEM_IC_SAMPLE 16
|
|
|
|
// marker for a disabled channel
|
|
#define CHAN_DISABLED 255
|
|
|
|
// #pragma GCC optimize("Og")
|
|
|
|
|
|
/*
|
|
* enable bi-directional telemetry request for a mask of channels. This is used
|
|
* with DShot to get telemetry feedback
|
|
*/
|
|
void RCOutput::set_bidir_dshot_mask(uint16_t mask)
|
|
{
|
|
_bdshot.mask = (mask >> 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++ ) {
|
|
pwm_group &group = pwm_group_list[i];
|
|
if (((group.ch_mask << chan_offset) & mask) == 0) {
|
|
// this group is not affected
|
|
continue;
|
|
}
|
|
set_group_mode(group);
|
|
}
|
|
}
|
|
|
|
bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
|
|
{
|
|
bool set_curr_chan = false;
|
|
|
|
for (uint8_t i = 0; i < 4; i++) {
|
|
if (group.chan[i] == CHAN_DISABLED ||
|
|
!group.dma_ch[i].have_dma || !(_bdshot.mask & (1 << group.chan[i]))) {
|
|
continue;
|
|
}
|
|
// make sure we don't start on a disabled channel
|
|
if (!set_curr_chan) {
|
|
group.bdshot.curr_telem_chan = i;
|
|
set_curr_chan = true;
|
|
}
|
|
pwmmode_t mode = group.pwm_cfg.channels[i].mode;
|
|
if (mode == PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW ||
|
|
mode == PWM_COMPLEMENTARY_OUTPUT_ACTIVE_HIGH) {
|
|
// Complementary channels don't support input capture
|
|
// 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) {
|
|
group.bdshot.ic_dma_handle[i] = group.dma_handle;
|
|
} else {
|
|
group.bdshot.ic_dma_handle[i] = new Shared_DMA(group.dma_ch[i].stream_id, SHARED_DMA_NONE,
|
|
FUNCTOR_BIND_MEMBER(&RCOutput::bdshot_ic_dma_allocate, void, Shared_DMA *),
|
|
FUNCTOR_BIND_MEMBER(&RCOutput::bdshot_ic_dma_deallocate, void, Shared_DMA *));
|
|
}
|
|
if (!group.bdshot.ic_dma_handle[i]) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
// We might need to do sharing of timers for telemetry feedback
|
|
// due to lack of available DMA channels
|
|
for (uint8_t i = 0; i < 4; i++) {
|
|
if (group.chan[i] == CHAN_DISABLED || !(_bdshot.mask & (1 << group.chan[i]))) {
|
|
continue;
|
|
}
|
|
uint8_t curr_chan = i;
|
|
if (group.bdshot.ic_dma_handle[i]) {
|
|
// we are all good just set and continue
|
|
group.bdshot.telem_tim_ch[i] = curr_chan;
|
|
} else {
|
|
// I guess we have to share, but only channels 1 & 2 or 3 & 4
|
|
if (curr_chan % 2 == 0) {
|
|
curr_chan = curr_chan + 1;
|
|
} else {
|
|
curr_chan = curr_chan - 1;
|
|
}
|
|
if (!group.dma_ch[curr_chan].have_dma) {
|
|
// We can't find a DMA channel to use so
|
|
// return error
|
|
return false;
|
|
}
|
|
if (group.bdshot.ic_dma_handle[i]) {
|
|
INTERNAL_ERROR(AP_InternalError::error_t::dma_fail);
|
|
return false;
|
|
}
|
|
// share up channel if required
|
|
if (group.dma_ch[curr_chan].stream_id == group.dma_up_stream_id) {
|
|
group.bdshot.ic_dma_handle[i] = group.dma_handle;
|
|
} else {
|
|
// we can use the next channel
|
|
group.bdshot.ic_dma_handle[i] = new Shared_DMA(group.dma_ch[curr_chan].stream_id, SHARED_DMA_NONE,
|
|
FUNCTOR_BIND_MEMBER(&RCOutput::bdshot_ic_dma_allocate, void, Shared_DMA *),
|
|
FUNCTOR_BIND_MEMBER(&RCOutput::bdshot_ic_dma_deallocate, void, Shared_DMA *));
|
|
}
|
|
if (!group.bdshot.ic_dma_handle[i]) {
|
|
return false;
|
|
}
|
|
group.bdshot.telem_tim_ch[i] = curr_chan;
|
|
group.dma_ch[i] = group.dma_ch[curr_chan];
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
allocate DMA channel
|
|
*/
|
|
void RCOutput::bdshot_ic_dma_allocate(Shared_DMA *ctx)
|
|
{
|
|
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);
|
|
}
|
|
#endif
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
deallocate DMA channel
|
|
*/
|
|
void RCOutput::bdshot_ic_dma_deallocate(Shared_DMA *ctx)
|
|
{
|
|
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();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625
|
|
// called from the interrupt
|
|
void RCOutput::bdshot_receive_pulses_DMAR(pwm_group* group)
|
|
{
|
|
// make sure the transaction finishes or times out, this function takes a little time to run so the most
|
|
// accurate timing is from the beginning. the pulse time is slightly longer than we need so an extra 10U
|
|
// 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;
|
|
|
|
// Configure Timer
|
|
group->pwm_drv->tim->SR = 0;
|
|
group->pwm_drv->tim->CCER = 0;
|
|
group->pwm_drv->tim->CCMR1 = 0;
|
|
group->pwm_drv->tim->CCMR2 = 0;
|
|
group->pwm_drv->tim->CCER = 0;
|
|
group->pwm_drv->tim->DIER = 0;
|
|
group->pwm_drv->tim->CR1 = 0;
|
|
group->pwm_drv->tim->CR2 = 0;
|
|
group->pwm_drv->tim->PSC = group->bdshot.telempsc;
|
|
|
|
group->dshot_state = DshotState::RECV_START;
|
|
|
|
//TOGGLE_PIN_CH_DEBUG(54, curr_ch);
|
|
group->pwm_drv->tim->ARR = 0xFFFF; // count forever
|
|
group->pwm_drv->tim->CNT = 0;
|
|
|
|
// Initialise ICU channels
|
|
bdshot_config_icu_dshot(group->pwm_drv->tim, curr_ch, group->bdshot.telem_tim_ch[curr_ch]);
|
|
|
|
uint32_t ic_channel = STM32_DMA_CR_CHSEL(group->dma_ch[curr_ch].channel);
|
|
// do a little DMA dance when sharing with UP
|
|
if (group->has_shared_ic_up_dma()) {
|
|
ic_channel = STM32_DMA_CR_CHSEL(group->dma_up_channel);
|
|
}
|
|
const stm32_dma_stream_t *ic_dma =
|
|
group->has_shared_ic_up_dma() ? group->dma : group->bdshot.ic_dma[curr_ch];
|
|
|
|
// Configure DMA
|
|
dmaStreamSetPeripheral(ic_dma, &(group->pwm_drv->tim->DMAR));
|
|
dmaStreamSetMemory0(ic_dma, group->dma_buffer);
|
|
dmaStreamSetTransactionSize(ic_dma, GCR_TELEMETRY_BIT_LEN);
|
|
dmaStreamSetFIFO(ic_dma, STM32_DMA_FCR_DMDIS | STM32_DMA_FCR_FTH_FULL);
|
|
dmaStreamSetMode(ic_dma,
|
|
ic_channel |
|
|
STM32_DMA_CR_DIR_P2M | STM32_DMA_CR_PSIZE_WORD | STM32_DMA_CR_MSIZE_WORD |
|
|
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
|
|
group->pwm_drv->tim->DCR = (0x0D + group->bdshot.telem_tim_ch[curr_ch]) | STM32_TIM_DCR_DBL(0);
|
|
|
|
// Start Timer
|
|
group->pwm_drv->tim->EGR |= STM32_TIM_EGR_UG;
|
|
group->pwm_drv->tim->SR = 0;
|
|
group->pwm_drv->tim->CR1 = TIM_CR1_ARPE | STM32_TIM_CR1_URS | STM32_TIM_CR1_UDIS | STM32_TIM_CR1_CEN;
|
|
dmaStreamEnable(ic_dma);
|
|
}
|
|
|
|
|
|
void RCOutput::bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch)
|
|
{
|
|
switch(ccr_ch) {
|
|
case 0: {
|
|
/* Disable the Channel 1: Reset the CC1E Bit */
|
|
TIMx->CCER &= (uint32_t)~TIM_CCER_CC1E;
|
|
|
|
const uint32_t CCMR1_FILT = TIM_CCMR1_IC1F_1; // 4 samples per output transition
|
|
// Select the Input and set the filter and the prescaler value
|
|
if (chan == 0) {
|
|
MODIFY_REG(TIMx->CCMR1,
|
|
(TIM_CCMR1_CC1S | TIM_CCMR1_IC1F | TIM_CCMR1_IC1PSC),
|
|
(TIM_CCMR1_CC1S_0 | CCMR1_FILT));
|
|
} else {
|
|
MODIFY_REG(TIMx->CCMR1,
|
|
(TIM_CCMR1_CC1S | TIM_CCMR1_IC1F | TIM_CCMR1_IC1PSC),
|
|
(TIM_CCMR1_CC1S_1 | CCMR1_FILT));
|
|
}
|
|
// Select the Polarity as Both Edge and set the CC1E Bit
|
|
MODIFY_REG(TIMx->CCER,
|
|
(TIM_CCER_CC1P | TIM_CCER_CC1NP | TIM_CCER_CC1E),
|
|
(TIM_CCER_CC1P | TIM_CCER_CC1NP | TIM_CCER_CC1E));
|
|
MODIFY_REG(TIMx->DIER, TIM_DIER_CC1DE, TIM_DIER_CC1DE);
|
|
break;
|
|
}
|
|
case 1: {
|
|
// Disable the Channel 2: Reset the CC2E Bit
|
|
TIMx->CCER &= (uint32_t)~TIM_CCER_CC2E;
|
|
|
|
const uint32_t CCMR1_FILT = TIM_CCMR1_IC2F_1;
|
|
// Select the Input and set the filter and the prescaler value
|
|
if (chan == 0) {
|
|
MODIFY_REG(TIMx->CCMR1,
|
|
(TIM_CCMR1_CC2S | TIM_CCMR1_IC2F | TIM_CCMR1_IC2PSC),
|
|
(TIM_CCMR1_CC2S_1 | CCMR1_FILT));
|
|
} else {
|
|
MODIFY_REG(TIMx->CCMR1,
|
|
(TIM_CCMR1_CC2S | TIM_CCMR1_IC2F | TIM_CCMR1_IC2PSC),
|
|
(TIM_CCMR1_CC2S_0 | CCMR1_FILT));
|
|
}
|
|
|
|
// Select the Polarity as Both Edge and set the CC2E Bit
|
|
MODIFY_REG(TIMx->CCER,
|
|
TIM_CCER_CC2P | TIM_CCER_CC2NP | TIM_CCER_CC2E,
|
|
(TIM_CCER_CC2P | TIM_CCER_CC2NP | TIM_CCER_CC2E));
|
|
MODIFY_REG(TIMx->DIER, TIM_DIER_CC2DE, TIM_DIER_CC2DE);
|
|
break;
|
|
}
|
|
case 2: {
|
|
// Disable the Channel 3: Reset the CC3E Bit
|
|
TIMx->CCER &= (uint32_t)~TIM_CCER_CC3E;
|
|
|
|
const uint32_t CCMR2_FILT = TIM_CCMR2_IC3F_1;
|
|
// Select the Input and set the filter and the prescaler value
|
|
if (chan == 2) {
|
|
MODIFY_REG(TIMx->CCMR2,
|
|
(TIM_CCMR2_CC3S | TIM_CCMR2_IC3F | TIM_CCMR2_IC3PSC),
|
|
(TIM_CCMR2_CC3S_0 | CCMR2_FILT));
|
|
} else {
|
|
MODIFY_REG(TIMx->CCMR2,
|
|
(TIM_CCMR2_CC3S | TIM_CCMR2_IC3F | TIM_CCMR2_IC3PSC),
|
|
(TIM_CCMR2_CC3S_1 | CCMR2_FILT));
|
|
}
|
|
|
|
// Select the Polarity as Both Edge and set the CC3E Bit
|
|
MODIFY_REG(TIMx->CCER,
|
|
(TIM_CCER_CC3P | TIM_CCER_CC3NP | TIM_CCER_CC3E),
|
|
(TIM_CCER_CC3P | TIM_CCER_CC3NP | TIM_CCER_CC3E));
|
|
MODIFY_REG(TIMx->DIER, TIM_DIER_CC3DE, TIM_DIER_CC3DE);
|
|
break;
|
|
}
|
|
case 3: {
|
|
// Disable the Channel 4: Reset the CC4E Bit
|
|
TIMx->CCER &= (uint32_t)~TIM_CCER_CC4E;
|
|
|
|
const uint32_t CCMR2_FILT = TIM_CCMR2_IC4F_1;
|
|
// Select the Input and set the filter and the prescaler value
|
|
if (chan == 2) {
|
|
MODIFY_REG(TIMx->CCMR2,
|
|
(TIM_CCMR2_CC4S | TIM_CCMR2_IC4F | TIM_CCMR2_IC4PSC),
|
|
(TIM_CCMR2_CC4S_1 | CCMR2_FILT));
|
|
} else {
|
|
MODIFY_REG(TIMx->CCMR2,
|
|
(TIM_CCMR2_CC4S | TIM_CCMR2_IC4F | TIM_CCMR2_IC4PSC),
|
|
(TIM_CCMR2_CC4S_0 | CCMR2_FILT));
|
|
}
|
|
|
|
// Select the Polarity as Both Edge and set the CC4E Bit
|
|
MODIFY_REG(TIMx->CCER,
|
|
(TIM_CCER_CC4P | TIM_CCER_CC4NP | TIM_CCER_CC4E),
|
|
(TIM_CCER_CC4P | TIM_CCER_CC4NP | TIM_CCER_CC4E));
|
|
|
|
MODIFY_REG(TIMx->DIER, TIM_DIER_CC4DE, TIM_DIER_CC4DE);
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*
|
|
unlock DMA channel after a bi-directional dshot transaction completes
|
|
*/
|
|
void RCOutput::bdshot_finish_dshot_gcr_transaction(void *p)
|
|
{
|
|
pwm_group *group = (pwm_group *)p;
|
|
chSysLockFromISR();
|
|
#ifdef HAL_GPIO_LINE_GPIO56
|
|
TOGGLE_PIN_DEBUG(56);
|
|
#endif
|
|
uint8_t curr_telem_chan = group->bdshot.curr_telem_chan;
|
|
|
|
const stm32_dma_stream_t *dma =
|
|
group->has_shared_ic_up_dma() ? group->dma : group->bdshot.ic_dma[curr_telem_chan];
|
|
// 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);
|
|
|
|
group->dshot_state = DshotState::RECV_COMPLETE;
|
|
|
|
// if using input capture DMA then clean up
|
|
if (group->ic_dma_enabled()) {
|
|
group->bdshot.ic_dma_handle[curr_telem_chan]->unlock_from_IRQ();
|
|
}
|
|
|
|
// rotate to the next input channel
|
|
group->bdshot.prev_telem_chan = group->bdshot.curr_telem_chan;
|
|
group->bdshot.curr_telem_chan = bdshot_find_next_ic_channel(*group);
|
|
group->dma_handle->unlock_from_IRQ();
|
|
#ifdef HAL_GPIO_LINE_GPIO56
|
|
TOGGLE_PIN_DEBUG(56);
|
|
#endif
|
|
chSysUnlockFromISR();
|
|
}
|
|
|
|
/*
|
|
decode returned data from bi-directional dshot
|
|
*/
|
|
bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan)
|
|
{
|
|
if (group.chan[chan] == CHAN_DISABLED) {
|
|
return true;
|
|
}
|
|
|
|
// evaluate dshot telemetry
|
|
group.bdshot.erpm[chan] = bdshot_decode_telemetry_packet(group.bdshot.dma_buffer_copy, group.bdshot.dma_tx_size);
|
|
|
|
group.dshot_state = DshotState::IDLE;
|
|
|
|
#if RCOU_DSHOT_TIMING_DEBUG
|
|
// Record Stats
|
|
if (group.bdshot.erpm[chan] != 0xFFFF) {
|
|
group.bdshot.telem_rate[chan]++;
|
|
} else {
|
|
#ifdef HAL_GPIO_LINE_GPIO57
|
|
TOGGLE_PIN_DEBUG(57);
|
|
#endif
|
|
group.bdshot.telem_err_rate[chan]++;
|
|
#ifdef HAL_GPIO_LINE_GPIO57
|
|
TOGGLE_PIN_DEBUG(57);
|
|
#endif
|
|
}
|
|
|
|
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],
|
|
100.0f * float(group.bdshot.telem_err_rate[chan]) / (group.bdshot.telem_err_rate[chan] + group.bdshot.telem_rate[chan]));
|
|
hal.console->printf(" %ld ", group.bdshot.dma_buffer_copy[0]);
|
|
for (uint8_t l = 1; l < group.bdshot.dma_tx_size; l++) {
|
|
hal.console->printf(" +%ld ", group.bdshot.dma_buffer_copy[l] - group.bdshot.dma_buffer_copy[l-1]);
|
|
}
|
|
hal.console->printf("\n");
|
|
|
|
group.bdshot.telem_rate[chan] = 0;
|
|
group.bdshot.telem_err_rate[chan] = 0;
|
|
group.bdshot.last_print = now;
|
|
}
|
|
#endif
|
|
return group.bdshot.erpm[chan] != 0xFFFF;
|
|
}
|
|
|
|
// Find next valid channel for dshot telem
|
|
uint8_t RCOutput::bdshot_find_next_ic_channel(const pwm_group& group)
|
|
{
|
|
uint8_t chan = group.bdshot.curr_telem_chan;
|
|
for (uint8_t i = 1; i < 4; i++) {
|
|
const uint8_t next_chan = (chan + i) % 4;
|
|
if (group.chan[next_chan] != CHAN_DISABLED &&
|
|
group.bdshot.ic_dma_handle[next_chan] != nullptr) {
|
|
return next_chan;
|
|
}
|
|
}
|
|
return chan;
|
|
}
|
|
|
|
/*
|
|
DMA UP channel interrupt handler. Used to mark DMA send completed for DShot
|
|
*/
|
|
void RCOutput::dma_up_irq_callback(void *p, uint32_t flags)
|
|
{
|
|
pwm_group *group = (pwm_group *)p;
|
|
chSysLockFromISR();
|
|
|
|
// there is a small chance the shared UP CH codepath will get here
|
|
if (group->bdshot.enabled && group->dshot_state == DshotState::RECV_START) {
|
|
chSysUnlockFromISR();
|
|
return;
|
|
}
|
|
|
|
// check nothing bad happened
|
|
if ((flags & STM32_DMA_ISR_TEIF) != 0) {
|
|
INTERNAL_ERROR(AP_InternalError::error_t::dma_fail);
|
|
}
|
|
dmaStreamDisable(group->dma);
|
|
|
|
if (group->in_serial_dma && irq.waiter) {
|
|
// tell the waiting process we've done the DMA
|
|
chEvtSignalI(irq.waiter, serial_event_mask);
|
|
} else if (group->bdshot.enabled) {
|
|
group->dshot_state = DshotState::SEND_COMPLETE;
|
|
// sending is done, in 30us the ESC will send telemetry
|
|
TOGGLE_PIN_DEBUG(55);
|
|
bdshot_receive_pulses_DMAR(group);
|
|
TOGGLE_PIN_DEBUG(55);
|
|
} else {
|
|
// non-bidir case
|
|
chVTSetI(&group->dma_timeout, chTimeUS2I(group->dshot_pulse_time_us + 40), dma_unlock, p);
|
|
}
|
|
|
|
chSysUnlockFromISR();
|
|
}
|
|
|
|
// DMA IC channel handler. Used to mark DMA receive completed for DShot
|
|
void RCOutput::bdshot_dma_ic_irq_callback(void *p, uint32_t flags)
|
|
{
|
|
chSysLockFromISR();
|
|
|
|
// check nothing bad happened
|
|
if ((flags & STM32_DMA_ISR_TEIF) != 0) {
|
|
INTERNAL_ERROR(AP_InternalError::error_t::dma_fail);
|
|
}
|
|
|
|
chSysUnlockFromISR();
|
|
}
|
|
|
|
/*
|
|
returns the bitrate in Hz of the given output_mode
|
|
*/
|
|
uint32_t RCOutput::bdshot_get_output_rate_hz(const enum output_mode mode)
|
|
{
|
|
switch (mode) {
|
|
case MODE_PWM_DSHOT150:
|
|
return 150000U * 5 / 4;
|
|
case MODE_PWM_DSHOT300:
|
|
return 300000U * 5 / 4;
|
|
case MODE_PWM_DSHOT600:
|
|
return 600000U * 5 / 4;
|
|
case MODE_PWM_DSHOT1200:
|
|
return 120000U * 5 / 4;
|
|
default:
|
|
// use 1 to prevent a possible divide-by-zero
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
// 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 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 >= 21) {
|
|
break;
|
|
}
|
|
len = (diff + TELEM_IC_SAMPLE/2) / TELEM_IC_SAMPLE;
|
|
} else {
|
|
len = 21 - bits;
|
|
}
|
|
|
|
value <<= len;
|
|
value |= 1 << (len - 1);
|
|
oldValue = buffer[i];
|
|
bits += len;
|
|
}
|
|
if (bits != 21) {
|
|
return 0xffff;
|
|
}
|
|
|
|
static const uint32_t decode[32] = {
|
|
0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 10, 11, 0, 13, 14, 15,
|
|
0, 0, 2, 3, 0, 5, 6, 7, 0, 0, 8, 1, 0, 4, 12, 0 };
|
|
|
|
uint32_t decodedValue = decode[value & 0x1f];
|
|
decodedValue |= decode[(value >> 5) & 0x1f] << 4;
|
|
decodedValue |= decode[(value >> 10) & 0x1f] << 8;
|
|
decodedValue |= decode[(value >> 15) & 0x1f] << 12;
|
|
|
|
uint32_t csum = decodedValue;
|
|
csum = csum ^ (csum >> 8); // xor bytes
|
|
csum = csum ^ (csum >> 4); // xor nibbles
|
|
|
|
if ((csum & 0xf) != 0xf) {
|
|
return 0xffff;
|
|
}
|
|
decodedValue >>= 4;
|
|
|
|
if (decodedValue == 0x0fff) {
|
|
return 0;
|
|
}
|
|
decodedValue = (decodedValue & 0x000001ff) << ((decodedValue & 0xfffffe00) >> 9);
|
|
if (!decodedValue) {
|
|
return 0xffff;
|
|
}
|
|
uint32_t ret = (1000000 * 60 / 100 + decodedValue / 2) / decodedValue;
|
|
return ret;
|
|
}
|
|
|
|
|
|
#endif // HAL_WITH_BIDIR_DSHOT
|