rewritten and cleaned up PWM driver
this has been hover-tested. all current functionality *should* work even though the driver has been rewritten. please test carefully, especially servo configurations! git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@197 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop
parent
9b7e26b274
commit
ee76242525
4843
obj/baseflight.hex
4843
obj/baseflight.hex
File diff suppressed because it is too large
Load Diff
882
src/drv_pwm.c
882
src/drv_pwm.c
|
|
@ -2,258 +2,21 @@
|
||||||
|
|
||||||
#define PULSE_1MS (1000) // 1ms pulse width
|
#define PULSE_1MS (1000) // 1ms pulse width
|
||||||
|
|
||||||
// Forward declaration
|
/* FreeFlight/Naze32 timer layout
|
||||||
static void pwmIRQHandler(TIM_TypeDef *tim);
|
TIM2_CH1 RC1 PWM1
|
||||||
static void ppmIRQHandler(TIM_TypeDef *tim);
|
TIM2_CH2 RC2 PWM2
|
||||||
|
TIM2_CH3 RC3/UA2_TX PWM3
|
||||||
// external vars (ugh)
|
TIM2_CH4 RC4/UA2_RX PWM4
|
||||||
extern int16_t failsafeCnt;
|
TIM3_CH1 RC5 PWM5
|
||||||
|
TIM3_CH2 RC6 PWM6
|
||||||
// local vars
|
TIM3_CH3 RC7 PWM7
|
||||||
static struct TIM_Channel {
|
TIM3_CH4 RC8 PWM8
|
||||||
TIM_TypeDef *tim;
|
TIM1_CH1 PWM1 PWM9
|
||||||
uint16_t channel;
|
TIM1_CH4 PWM2 PWM10
|
||||||
uint16_t cc;
|
TIM4_CH1 PWM3 PWM11
|
||||||
} Channels[] = {
|
TIM4_CH2 PWM4 PWM12
|
||||||
{ TIM2, TIM_Channel_1, TIM_IT_CC1 },
|
TIM4_CH3 PWM5 PWM13
|
||||||
{ TIM2, TIM_Channel_2, TIM_IT_CC2 },
|
TIM4_CH4 PWM6 PWM14
|
||||||
{ 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);
|
|
||||||
|
|
||||||
diff = now - last;
|
|
||||||
|
|
||||||
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)
|
|
||||||
{
|
|
||||||
uint32_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, };
|
|
||||||
uint32_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_2MHz;
|
|
||||||
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_2MHz;
|
|
||||||
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]
|
// RX1 TIM2_CH1 PA0 [also PPM] [also used for throttle calibration]
|
||||||
// RX2 TIM2_CH2 PA1
|
// RX2 TIM2_CH2 PA1
|
||||||
|
|
@ -267,154 +30,507 @@ bool pwmInit(drv_pwm_config_t *init)
|
||||||
// Outputs
|
// Outputs
|
||||||
// PWM1 TIM1_CH1 PA8
|
// PWM1 TIM1_CH1 PA8
|
||||||
// PWM2 TIM1_CH4 PA11
|
// PWM2 TIM1_CH4 PA11
|
||||||
// PWM3 TIM4_CH1 PB6 [also I2C1_SCL]
|
// PWM3 TIM4_CH1 PB6? [also I2C1_SCL]
|
||||||
// PWM4 TIM4_CH2 PB7 [also I2C1_SDA]
|
// PWM4 TIM4_CH2 PB7 [also I2C1_SDA]
|
||||||
// PWM5 TIM4_CH3 PB8
|
// PWM5 TIM4_CH3 PB8
|
||||||
// PWM6 TIM4_CH4 PB9
|
// PWM6 TIM4_CH4 PB9
|
||||||
|
|
||||||
// automatic throttle calibration detection: PA0 to ground via bindplug
|
Groups that allow running different period (ex 50Hz servos + 400Hz throttle + etc):
|
||||||
// Configure TIM2_CH1 for input
|
TIM2 4 channels
|
||||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
|
TIM3 4 channels
|
||||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
|
TIM1 2 channels
|
||||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
|
TIM4 4 channels
|
||||||
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
|
||||||
|
|
||||||
#if 0
|
Configuration maps:
|
||||||
// wait a while
|
|
||||||
delay(100);
|
|
||||||
|
|
||||||
for (c = 0; c < 50000; c++) {
|
1) multirotor PPM input
|
||||||
val = GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0);
|
PWM1 used for PPM
|
||||||
if (val) {
|
PWM5..8 used for motors
|
||||||
throttleCal = false;
|
PWM9..10 used for servo or else motors
|
||||||
break;
|
PWM11..14 used for motors
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// use PPM or PWM input
|
2) multirotor PPM input with more servos
|
||||||
usePPMFlag = init->usePPM;
|
PWM1 used for PPM
|
||||||
|
PWM5..8 used for motors
|
||||||
|
PWM9..10 used for servo or else motors
|
||||||
|
PWM11..14 used for servos
|
||||||
|
|
||||||
// preset channels to center
|
2) multirotor PWM input
|
||||||
for (i = 0; i < 8; i++)
|
PWM1..8 used for input
|
||||||
Inputs[i].capture = 1500;
|
PWM9..10 used for servo or else motors
|
||||||
|
PWM11..14 used for motors
|
||||||
|
|
||||||
// Timers run at 1mhz.
|
3) airplane / flying wing w/PWM
|
||||||
// TODO: clean this shit up. Make it all dynamic etc.
|
PWM1..8 used for input
|
||||||
if (init->enableInput)
|
PWM9 used for motor throttle +PWM10 for 2nd motor
|
||||||
pwmInitializeInput(usePPMFlag);
|
PWM11.14 used for servos
|
||||||
|
|
||||||
// Output pins
|
4) airplane / flying wing with PPM
|
||||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_11;
|
PWM1 used for PPM
|
||||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
|
PWM5..8 used for servos
|
||||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
|
PWM9 used for motor throttle +PWM10 for 2nd motor
|
||||||
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
PWM11.14 used for servos
|
||||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9;
|
*/
|
||||||
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
|
||||||
|
typedef void pwmCallbackPtr(uint8_t port, uint16_t capture);
|
||||||
|
|
||||||
|
// This indexes into the read-only hardware definition structure below, as well as into pwmPorts[] structure with dynamic data.
|
||||||
|
enum {
|
||||||
|
PWM1 = 0,
|
||||||
|
PWM2,
|
||||||
|
PWM3,
|
||||||
|
PWM4,
|
||||||
|
PWM5,
|
||||||
|
PWM6,
|
||||||
|
PWM7,
|
||||||
|
PWM8,
|
||||||
|
PWM9,
|
||||||
|
PWM10,
|
||||||
|
PWM11,
|
||||||
|
PWM12,
|
||||||
|
PWM13,
|
||||||
|
PWM14,
|
||||||
|
MAX_PORTS
|
||||||
|
};
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
TIM_TypeDef *tim;
|
||||||
|
GPIO_TypeDef *gpio;
|
||||||
|
uint32_t pin;
|
||||||
|
uint8_t channel;
|
||||||
|
uint8_t irq;
|
||||||
|
uint8_t outputEnable;
|
||||||
|
} pwmHardware_t;
|
||||||
|
|
||||||
|
static pwmHardware_t timerHardware[] = {
|
||||||
|
{ TIM2, GPIOA, GPIO_Pin_0, TIM_Channel_1, TIM2_IRQn, 0, }, // PWM1
|
||||||
|
{ TIM2, GPIOA, GPIO_Pin_1, TIM_Channel_2, TIM2_IRQn, 0, }, // PWM2
|
||||||
|
{ TIM2, GPIOA, GPIO_Pin_2, TIM_Channel_3, TIM2_IRQn, 0, }, // PWM3
|
||||||
|
{ TIM2, GPIOA, GPIO_Pin_3, TIM_Channel_4, TIM2_IRQn, 0, }, // PWM4
|
||||||
|
{ TIM3, GPIOA, GPIO_Pin_6, TIM_Channel_1, TIM3_IRQn, 0, }, // PWM5
|
||||||
|
{ TIM3, GPIOA, GPIO_Pin_7, TIM_Channel_2, TIM3_IRQn, 0, }, // PWM6
|
||||||
|
{ TIM3, GPIOB, GPIO_Pin_0, TIM_Channel_3, TIM3_IRQn, 0, }, // PWM7
|
||||||
|
{ TIM3, GPIOB, GPIO_Pin_1, TIM_Channel_4, TIM3_IRQn, 0, }, // PWM8
|
||||||
|
{ TIM1, GPIOA, GPIO_Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, }, // PWM9
|
||||||
|
{ TIM1, GPIOA, GPIO_Pin_11, TIM_Channel_4, TIM1_CC_IRQn, 1, }, // PWM10
|
||||||
|
{ TIM4, GPIOB, GPIO_Pin_6, TIM_Channel_1, TIM4_IRQn, 0, }, // PWM11
|
||||||
|
{ TIM4, GPIOB, GPIO_Pin_7, TIM_Channel_2, TIM4_IRQn, 0, }, // PWM12
|
||||||
|
{ TIM4, GPIOB, GPIO_Pin_8, TIM_Channel_3, TIM4_IRQn, 0, }, // PWM13
|
||||||
|
{ TIM4, GPIOB, GPIO_Pin_9, TIM_Channel_4, TIM4_IRQn, 0, }, // PWM14
|
||||||
|
};
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
pwmCallbackPtr *callback;
|
||||||
|
volatile uint16_t *ccr;
|
||||||
|
uint16_t period;
|
||||||
|
|
||||||
|
// for input only
|
||||||
|
uint8_t channel;
|
||||||
|
uint8_t state;
|
||||||
|
uint16_t rise;
|
||||||
|
uint16_t fall;
|
||||||
|
uint16_t capture;
|
||||||
|
} pwmPortData_t;
|
||||||
|
|
||||||
|
enum {
|
||||||
|
TYPE_IP = 0x10,
|
||||||
|
TYPE_IW = 0x20,
|
||||||
|
TYPE_M = 0x40,
|
||||||
|
TYPE_S = 0x80,
|
||||||
|
};
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
uint8_t port;
|
||||||
|
uint8_t type;
|
||||||
|
} pwmPortConfig_t;
|
||||||
|
|
||||||
|
#define MAX_MOTORS 12
|
||||||
|
#define MAX_SERVOS 8
|
||||||
|
#define MAX_INPUTS 8
|
||||||
|
|
||||||
|
static pwmPortData_t pwmPorts[MAX_PORTS];
|
||||||
|
static uint16_t captures[MAX_INPUTS];
|
||||||
|
static pwmPortData_t *motors[MAX_MOTORS];
|
||||||
|
static pwmPortData_t *servos[MAX_SERVOS];
|
||||||
|
static uint8_t numMotors = 0;
|
||||||
|
static uint8_t numServos = 0;
|
||||||
|
static uint8_t numInputs = 0;
|
||||||
|
// external vars (ugh)
|
||||||
|
extern int16_t failsafeCnt;
|
||||||
|
|
||||||
|
static const uint8_t multiPPM[] = {
|
||||||
|
PWM1 | TYPE_IP,
|
||||||
|
PWM9 | TYPE_M, // Swap to servo if needed
|
||||||
|
PWM10 | TYPE_M, // Swap to servo if needed
|
||||||
|
PWM11 | TYPE_M,
|
||||||
|
PWM12 | TYPE_M,
|
||||||
|
PWM13 | TYPE_M,
|
||||||
|
PWM14 | TYPE_M,
|
||||||
|
PWM5 | TYPE_M, // Swap to servo if needed
|
||||||
|
PWM6 | TYPE_M, // Swap to servo if needed
|
||||||
|
PWM7 | TYPE_M, // Swap to servo if needed
|
||||||
|
PWM8 | TYPE_M, // Swap to servo if needed
|
||||||
|
0xFF
|
||||||
|
};
|
||||||
|
|
||||||
|
static const uint8_t multiPWM[] = {
|
||||||
|
PWM1 | TYPE_IW,
|
||||||
|
PWM2 | TYPE_IW,
|
||||||
|
PWM3 | TYPE_IW,
|
||||||
|
PWM4 | TYPE_IW,
|
||||||
|
PWM5 | TYPE_IW,
|
||||||
|
PWM6 | TYPE_IW,
|
||||||
|
PWM7 | TYPE_IW,
|
||||||
|
PWM8 | TYPE_IW,
|
||||||
|
PWM9 | TYPE_M, // Swap to servo if needed
|
||||||
|
PWM10 | TYPE_M, // Swap to servo if needed
|
||||||
|
PWM11 | TYPE_M,
|
||||||
|
PWM12 | TYPE_M,
|
||||||
|
PWM13 | TYPE_M,
|
||||||
|
PWM14 | TYPE_M,
|
||||||
|
0xFF
|
||||||
|
};
|
||||||
|
|
||||||
|
static const uint8_t airPPM[] = {
|
||||||
|
PWM1 | TYPE_IP,
|
||||||
|
PWM5 | TYPE_S,
|
||||||
|
PWM6 | TYPE_S,
|
||||||
|
PWM7 | TYPE_S,
|
||||||
|
PWM8 | TYPE_S,
|
||||||
|
PWM9 | TYPE_M,
|
||||||
|
PWM10 | TYPE_M,
|
||||||
|
PWM11 | TYPE_S,
|
||||||
|
PWM12 | TYPE_S,
|
||||||
|
PWM13 | TYPE_S,
|
||||||
|
PWM14 | TYPE_S,
|
||||||
|
0xFF
|
||||||
|
};
|
||||||
|
|
||||||
|
static const uint8_t airPWM[] = {
|
||||||
|
PWM1 | TYPE_IW,
|
||||||
|
PWM2 | TYPE_IW,
|
||||||
|
PWM3 | TYPE_IW,
|
||||||
|
PWM4 | TYPE_IW,
|
||||||
|
PWM5 | TYPE_IW,
|
||||||
|
PWM6 | TYPE_IW,
|
||||||
|
PWM7 | TYPE_IW,
|
||||||
|
PWM8 | TYPE_IW,
|
||||||
|
PWM9 | TYPE_M,
|
||||||
|
PWM10 | TYPE_M,
|
||||||
|
PWM11 | TYPE_S,
|
||||||
|
PWM12 | TYPE_S,
|
||||||
|
PWM13 | TYPE_S,
|
||||||
|
PWM14 | TYPE_S,
|
||||||
|
0xFF
|
||||||
|
};
|
||||||
|
|
||||||
|
static const uint8_t *hardwareMaps[] = {
|
||||||
|
multiPWM,
|
||||||
|
multiPPM,
|
||||||
|
airPWM,
|
||||||
|
airPPM,
|
||||||
|
};
|
||||||
|
|
||||||
|
static void pwmTimeBase(TIM_TypeDef *tim, uint32_t period)
|
||||||
|
{
|
||||||
|
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
|
||||||
|
|
||||||
// Output timers
|
|
||||||
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
|
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
|
||||||
TIM_TimeBaseStructure.TIM_Prescaler = (72 - 1);
|
TIM_TimeBaseStructure.TIM_Period = period - 1;
|
||||||
|
TIM_TimeBaseStructure.TIM_Prescaler = 72 - 1; // all TIM on F1 runs at 72MHz
|
||||||
if (init->useServos) {
|
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
|
||||||
// ch1, 2 for servo
|
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
||||||
TIM_TimeBaseStructure.TIM_Period = (1000000 / init->servoPwmRate) - 1;
|
TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure);
|
||||||
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_2MHz;
|
|
||||||
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
|
||||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
|
|
||||||
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
|
||||||
|
|
||||||
// when in extra servos mode, init lower 4 channels as 50Hz outputs
|
|
||||||
TIM_TimeBaseStructure.TIM_Period = (1000000 / (init->extraServos ? 50 : 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 = init->extraServos ? 6 : 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)
|
static void pwmNVICConfig(uint8_t irq)
|
||||||
{
|
{
|
||||||
if (channel < numOutputChannels)
|
NVIC_InitTypeDef NVIC_InitStructure;
|
||||||
*OutputChannels[channel] = value;
|
|
||||||
|
NVIC_InitStructure.NVIC_IRQChannel = irq;
|
||||||
|
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
|
||||||
|
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
|
||||||
|
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
|
||||||
|
NVIC_Init(&NVIC_InitStructure);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value)
|
||||||
|
{
|
||||||
|
TIM_OCInitTypeDef TIM_OCInitStructure;
|
||||||
|
|
||||||
|
TIM_OCStructInit(&TIM_OCInitStructure);
|
||||||
|
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 = value;
|
||||||
|
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
|
||||||
|
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
|
||||||
|
|
||||||
|
switch (channel) {
|
||||||
|
case TIM_Channel_1:
|
||||||
|
TIM_OC1Init(tim, &TIM_OCInitStructure);
|
||||||
|
TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable);
|
||||||
|
break;
|
||||||
|
case TIM_Channel_2:
|
||||||
|
TIM_OC2Init(tim, &TIM_OCInitStructure);
|
||||||
|
TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable);
|
||||||
|
break;
|
||||||
|
case TIM_Channel_3:
|
||||||
|
TIM_OC3Init(tim, &TIM_OCInitStructure);
|
||||||
|
TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable);
|
||||||
|
break;
|
||||||
|
case TIM_Channel_4:
|
||||||
|
TIM_OC4Init(tim, &TIM_OCInitStructure);
|
||||||
|
TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
|
||||||
|
{
|
||||||
|
TIM_ICInitTypeDef TIM_ICInitStructure;
|
||||||
|
|
||||||
|
TIM_ICStructInit(&TIM_ICInitStructure);
|
||||||
|
TIM_ICInitStructure.TIM_Channel = channel;
|
||||||
|
TIM_ICInitStructure.TIM_ICPolarity = polarity;
|
||||||
|
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
|
||||||
|
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
|
||||||
|
TIM_ICInitStructure.TIM_ICFilter = 0x0;
|
||||||
|
|
||||||
|
TIM_ICInit(tim, &TIM_ICInitStructure);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, uint8_t input)
|
||||||
|
{
|
||||||
|
GPIO_InitTypeDef GPIO_InitStructure;
|
||||||
|
|
||||||
|
GPIO_StructInit(&GPIO_InitStructure);
|
||||||
|
GPIO_InitStructure.GPIO_Pin = pin;
|
||||||
|
if (input)
|
||||||
|
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
|
||||||
|
else
|
||||||
|
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
|
||||||
|
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
|
||||||
|
GPIO_Init(gpio, &GPIO_InitStructure);
|
||||||
|
}
|
||||||
|
|
||||||
|
static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value)
|
||||||
|
{
|
||||||
|
pwmPortData_t *p = &pwmPorts[port];
|
||||||
|
pwmTimeBase(timerHardware[port].tim, period);
|
||||||
|
pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, 0);
|
||||||
|
pwmOCConfig(timerHardware[port].tim, timerHardware[port].channel, value);
|
||||||
|
// Needed only on TIM1
|
||||||
|
if (timerHardware[port].outputEnable)
|
||||||
|
TIM_CtrlPWMOutputs(timerHardware[port].tim, ENABLE);
|
||||||
|
TIM_Cmd(timerHardware[port].tim, ENABLE);
|
||||||
|
|
||||||
|
switch (timerHardware[port].channel) {
|
||||||
|
case TIM_Channel_1:
|
||||||
|
p->ccr = &timerHardware[port].tim->CCR1;
|
||||||
|
break;
|
||||||
|
case TIM_Channel_2:
|
||||||
|
p->ccr = &timerHardware[port].tim->CCR2;
|
||||||
|
break;
|
||||||
|
case TIM_Channel_3:
|
||||||
|
p->ccr = &timerHardware[port].tim->CCR3;
|
||||||
|
break;
|
||||||
|
case TIM_Channel_4:
|
||||||
|
p->ccr = &timerHardware[port].tim->CCR4;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
return p;
|
||||||
|
}
|
||||||
|
|
||||||
|
static pwmPortData_t *pwmInConfig(uint8_t port, pwmCallbackPtr callback, uint8_t channel)
|
||||||
|
{
|
||||||
|
pwmPortData_t *p = &pwmPorts[port];
|
||||||
|
pwmTimeBase(timerHardware[port].tim, 0xFFFF);
|
||||||
|
pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, 1);
|
||||||
|
pwmICConfig(timerHardware[port].tim, timerHardware[port].channel, TIM_ICPolarity_Rising);
|
||||||
|
TIM_Cmd(timerHardware[port].tim, ENABLE);
|
||||||
|
pwmNVICConfig(timerHardware[port].irq);
|
||||||
|
// set callback before configuring interrupts
|
||||||
|
p->callback = callback;
|
||||||
|
p->channel = channel;
|
||||||
|
|
||||||
|
switch (timerHardware[port].channel) {
|
||||||
|
case TIM_Channel_1:
|
||||||
|
TIM_ITConfig(timerHardware[port].tim, TIM_IT_CC1, ENABLE);
|
||||||
|
break;
|
||||||
|
case TIM_Channel_2:
|
||||||
|
TIM_ITConfig(timerHardware[port].tim, TIM_IT_CC2, ENABLE);
|
||||||
|
break;
|
||||||
|
case TIM_Channel_3:
|
||||||
|
TIM_ITConfig(timerHardware[port].tim, TIM_IT_CC3, ENABLE);
|
||||||
|
break;
|
||||||
|
case TIM_Channel_4:
|
||||||
|
TIM_ITConfig(timerHardware[port].tim, TIM_IT_CC4, ENABLE);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
return p;
|
||||||
|
}
|
||||||
|
|
||||||
|
void TIM1_CC_IRQHandler(void)
|
||||||
|
{
|
||||||
|
uint8_t port;
|
||||||
|
|
||||||
|
if (TIM_GetITStatus(TIM1, TIM_IT_CC1) == SET) {
|
||||||
|
port = PWM9;
|
||||||
|
TIM_ClearITPendingBit(TIM1, TIM_IT_CC1);
|
||||||
|
pwmPorts[port].callback(port, TIM_GetCapture1(TIM1));
|
||||||
|
} else if (TIM_GetITStatus(TIM1, TIM_IT_CC4) == SET) {
|
||||||
|
port = PWM10;
|
||||||
|
TIM_ClearITPendingBit(TIM1, TIM_IT_CC4);
|
||||||
|
pwmPorts[port].callback(port, TIM_GetCapture4(TIM1));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static void pwmTIMxHandler(TIM_TypeDef *tim, uint8_t portBase)
|
||||||
|
{
|
||||||
|
int8_t port;
|
||||||
|
|
||||||
|
// Generic CC handler for TIM2,3,4
|
||||||
|
if (TIM_GetITStatus(tim, TIM_IT_CC1) == SET) {
|
||||||
|
port = portBase + 0;
|
||||||
|
TIM_ClearITPendingBit(tim, TIM_IT_CC1);
|
||||||
|
pwmPorts[port].callback(port, TIM_GetCapture1(tim));
|
||||||
|
} else if (TIM_GetITStatus(tim, TIM_IT_CC2) == SET) {
|
||||||
|
port = portBase + 1;
|
||||||
|
TIM_ClearITPendingBit(tim, TIM_IT_CC2);
|
||||||
|
pwmPorts[port].callback(port, TIM_GetCapture2(tim));
|
||||||
|
} else if (TIM_GetITStatus(tim, TIM_IT_CC3) == SET) {
|
||||||
|
port = portBase + 2;
|
||||||
|
TIM_ClearITPendingBit(tim, TIM_IT_CC3);
|
||||||
|
pwmPorts[port].callback(port, TIM_GetCapture3(tim));
|
||||||
|
} else if (TIM_GetITStatus(tim, TIM_IT_CC4) == SET) {
|
||||||
|
port = portBase + 3;
|
||||||
|
TIM_ClearITPendingBit(tim, TIM_IT_CC4);
|
||||||
|
pwmPorts[port].callback(port, TIM_GetCapture4(tim));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void TIM2_IRQHandler(void)
|
||||||
|
{
|
||||||
|
pwmTIMxHandler(TIM2, PWM1); // PWM1..4
|
||||||
|
}
|
||||||
|
|
||||||
|
void TIM3_IRQHandler(void)
|
||||||
|
{
|
||||||
|
pwmTIMxHandler(TIM3, PWM5); // PWM5..8
|
||||||
|
}
|
||||||
|
|
||||||
|
void TIM4_IRQHandler(void)
|
||||||
|
{
|
||||||
|
pwmTIMxHandler(TIM4, PWM11); // PWM11..14
|
||||||
|
}
|
||||||
|
|
||||||
|
static void ppmCallback(uint8_t port, uint16_t capture)
|
||||||
|
{
|
||||||
|
uint16_t diff;
|
||||||
|
static uint16_t now;
|
||||||
|
static uint16_t last = 0;
|
||||||
|
static uint8_t chan = 0;
|
||||||
|
|
||||||
|
last = now;
|
||||||
|
now = capture;
|
||||||
|
diff = now - last;
|
||||||
|
|
||||||
|
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
|
||||||
|
captures[chan] = diff;
|
||||||
|
}
|
||||||
|
chan++;
|
||||||
|
failsafeCnt = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static void pwmCallback(uint8_t port, uint16_t capture)
|
||||||
|
{
|
||||||
|
if (pwmPorts[port].state == 0) {
|
||||||
|
pwmPorts[port].rise = capture;
|
||||||
|
pwmPorts[port].state = 1;
|
||||||
|
pwmICConfig(timerHardware[port].tim, timerHardware[port].channel, TIM_ICPolarity_Falling);
|
||||||
|
} else {
|
||||||
|
pwmPorts[port].fall = capture;
|
||||||
|
// compute capture
|
||||||
|
pwmPorts[port].capture = pwmPorts[port].fall - pwmPorts[port].rise;
|
||||||
|
captures[pwmPorts[port].channel] = pwmPorts[port].capture;
|
||||||
|
// switch state
|
||||||
|
pwmPorts[port].state = 0;
|
||||||
|
pwmICConfig(timerHardware[port].tim, timerHardware[port].channel, TIM_ICPolarity_Rising);
|
||||||
|
// reset failsafe
|
||||||
|
failsafeCnt = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool pwmInit(drv_pwm_config_t *init)
|
||||||
|
{
|
||||||
|
int i = 0;
|
||||||
|
const uint8_t *setup;
|
||||||
|
|
||||||
|
// this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ]
|
||||||
|
if (init->airplane)
|
||||||
|
i = 2; // switch to air hardware config
|
||||||
|
if (init->usePPM)
|
||||||
|
i++; // next index is for PPM
|
||||||
|
|
||||||
|
setup = hardwareMaps[i];
|
||||||
|
|
||||||
|
for (i = 0; i < MAX_PORTS; i++) {
|
||||||
|
uint8_t port = setup[i] & 0x0F;
|
||||||
|
uint8_t mask = setup[i] & 0xF0;
|
||||||
|
|
||||||
|
if (setup[i] == 0xFF) // terminator
|
||||||
|
break;
|
||||||
|
|
||||||
|
// hacks to allow current functionality
|
||||||
|
if (mask & (TYPE_IP | TYPE_IW) && !init->enableInput)
|
||||||
|
mask = 0;
|
||||||
|
|
||||||
|
if (init->useServos) {
|
||||||
|
// remap PWM9+10 as servos
|
||||||
|
if (port == PWM9 || port == PWM10)
|
||||||
|
mask = TYPE_S;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (init->extraServos) {
|
||||||
|
// remap PWM5..8 as servos when used in extended servo mode
|
||||||
|
if (port >= PWM5 && port <= PWM8)
|
||||||
|
mask = TYPE_S;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (mask & TYPE_IP) {
|
||||||
|
pwmInConfig(port, ppmCallback, 0);
|
||||||
|
numInputs = 8;
|
||||||
|
} else if (mask & TYPE_IW) {
|
||||||
|
pwmInConfig(port, pwmCallback, numInputs);
|
||||||
|
numInputs++;
|
||||||
|
} else if (mask & TYPE_M) {
|
||||||
|
motors[numMotors++] = pwmOutConfig(port, 1000000 / init->motorPwmRate, PULSE_1MS);
|
||||||
|
} else if (mask & TYPE_S) {
|
||||||
|
servos[numServos++] = pwmOutConfig(port, 1000000 / init->servoPwmRate, PULSE_1MS);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
void pwmWriteMotor(uint8_t index, uint16_t value)
|
||||||
|
{
|
||||||
|
if (index < numMotors)
|
||||||
|
*motors[index]->ccr = value;
|
||||||
|
}
|
||||||
|
|
||||||
|
void pwmWriteServo(uint8_t index, uint16_t value)
|
||||||
|
{
|
||||||
|
if (index < numServos)
|
||||||
|
*servos[index]->ccr = value;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint16_t pwmRead(uint8_t channel)
|
uint16_t pwmRead(uint8_t channel)
|
||||||
{
|
{
|
||||||
return Inputs[channel].capture;
|
return captures[channel];
|
||||||
}
|
|
||||||
|
|
||||||
uint8_t pwmGetNumOutputChannels(void)
|
|
||||||
{
|
|
||||||
return numOutputChannels;
|
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -5,11 +5,14 @@ typedef struct drv_pwm_config_t {
|
||||||
bool usePPM;
|
bool usePPM;
|
||||||
bool useServos;
|
bool useServos;
|
||||||
bool extraServos; // configure additional 4 channels in PPM mode as servos, not motors
|
bool extraServos; // configure additional 4 channels in PPM mode as servos, not motors
|
||||||
|
bool airplane; // fixed wing hardware config, lots of servos etc
|
||||||
uint16_t motorPwmRate;
|
uint16_t motorPwmRate;
|
||||||
uint16_t servoPwmRate;
|
uint16_t servoPwmRate;
|
||||||
} drv_pwm_config_t;
|
} drv_pwm_config_t;
|
||||||
|
|
||||||
bool pwmInit(drv_pwm_config_t *init); // returns whether driver is asking to calibrate throttle or not
|
bool pwmInit(drv_pwm_config_t *init); // returns whether driver is asking to calibrate throttle or not
|
||||||
void pwmWrite(uint8_t channel, uint16_t value);
|
void pwmWriteMotor(uint8_t index, uint16_t value);
|
||||||
|
void pwmWriteServo(uint8_t index, uint16_t value);
|
||||||
uint16_t pwmRead(uint8_t channel);
|
uint16_t pwmRead(uint8_t channel);
|
||||||
uint8_t pwmGetNumOutputChannels(void);
|
|
||||||
|
// void pwmWrite(uint8_t channel, uint16_t value);
|
||||||
|
|
|
||||||
|
|
@ -51,7 +51,6 @@ static struct PWM_State {
|
||||||
static TIM_ICInitTypeDef TIM_ICInitStructure = { 0, };
|
static TIM_ICInitTypeDef TIM_ICInitStructure = { 0, };
|
||||||
static bool usePPMFlag = false;
|
static bool usePPMFlag = false;
|
||||||
static uint8_t numOutputChannels = 0;
|
static uint8_t numOutputChannels = 0;
|
||||||
static volatile bool rcActive = false;
|
|
||||||
|
|
||||||
void TIM2_IRQHandler(void)
|
void TIM2_IRQHandler(void)
|
||||||
{
|
{
|
||||||
|
|
@ -104,8 +103,6 @@ static void pwmIRQHandler(TIM_TypeDef *tim)
|
||||||
|
|
||||||
if (channel.tim == tim && (TIM_GetITStatus(tim, channel.cc) == SET)) {
|
if (channel.tim == tim && (TIM_GetITStatus(tim, channel.cc) == SET)) {
|
||||||
TIM_ClearITPendingBit(channel.tim, channel.cc);
|
TIM_ClearITPendingBit(channel.tim, channel.cc);
|
||||||
if (i == 0)
|
|
||||||
rcActive = true;
|
|
||||||
|
|
||||||
switch (channel.channel) {
|
switch (channel.channel) {
|
||||||
case TIM_Channel_1:
|
case TIM_Channel_1:
|
||||||
|
|
@ -334,9 +331,4 @@ uint16_t pwmRead(uint8_t channel)
|
||||||
{
|
{
|
||||||
return Inputs[channel].capture;
|
return Inputs[channel].capture;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t pwmGetNumOutputChannels(void)
|
|
||||||
{
|
|
||||||
return numOutputChannels;
|
|
||||||
}
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
25
src/main.c
25
src/main.c
|
|
@ -8,28 +8,6 @@ extern rcReadRawDataPtr rcReadRawFunc;
|
||||||
extern uint16_t pwmReadRawRC(uint8_t chan);
|
extern uint16_t pwmReadRawRC(uint8_t chan);
|
||||||
extern uint16_t spektrumReadRawRC(uint8_t chan);
|
extern uint16_t spektrumReadRawRC(uint8_t chan);
|
||||||
|
|
||||||
void throttleCalibration(void)
|
|
||||||
{
|
|
||||||
uint8_t offset = useServo ? 2 : 0;
|
|
||||||
uint8_t len = pwmGetNumOutputChannels() - offset;
|
|
||||||
uint8_t i;
|
|
||||||
|
|
||||||
LED1_ON;
|
|
||||||
|
|
||||||
// write maxthrottle (high)
|
|
||||||
for (i = offset; i < len; i++)
|
|
||||||
pwmWrite(i, cfg.maxthrottle);
|
|
||||||
|
|
||||||
delay(3000); // 3s delay on high
|
|
||||||
|
|
||||||
// write 1000us (low)
|
|
||||||
for (i = offset; i < len; i++)
|
|
||||||
pwmWrite(i, 1000);
|
|
||||||
|
|
||||||
// blink leds to show we're calibrated and time to remove bind plug
|
|
||||||
failureMode(4);
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(void)
|
int main(void)
|
||||||
{
|
{
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
|
|
@ -83,8 +61,7 @@ int main(void)
|
||||||
pwm_params.motorPwmRate = cfg.motor_pwm_rate;
|
pwm_params.motorPwmRate = cfg.motor_pwm_rate;
|
||||||
pwm_params.servoPwmRate = cfg.servo_pwm_rate;
|
pwm_params.servoPwmRate = cfg.servo_pwm_rate;
|
||||||
|
|
||||||
if (pwmInit(&pwm_params))
|
pwmInit(&pwm_params);
|
||||||
throttleCalibration(); // noreturn
|
|
||||||
|
|
||||||
// configure PWM/CPPM read function. spektrum will override that
|
// configure PWM/CPPM read function. spektrum will override that
|
||||||
rcReadRawFunc = pwmReadRawRC;
|
rcReadRawFunc = pwmReadRawRC;
|
||||||
|
|
|
||||||
20
src/mixer.c
20
src/mixer.c
|
|
@ -57,13 +57,13 @@ void writeServos(void)
|
||||||
|
|
||||||
if (cfg.mixerConfiguration == MULTITYPE_TRI || cfg.mixerConfiguration == MULTITYPE_BI) {
|
if (cfg.mixerConfiguration == MULTITYPE_TRI || cfg.mixerConfiguration == MULTITYPE_BI) {
|
||||||
/* One servo on Motor #4 */
|
/* One servo on Motor #4 */
|
||||||
pwmWrite(0, servo[4]);
|
pwmWriteServo(0, servo[4]);
|
||||||
if (cfg.mixerConfiguration == MULTITYPE_BI)
|
if (cfg.mixerConfiguration == MULTITYPE_BI)
|
||||||
pwmWrite(1, servo[5]);
|
pwmWriteServo(1, servo[5]);
|
||||||
} else {
|
} else {
|
||||||
/* Two servos for camstab or FLYING_WING */
|
/* Two servos for camstab or FLYING_WING */
|
||||||
pwmWrite(0, servo[0]);
|
pwmWriteServo(0, servo[0]);
|
||||||
pwmWrite(1, servo[1]);
|
pwmWriteServo(1, servo[1]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -72,14 +72,9 @@ extern uint8_t cliMode;
|
||||||
void writeMotors(void)
|
void writeMotors(void)
|
||||||
{
|
{
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
uint8_t offset = 0;
|
|
||||||
|
|
||||||
// when servos are enabled, motor outputs 1..2 are for servos only
|
|
||||||
if (useServo)
|
|
||||||
offset = 2;
|
|
||||||
|
|
||||||
for (i = 0; i < numberMotor; i++)
|
for (i = 0; i < numberMotor; i++)
|
||||||
pwmWrite(i + offset, motor[i]);
|
pwmWriteMotor(i, motor[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
void writeAllMotors(int16_t mc)
|
void writeAllMotors(int16_t mc)
|
||||||
|
|
@ -245,8 +240,11 @@ void mixTable(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) {
|
if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) {
|
||||||
|
int offset = 0;
|
||||||
|
if (feature(FEATURE_SERVO_TILT))
|
||||||
|
offset = 2;
|
||||||
for (i = 0; i < 4; i++)
|
for (i = 0; i < 4; i++)
|
||||||
pwmWrite(6 + i, rcData[AUX1 + i]);
|
pwmWriteServo(i + offset, rcData[AUX1 + i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
maxMotor = motor[0];
|
maxMotor = motor[0];
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue