mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 11:29:58 +03:00
2fd5645dce
changed PPM pulse idle detect to 2.7ms per JimDrew instructions updated keil project w /sonar source files. git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@168 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
423 lines
14 KiB
C
Executable file
423 lines
14 KiB
C
Executable file
#include "board.h"
|
|
|
|
#define PULSE_1MS (1000) // 1ms pulse width
|
|
|
|
// Forward declaration
|
|
static void pwmIRQHandler(TIM_TypeDef *tim);
|
|
static void ppmIRQHandler(TIM_TypeDef *tim);
|
|
|
|
// external vars (ugh)
|
|
extern int16_t failsafeCnt;
|
|
|
|
// local vars
|
|
static struct TIM_Channel {
|
|
TIM_TypeDef *tim;
|
|
uint16_t channel;
|
|
uint16_t cc;
|
|
} Channels[] = {
|
|
{ TIM2, TIM_Channel_1, TIM_IT_CC1 },
|
|
{ TIM2, TIM_Channel_2, TIM_IT_CC2 },
|
|
{ TIM2, TIM_Channel_3, TIM_IT_CC3 },
|
|
{ TIM2, TIM_Channel_4, TIM_IT_CC4 },
|
|
{ TIM3, TIM_Channel_1, TIM_IT_CC1 },
|
|
{ TIM3, TIM_Channel_2, TIM_IT_CC2 },
|
|
{ TIM3, TIM_Channel_3, TIM_IT_CC3 },
|
|
{ TIM3, TIM_Channel_4, TIM_IT_CC4 },
|
|
};
|
|
|
|
static volatile uint16_t *OutputChannels[] = {
|
|
&(TIM1->CCR1),
|
|
&(TIM1->CCR4),
|
|
&(TIM4->CCR1),
|
|
&(TIM4->CCR2),
|
|
&(TIM4->CCR3),
|
|
&(TIM4->CCR4),
|
|
// Extended use during CPPM input
|
|
&(TIM3->CCR1),
|
|
&(TIM3->CCR2),
|
|
&(TIM3->CCR3),
|
|
&(TIM3->CCR4),
|
|
};
|
|
|
|
static struct PWM_State {
|
|
uint8_t state;
|
|
uint16_t rise;
|
|
uint16_t fall;
|
|
uint16_t capture;
|
|
} Inputs[8] = { { 0, } };
|
|
|
|
static TIM_ICInitTypeDef TIM_ICInitStructure = { 0, };
|
|
static bool usePPMFlag = false;
|
|
static uint8_t numOutputChannels = 0;
|
|
static volatile bool rcActive = false;
|
|
|
|
void TIM2_IRQHandler(void)
|
|
{
|
|
if (usePPMFlag)
|
|
ppmIRQHandler(TIM2);
|
|
else
|
|
pwmIRQHandler(TIM2);
|
|
}
|
|
|
|
void TIM3_IRQHandler(void)
|
|
{
|
|
pwmIRQHandler(TIM3);
|
|
}
|
|
|
|
static void ppmIRQHandler(TIM_TypeDef *tim)
|
|
{
|
|
uint16_t diff;
|
|
static uint16_t now;
|
|
static uint16_t last = 0;
|
|
static uint8_t chan = 0;
|
|
|
|
if (TIM_GetITStatus(tim, TIM_IT_CC1) == SET) {
|
|
last = now;
|
|
now = TIM_GetCapture1(tim);
|
|
rcActive = true;
|
|
}
|
|
|
|
TIM_ClearITPendingBit(tim, TIM_IT_CC1);
|
|
|
|
if (now > last) {
|
|
diff = (now - last);
|
|
} else {
|
|
diff = ((0xFFFF - last) + now);
|
|
}
|
|
|
|
if (diff > 2700) { // Per http://www.rcgroups.com/forums/showpost.php?p=21996147&postcount=3960 "So, if you use 2.5ms or higher as being the reset for the PPM stream start, you will be fine. I use 2.7ms just to be safe."
|
|
chan = 0;
|
|
} else {
|
|
if (diff > 750 && diff < 2250 && chan < 8) { // 750 to 2250 ms is our 'valid' channel range
|
|
Inputs[chan].capture = diff;
|
|
}
|
|
chan++;
|
|
failsafeCnt = 0;
|
|
}
|
|
}
|
|
|
|
static void pwmIRQHandler(TIM_TypeDef *tim)
|
|
{
|
|
uint8_t i;
|
|
uint16_t val = 0;
|
|
|
|
for (i = 0; i < 8; i++) {
|
|
struct TIM_Channel channel = Channels[i];
|
|
struct PWM_State *state = &Inputs[i];
|
|
|
|
if (channel.tim == tim && (TIM_GetITStatus(tim, channel.cc) == SET)) {
|
|
TIM_ClearITPendingBit(channel.tim, channel.cc);
|
|
if (i == 0)
|
|
rcActive = true;
|
|
|
|
switch (channel.channel) {
|
|
case TIM_Channel_1:
|
|
val = TIM_GetCapture1(channel.tim);
|
|
break;
|
|
case TIM_Channel_2:
|
|
val = TIM_GetCapture2(channel.tim);
|
|
break;
|
|
case TIM_Channel_3:
|
|
val = TIM_GetCapture3(channel.tim);
|
|
break;
|
|
case TIM_Channel_4:
|
|
val = TIM_GetCapture4(channel.tim);
|
|
break;
|
|
}
|
|
|
|
if (state->state == 0)
|
|
state->rise = val;
|
|
else
|
|
state->fall = val;
|
|
|
|
if (state->state == 0) {
|
|
// switch states
|
|
state->state = 1;
|
|
|
|
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
|
|
TIM_ICInitStructure.TIM_Channel = channel.channel;
|
|
TIM_ICInit(channel.tim, &TIM_ICInitStructure);
|
|
} else {
|
|
// compute capture
|
|
if (state->fall > state->rise)
|
|
state->capture = (state->fall - state->rise);
|
|
else
|
|
state->capture = ((0xffff - state->rise) + state->fall);
|
|
|
|
// switch state
|
|
state->state = 0;
|
|
// reset failsafe
|
|
failsafeCnt = 0;
|
|
|
|
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
|
|
TIM_ICInitStructure.TIM_Channel = channel.channel;
|
|
TIM_ICInit(channel.tim, &TIM_ICInitStructure);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
static void pwmInitializeInput(bool usePPM)
|
|
{
|
|
GPIO_InitTypeDef GPIO_InitStructure = { 0, };
|
|
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, };
|
|
NVIC_InitTypeDef NVIC_InitStructure = { 0, };
|
|
uint8_t i;
|
|
|
|
// Input pins
|
|
if (usePPM) {
|
|
// Configure TIM2_CH1 for PPM input
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
|
|
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
|
|
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
|
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
|
|
|
// Input timer on TIM2 only for PPM
|
|
NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
|
|
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
|
|
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
|
|
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
|
|
NVIC_Init(&NVIC_InitStructure);
|
|
|
|
// TIM2 timebase
|
|
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
|
|
TIM_TimeBaseStructure.TIM_Prescaler = (72 - 1);
|
|
TIM_TimeBaseStructure.TIM_Period = 0xffff;
|
|
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
|
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
|
|
|
|
// Input capture on TIM2_CH1 for PPM
|
|
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
|
|
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
|
|
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
|
|
TIM_ICInitStructure.TIM_ICFilter = 0x0;
|
|
TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
|
|
TIM_ICInit(TIM2, &TIM_ICInitStructure);
|
|
|
|
// TIM2_CH1 capture compare interrupt enable
|
|
TIM_ITConfig(TIM2, TIM_IT_CC1, ENABLE);
|
|
TIM_Cmd(TIM2, ENABLE);
|
|
|
|
// configure number of PWM outputs, in PPM mode, we use bottom 4 channels more more motors
|
|
numOutputChannels = 10;
|
|
} else {
|
|
// Configure TIM2, TIM3 all 4 channels
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_6 | GPIO_Pin_7;
|
|
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
|
|
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
|
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
|
|
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
|
|
|
// Input timers on TIM2 and TIM3 for PWM
|
|
NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
|
|
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
|
|
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
|
|
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
|
|
NVIC_Init(&NVIC_InitStructure);
|
|
NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
|
|
NVIC_Init(&NVIC_InitStructure);
|
|
|
|
// TIM2 and TIM3 timebase
|
|
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
|
|
TIM_TimeBaseStructure.TIM_Prescaler = (72 - 1);
|
|
TIM_TimeBaseStructure.TIM_Period = 0xffff;
|
|
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
|
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
|
|
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
|
|
|
|
// PWM Input capture
|
|
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
|
|
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
|
|
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
|
|
TIM_ICInitStructure.TIM_ICFilter = 0x0;
|
|
|
|
for (i = 0; i < 8; i++) {
|
|
TIM_ICInitStructure.TIM_Channel = Channels[i].channel;
|
|
TIM_ICInit(Channels[i].tim, &TIM_ICInitStructure);
|
|
}
|
|
|
|
TIM_ITConfig(TIM2, TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3 | TIM_IT_CC4, ENABLE);
|
|
TIM_ITConfig(TIM3, TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3 | TIM_IT_CC4, ENABLE);
|
|
TIM_Cmd(TIM2, ENABLE);
|
|
TIM_Cmd(TIM3, ENABLE);
|
|
|
|
// In PWM input mode, all 8 channels are wasted
|
|
numOutputChannels = 6;
|
|
}
|
|
}
|
|
|
|
bool pwmInit(drv_pwm_config_t *init)
|
|
{
|
|
GPIO_InitTypeDef GPIO_InitStructure = { 0, };
|
|
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, };
|
|
TIM_OCInitTypeDef TIM_OCInitStructure = { 0, };
|
|
|
|
uint8_t i, val;
|
|
uint16_t c;
|
|
bool throttleCal = false;
|
|
|
|
// Inputs
|
|
|
|
// RX1 TIM2_CH1 PA0 [also PPM] [also used for throttle calibration]
|
|
// RX2 TIM2_CH2 PA1
|
|
// RX3 TIM2_CH3 PA2 [also UART2_TX]
|
|
// RX4 TIM2_CH4 PA3 [also UART2_RX]
|
|
// RX5 TIM3_CH1 PA6 [also ADC_IN6]
|
|
// RX6 TIM3_CH2 PA7 [also ADC_IN7]
|
|
// RX7 TIM3_CH3 PB0 [also ADC_IN8]
|
|
// RX8 TIM3_CH4 PB1 [also ADC_IN9]
|
|
|
|
// Outputs
|
|
// PWM1 TIM1_CH1 PA8
|
|
// PWM2 TIM1_CH4 PA11
|
|
// PWM3 TIM4_CH1 PB6 [also I2C1_SCL]
|
|
// PWM4 TIM4_CH2 PB7 [also I2C1_SDA]
|
|
// PWM5 TIM4_CH3 PB8
|
|
// PWM6 TIM4_CH4 PB9
|
|
|
|
// automatic throttle calibration detection: PA0 to ground via bindplug
|
|
// Configure TIM2_CH1 for input
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
|
|
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
|
|
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
|
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
|
|
|
#if 0
|
|
// wait a while
|
|
delay(100);
|
|
|
|
for (c = 0; c < 50000; c++) {
|
|
val = GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0);
|
|
if (val) {
|
|
throttleCal = false;
|
|
break;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
// use PPM or PWM input
|
|
usePPMFlag = init->usePPM;
|
|
|
|
// preset channels to center
|
|
for (i = 0; i < 8; i++)
|
|
Inputs[i].capture = 1500;
|
|
|
|
// Timers run at 1mhz.
|
|
// TODO: clean this shit up. Make it all dynamic etc.
|
|
if (init->enableInput)
|
|
pwmInitializeInput(usePPMFlag);
|
|
|
|
// Output pins
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_11;
|
|
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
|
|
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
|
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9;
|
|
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
|
|
|
// Output timers
|
|
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
|
|
TIM_TimeBaseStructure.TIM_Prescaler = (72 - 1);
|
|
|
|
if (init->useServos) {
|
|
// ch1, 2 for servo
|
|
TIM_TimeBaseStructure.TIM_Period = (1000000 / init->servoPwmRate) - 1;
|
|
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
|
|
TIM_TimeBaseStructure.TIM_Period = (1000000 / init->motorPwmRate) - 1;
|
|
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
|
|
} else {
|
|
TIM_TimeBaseStructure.TIM_Period = (1000000 / init->motorPwmRate) - 1;
|
|
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
|
|
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
|
|
}
|
|
|
|
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
|
|
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
|
|
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
|
|
TIM_OCInitStructure.TIM_Pulse = PULSE_1MS;
|
|
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
|
|
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
|
|
|
|
// PWM1,2
|
|
TIM_OC1Init(TIM1, &TIM_OCInitStructure);
|
|
TIM_OC4Init(TIM1, &TIM_OCInitStructure);
|
|
TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable);
|
|
TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Enable);
|
|
|
|
// PWM3,4,5,6
|
|
TIM_OC1Init(TIM4, &TIM_OCInitStructure);
|
|
TIM_OC2Init(TIM4, &TIM_OCInitStructure);
|
|
TIM_OC3Init(TIM4, &TIM_OCInitStructure);
|
|
TIM_OC4Init(TIM4, &TIM_OCInitStructure);
|
|
TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);
|
|
TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);
|
|
TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);
|
|
TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);
|
|
|
|
TIM_Cmd(TIM1, ENABLE);
|
|
TIM_Cmd(TIM4, ENABLE);
|
|
TIM_CtrlPWMOutputs(TIM1, ENABLE);
|
|
TIM_CtrlPWMOutputs(TIM4, ENABLE);
|
|
|
|
// turn on more motor outputs if we're using ppm / not using pwm input
|
|
if (!init->enableInput || init->usePPM) {
|
|
// PWM 7,8,9,10
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
|
|
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
|
|
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
|
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
|
|
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
|
|
|
TIM_TimeBaseStructure.TIM_Period = (1000000 / init->motorPwmRate) - 1;
|
|
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
|
|
|
|
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
|
|
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
|
|
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
|
|
TIM_OCInitStructure.TIM_Pulse = PULSE_1MS;
|
|
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
|
|
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
|
|
|
|
TIM_OC1Init(TIM3, &TIM_OCInitStructure);
|
|
TIM_OC2Init(TIM3, &TIM_OCInitStructure);
|
|
TIM_OC3Init(TIM3, &TIM_OCInitStructure);
|
|
TIM_OC4Init(TIM3, &TIM_OCInitStructure);
|
|
TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
|
|
TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
|
|
TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);
|
|
TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);
|
|
|
|
TIM_Cmd(TIM3, ENABLE);
|
|
TIM_CtrlPWMOutputs(TIM3, ENABLE);
|
|
// configure number of PWM outputs, in PPM/spektrum mode, we use bottom 4 channels more more motors
|
|
numOutputChannels = 10;
|
|
}
|
|
|
|
#if 0
|
|
// throttleCal check part 2: delay 50ms, check if any RC pulses have been received
|
|
delay(50);
|
|
// if rc is on, it was set, check if rc is alive. if it is, cancel.
|
|
if (rcActive)
|
|
throttleCal = false;
|
|
#endif
|
|
|
|
return throttleCal;
|
|
}
|
|
|
|
void pwmWrite(uint8_t channel, uint16_t value)
|
|
{
|
|
if (channel < numOutputChannels)
|
|
*OutputChannels[channel] = value;
|
|
}
|
|
|
|
uint16_t pwmRead(uint8_t channel)
|
|
{
|
|
return Inputs[channel].capture;
|
|
}
|
|
|
|
uint8_t pwmGetNumOutputChannels(void)
|
|
{
|
|
return numOutputChannels;
|
|
}
|