Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp

438 lines
18 KiB
C++
Raw Blame History

/*
* 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/>.
*
* Code by Andy Piper and Siddharth Bharat Purohit
*
* There really is no dshot reference. For information try these resources:
* https://blck.mn/2016/11/dshot-the-new-kid-on-the-block/
* https://www.swallenhardware.io/battlebots/2019/4/20/a-developers-guide-to-dshot-escs
*/
#include <hal.h>
#if defined(IOMCU_FW) && HAL_DSHOT_ENABLED
// need to give the little guy as much help as possible
#pragma GCC optimize("O2")
#include "RCOutput.h"
#include <AP_Math/AP_Math.h>
#include "GPIO.h"
#include "Scheduler.h"
#if HAL_USE_PWM == TRUE
using namespace ChibiOS;
extern const AP_HAL::HAL& hal;
#ifdef HAL_WITH_BIDIR_DSHOT
THD_WORKING_AREA(dshot_thread_wa, 512);
#else
THD_WORKING_AREA(dshot_thread_wa, 64);
#endif
static const char* rcout_thread_name = "rcout";
void RCOutput::timer_tick()
{
if (dshot_timer_setup) {
return;
}
uint32_t dshot_mask;
if (is_dshot_protocol(get_output_mode(dshot_mask))) {
chThdCreateStatic(dshot_thread_wa, sizeof(dshot_thread_wa),
APM_RCOUT_PRIORITY, &RCOutput::dshot_send_trampoline, this);
dshot_timer_setup = true;
}
}
void RCOutput::dshot_send_trampoline(void *p)
{
RCOutput *rcout = (RCOutput *)p;
rcout->rcout_thread();
}
/*
thread for handling RCOutput send on IOMCU
*/
void RCOutput::rcout_thread() {
// don't start outputting until fully configured
while (!hal.scheduler->is_system_initialized()) {
hal.scheduler->delay_microseconds(1000);
}
rcout_thread_ctx = chThdGetSelfX();
chRegSetThreadNameX(rcout_thread_ctx, rcout_thread_name);
rcout_timer_t last_cycle_run_us = 0;
while (true) {
chEvtWaitOne(EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND);
// start the clock
const rcout_timer_t last_thread_run_us = rcout_micros();
// this is when the cycle is supposed to start
if (_dshot_cycle == 0) {
last_cycle_run_us = rcout_micros();
// 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);
}
}
// 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
const rcout_timer_t timeout_period_us = _dshot_rate ? (_dshot_cycle + 1) * _dshot_period_us : _dshot_period_us;
// timeout is measured from the beginning of the push() that initiated it to preserve periodicity
const rcout_timer_t cycle_start_us = _dshot_rate ? last_cycle_run_us : last_thread_run_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(cycle_start_us, timeout_period_us);
#if AP_HAL_SHARED_DMA_ENABLED
dshot_collect_dma_locks(cycle_start_us, timeout_period_us);
#endif
if (_dshot_rate > 0) {
_dshot_cycle = (_dshot_cycle + 1) % _dshot_rate;
}
}
}
#if defined(STM32F1)
void RCOutput::bdshot_disable_pwm_f1(pwm_group& group)
{
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. */
}
#endif
#if defined(HAL_WITH_BIDIR_DSHOT) && defined(STM32F1)
// 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_f1(pwm_group& group, uint8_t telem_channel)
{
osalSysLock();
stm32_tim_t* TIMx = group.pwm_drv->tim;
bdshot_disable_pwm_f1(group);
// 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.is_chan_enabled(i)) {
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();
}
// see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625
// called from the interrupt
void RCOutput::bdshot_receive_pulses_DMAR_f1(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);
group->pwm_drv->tim->CR1 = 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
group->pwm_drv->tim->DIER = 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;
uint8_t curr_ch = group->bdshot.curr_telem_chan;
// 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->is_chan_enabled(i)) {
continue;
}
if (group->bdshot.telem_tim_ch[curr_ch] == group->bdshot.telem_tim_ch[i]) {
palSetLineMode(group->pal_lines[i], PAL_MODE_INPUT_PULLUP);
}
}
// Initialise ICU channels
bdshot_config_icu_dshot_f1(group->pwm_drv->tim, curr_ch, group->bdshot.telem_tim_ch[curr_ch]);
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);
dmaStreamSetMode(ic_dma,
STM32_DMA_CR_CHSEL(group->dma_ch[curr_ch].channel) |
STM32_DMA_CR_DIR_P2M |
STM32_DMA_CR_PSIZE_HWORD |
STM32_DMA_CR_MSIZE_HWORD |
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
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
// 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_f1(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch)
{
// 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;
}
}
// 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_f1(dmar_uint_t* buffer, uint32_t count, bool reversed)
{
if (!reversed) {
return bdshot_decode_telemetry_packet(buffer, count);
}
uint32_t value = 0;
uint32_t bits = 0;
uint32_t len;
// 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;
}
value <<= len;
value |= 1U << (len - 1U);
oldValue = buffer[i];
bits += len;
i += (i%2 ? -1 : 3);
}
if (bits != 21U) {
return INVALID_ERPM;
}
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 & 0x1fU];
decodedValue |= decode[(value >> 5U) & 0x1fU] << 4U;
decodedValue |= decode[(value >> 10U) & 0x1fU] << 8U;
decodedValue |= decode[(value >> 15U) & 0x1fU] << 12U;
uint32_t csum = decodedValue;
csum = csum ^ (csum >> 8U); // xor bytes
csum = csum ^ (csum >> 4U); // xor nibbles
if ((csum & 0xfU) != 0xfU) {
return INVALID_ERPM;
}
decodedValue >>= 4;
return decodedValue;
}
#endif // HAL_WITH_BIDIR_DSHOT && STM32F1
#endif // HAL_USE_PWM
#endif // IOMCU_FW && HAL_DSHOT_ENABLED
Reference in New Issue View Git Blame Copy Permalink