Separate timer code from PWM/PPM code. The method of looking up a callback is now generic which means other (forthcoming) code can configure callback handlers.
Killed some leftovers of GPIO_Pin* stuff I forgot about. git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@387 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
parent
f663a57613
commit
003c2a91c5
2
Makefile
2
Makefile
|
@ -77,6 +77,7 @@ NAZE_SRC = drv_spi.c \
|
|||
drv_mpu6050.c \
|
||||
drv_l3g4200d.c \
|
||||
drv_pwm.c \
|
||||
drv_timer.c \
|
||||
$(COMMON_SRC)
|
||||
|
||||
# Source files for the FY90Q target
|
||||
|
@ -92,6 +93,7 @@ OLIMEXINO_SRC = drv_spi.c \
|
|||
drv_mpu6050.c \
|
||||
drv_l3g4200d.c \
|
||||
drv_pwm.c \
|
||||
drv_timer.c \
|
||||
$(COMMON_SRC)
|
||||
|
||||
# Search path for baseflight sources
|
||||
|
|
|
@ -647,6 +647,11 @@
|
|||
<FileType>1</FileType>
|
||||
<FilePath>.\src\drv_spi.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>drv_timer.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>.\src\drv_timer.c</FilePath>
|
||||
</File>
|
||||
</Files>
|
||||
</Group>
|
||||
<Group>
|
||||
|
@ -1468,6 +1473,11 @@
|
|||
<FileType>1</FileType>
|
||||
<FilePath>.\src\drv_spi.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>drv_timer.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>.\src\drv_timer.c</FilePath>
|
||||
</File>
|
||||
</Files>
|
||||
</Group>
|
||||
<Group>
|
||||
|
@ -2473,6 +2483,11 @@
|
|||
<FileType>1</FileType>
|
||||
<FilePath>.\src\drv_spi.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>drv_timer.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>.\src\drv_timer.c</FilePath>
|
||||
</File>
|
||||
</Files>
|
||||
</Group>
|
||||
<Group>
|
||||
|
|
42
src/board.h
42
src/board.h
|
@ -110,9 +110,9 @@ typedef struct baro_t
|
|||
#ifdef FY90Q
|
||||
// FY90Q
|
||||
#define LED0_GPIO GPIOC
|
||||
#define LED0_PIN GPIO_Pin_12
|
||||
#define LED0_PIN Pin_12
|
||||
#define LED1_GPIO GPIOA
|
||||
#define LED1_PIN GPIO_Pin_15
|
||||
#define LED1_PIN Pin_15
|
||||
|
||||
#define GYRO
|
||||
#define ACC
|
||||
|
@ -126,9 +126,9 @@ typedef struct baro_t
|
|||
|
||||
// LED2 is using one of the pwm pins (PWM2), so we must not use PWM2. @See pwmInit()
|
||||
#define LED0_GPIO GPIOA
|
||||
#define LED0_PIN GPIO_Pin_1 // D3, PA1/USART2_RTS/ADC1/TIM2_CH3 - "LED2" on silkscreen, Yellow
|
||||
#define LED0_PIN Pin_1 // D3, PA1/USART2_RTS/ADC1/TIM2_CH3 - "LED2" on silkscreen, Yellow
|
||||
#define LED1_GPIO GPIOA
|
||||
#define LED1_PIN GPIO_Pin_5 // D13, PA5/SPI1_SCK/ADC5 - "LED1" on silkscreen, Green
|
||||
#define LED1_PIN Pin_5 // D13, PA5/SPI1_SCK/ADC5 - "LED1" on silkscreen, Green
|
||||
|
||||
#define GYRO
|
||||
#define ACC
|
||||
|
@ -138,13 +138,13 @@ typedef struct baro_t
|
|||
#else
|
||||
// Afroflight32
|
||||
#define LED0_GPIO GPIOB
|
||||
#define LED0_PIN GPIO_Pin_3 // PB3 (LED)
|
||||
#define LED0_PIN Pin_3 // PB3 (LED)
|
||||
#define LED1_GPIO GPIOB
|
||||
#define LED1_PIN GPIO_Pin_4 // PB4 (LED)
|
||||
#define LED1_PIN Pin_4 // PB4 (LED)
|
||||
#define BEEP_GPIO GPIOA
|
||||
#define BEEP_PIN GPIO_Pin_12 // PA12 (Buzzer)
|
||||
#define BEEP_PIN Pin_12 // PA12 (Buzzer)
|
||||
#define BARO_GPIO GPIOC
|
||||
#define BARO_PIN GPIO_Pin_13
|
||||
#define BARO_PIN Pin_13
|
||||
|
||||
#define GYRO
|
||||
#define ACC
|
||||
|
@ -194,12 +194,13 @@ typedef struct baro_t
|
|||
#include "drv_i2c.h"
|
||||
#include "drv_adxl345.h"
|
||||
#include "drv_mpu3050.h"
|
||||
#include "drv_mpu6050.h"
|
||||
#include "drv_l3g4200d.h"
|
||||
#include "drv_pwm.h"
|
||||
#include "drv_uart.h"
|
||||
#else
|
||||
|
||||
#include "drv_mpu6050.h"
|
||||
#include "drv_l3g4200d.h"
|
||||
#include "drv_pwm.h"
|
||||
#include "drv_timer.h"
|
||||
#include "drv_uart.h"
|
||||
#else
|
||||
|
||||
// AfroFlight32
|
||||
#include "drv_adc.h"
|
||||
#include "drv_adxl345.h"
|
||||
|
@ -211,11 +212,12 @@ typedef struct baro_t
|
|||
#include "drv_ledring.h"
|
||||
#include "drv_mma845x.h"
|
||||
#include "drv_mpu3050.h"
|
||||
#include "drv_mpu6050.h"
|
||||
#include "drv_l3g4200d.h"
|
||||
#include "drv_pwm.h"
|
||||
#include "drv_uart.h"
|
||||
#include "drv_hcsr04.h"
|
||||
|
||||
#include "drv_mpu6050.h"
|
||||
#include "drv_l3g4200d.h"
|
||||
#include "drv_pwm.h"
|
||||
#include "drv_timer.h"
|
||||
#include "drv_uart.h"
|
||||
#include "drv_hcsr04.h"
|
||||
|
||||
#endif
|
||||
#endif
|
||||
|
|
|
@ -7,14 +7,14 @@
|
|||
|
||||
#ifdef SOFT_I2C
|
||||
|
||||
#define SCL_H GPIOB->BSRR = GPIO_Pin_10 /* GPIO_SetBits(GPIOB , GPIO_Pin_10) */
|
||||
#define SCL_L GPIOB->BRR = GPIO_Pin_10 /* GPIO_ResetBits(GPIOB , GPIO_Pin_10) */
|
||||
#define SCL_H GPIOB->BSRR = Pin_10 /* GPIO_SetBits(GPIOB , GPIO_Pin_10) */
|
||||
#define SCL_L GPIOB->BRR = Pin_10 /* GPIO_ResetBits(GPIOB , GPIO_Pin_10) */
|
||||
|
||||
#define SDA_H GPIOB->BSRR = GPIO_Pin_11 /* GPIO_SetBits(GPIOB , GPIO_Pin_11) */
|
||||
#define SDA_L GPIOB->BRR = GPIO_Pin_11 /* GPIO_ResetBits(GPIOB , GPIO_Pin_11) */
|
||||
#define SDA_H GPIOB->BSRR = Pin_11 /* GPIO_SetBits(GPIOB , GPIO_Pin_11) */
|
||||
#define SDA_L GPIOB->BRR = Pin_11 /* GPIO_ResetBits(GPIOB , GPIO_Pin_11) */
|
||||
|
||||
#define SCL_read (GPIOB->IDR & GPIO_Pin_10) /* GPIO_ReadInputDataBit(GPIOB , GPIO_Pin_10) */
|
||||
#define SDA_read (GPIOB->IDR & GPIO_Pin_11) /* GPIO_ReadInputDataBit(GPIOB , GPIO_Pin_11) */
|
||||
#define SCL_read (GPIOB->IDR & Pin_10) /* GPIO_ReadInputDataBit(GPIOB , GPIO_Pin_10) */
|
||||
#define SDA_read (GPIOB->IDR & Pin_11) /* GPIO_ReadInputDataBit(GPIOB , GPIO_Pin_11) */
|
||||
|
||||
static void I2C_delay(void)
|
||||
{
|
||||
|
|
232
src/drv_pwm.c
232
src/drv_pwm.c
|
@ -1,49 +1,11 @@
|
|||
#include "board.h"
|
||||
|
||||
#define PULSE_1MS (1000) // 1ms pulse width
|
||||
|
||||
/* FreeFlight/Naze32 timer layout
|
||||
TIM2_CH1 RC1 PWM1
|
||||
TIM2_CH2 RC2 PWM2
|
||||
TIM2_CH3 RC3/UA2_TX PWM3
|
||||
TIM2_CH4 RC4/UA2_RX PWM4
|
||||
TIM3_CH1 RC5 PWM5
|
||||
TIM3_CH2 RC6 PWM6
|
||||
TIM3_CH3 RC7 PWM7
|
||||
TIM3_CH4 RC8 PWM8
|
||||
TIM1_CH1 PWM1 PWM9
|
||||
TIM1_CH4 PWM2 PWM10
|
||||
TIM4_CH1 PWM3 PWM11
|
||||
TIM4_CH2 PWM4 PWM12
|
||||
TIM4_CH3 PWM5 PWM13
|
||||
TIM4_CH4 PWM6 PWM14
|
||||
|
||||
// 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
|
||||
|
||||
Groups that allow running different period (ex 50Hz servos + 400Hz throttle + etc):
|
||||
TIM2 4 channels
|
||||
TIM3 4 channels
|
||||
TIM1 2 channels
|
||||
TIM4 4 channels
|
||||
|
||||
Configuration maps:
|
||||
|
||||
1) multirotor PPM input
|
||||
|
||||
#define PULSE_1MS (1000) // 1ms pulse width
|
||||
|
||||
/*
|
||||
Configuration maps:
|
||||
|
||||
1) multirotor PPM input
|
||||
PWM1 used for PPM
|
||||
PWM5..8 used for motors
|
||||
PWM9..10 used for servo or else motors
|
||||
|
@ -69,33 +31,13 @@
|
|||
PWM1 used for PPM
|
||||
PWM5..8 used for servos
|
||||
PWM9 used for motor throttle +PWM10 for 2nd motor
|
||||
PWM11.14 used for servos
|
||||
*/
|
||||
|
||||
typedef void pwmCallbackPtr(uint8_t port, uint16_t capture);
|
||||
|
||||
static const 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;
|
||||
|
||||
PWM11.14 used for servos
|
||||
*/
|
||||
|
||||
typedef struct {
|
||||
volatile uint16_t *ccr;
|
||||
uint16_t period;
|
||||
|
||||
// for input only
|
||||
uint8_t channel;
|
||||
uint8_t state;
|
||||
|
@ -204,23 +146,12 @@ static void pwmTimeBase(TIM_TypeDef *tim, uint32_t period)
|
|||
TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / 1000000) - 1; // all timers run at 1MHz
|
||||
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
|
||||
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
||||
TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure);
|
||||
}
|
||||
|
||||
static void pwmNVICConfig(uint8_t irq)
|
||||
{
|
||||
NVIC_InitTypeDef NVIC_InitStructure;
|
||||
|
||||
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_TimeBaseInit(tim, &TIM_TimeBaseStructure);
|
||||
}
|
||||
|
||||
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;
|
||||
|
@ -299,93 +230,26 @@ static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value
|
|||
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, Mode_IPD);
|
||||
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)
|
||||
return p;
|
||||
}
|
||||
|
||||
static pwmPortData_t *pwmInConfig(uint8_t port, timerCCCallbackPtr callback, uint8_t channel)
|
||||
{
|
||||
pwmPortData_t *p = &pwmPorts[port];
|
||||
pwmTimeBase(timerHardware[port].tim, 0xFFFF);
|
||||
pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_IPD);
|
||||
pwmICConfig(timerHardware[port].tim, timerHardware[port].channel, TIM_ICPolarity_Rising);
|
||||
TIM_Cmd(timerHardware[port].tim, ENABLE);
|
||||
timerNVICConfig(timerHardware[port].irq);
|
||||
|
||||
p->channel = channel;
|
||||
|
||||
configureTimerCaptureCompareInterrupt(&(timerHardware[port]), port, callback);
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
static void ppmCallback(uint8_t port, uint16_t capture)
|
||||
{
|
||||
uint16_t diff;
|
||||
static uint16_t now;
|
||||
|
@ -456,14 +320,14 @@ bool pwmInit(drv_pwm_config_t *init)
|
|||
uint8_t port = setup[i] & 0x0F;
|
||||
uint8_t mask = setup[i] & 0xF0;
|
||||
|
||||
if (setup[i] == 0xFF) // terminator
|
||||
break;
|
||||
|
||||
#ifdef OLIMEXINO
|
||||
// PWM2 is connected to LED2 on the board and cannot be connected.
|
||||
if (port == PWM2)
|
||||
continue;
|
||||
#endif
|
||||
if (setup[i] == 0xFF) // terminator
|
||||
break;
|
||||
|
||||
#ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER
|
||||
// PWM2 is connected to LED2 on the board and cannot be connected unless you cut LED2_E
|
||||
if (port == PWM2)
|
||||
continue;
|
||||
#endif
|
||||
|
||||
// skip UART ports for GPS
|
||||
if (init->useUART && (port == PWM3 || port == PWM4))
|
||||
|
|
|
@ -33,21 +33,12 @@ enum {
|
|||
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;
|
||||
|
||||
bool pwmInit(drv_pwm_config_t *init); // returns whether driver is asking to calibrate throttle or not
|
||||
void pwmWriteMotor(uint8_t index, uint16_t value);
|
||||
void pwmWriteServo(uint8_t index, uint16_t value);
|
||||
MAX_PORTS
|
||||
};
|
||||
|
||||
bool pwmInit(drv_pwm_config_t *init); // returns whether driver is asking to calibrate throttle or not
|
||||
void pwmWriteMotor(uint8_t index, uint16_t value);
|
||||
void pwmWriteServo(uint8_t index, uint16_t value);
|
||||
uint16_t pwmRead(uint8_t channel);
|
||||
|
||||
// void pwmWrite(uint8_t channel, uint16_t value);
|
||||
|
|
|
@ -0,0 +1,200 @@
|
|||
#include "board.h"
|
||||
|
||||
#define PULSE_1MS (1000) // 1ms pulse width
|
||||
|
||||
/* FreeFlight/Naze32 timer layout
|
||||
TIM2_CH1 RC1 PWM1
|
||||
TIM2_CH2 RC2 PWM2
|
||||
TIM2_CH3 RC3/UA2_TX PWM3
|
||||
TIM2_CH4 RC4/UA2_RX PWM4
|
||||
TIM3_CH1 RC5 PWM5
|
||||
TIM3_CH2 RC6 PWM6
|
||||
TIM3_CH3 RC7 PWM7
|
||||
TIM3_CH4 RC8 PWM8
|
||||
TIM1_CH1 PWM1 PWM9
|
||||
TIM1_CH4 PWM2 PWM10
|
||||
TIM4_CH1 PWM3 PWM11
|
||||
TIM4_CH2 PWM4 PWM12
|
||||
TIM4_CH3 PWM5 PWM13
|
||||
TIM4_CH4 PWM6 PWM14
|
||||
|
||||
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
|
||||
|
||||
Groups that allow running different period (ex 50Hz servos + 400Hz throttle + etc):
|
||||
TIM2 4 channels
|
||||
TIM3 4 channels
|
||||
TIM1 2 channels
|
||||
TIM4 4 channels
|
||||
*/
|
||||
|
||||
const timerHardware_t timerHardware[] = {
|
||||
{ TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, }, // PWM1
|
||||
{ TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, }, // PWM2
|
||||
{ TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 0, }, // PWM3
|
||||
{ TIM2, GPIOA, Pin_3, TIM_Channel_4, TIM2_IRQn, 0, }, // PWM4
|
||||
{ TIM3, GPIOA, Pin_6, TIM_Channel_1, TIM3_IRQn, 0, }, // PWM5
|
||||
{ TIM3, GPIOA, Pin_7, TIM_Channel_2, TIM3_IRQn, 0, }, // PWM6
|
||||
{ TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, }, // PWM7
|
||||
{ TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, }, // PWM8
|
||||
{ TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, }, // PWM9
|
||||
{ TIM1, GPIOA, Pin_11, TIM_Channel_4, TIM1_CC_IRQn, 1, }, // PWM10
|
||||
{ TIM4, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 0, }, // PWM11
|
||||
{ TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 0, }, // PWM12
|
||||
{ TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 0, }, // PWM13
|
||||
{ TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 0, }, // PWM14
|
||||
};
|
||||
|
||||
#define MAX_TIMERS 4 // TIM1..TIM4
|
||||
#define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4
|
||||
|
||||
static TIM_TypeDef *timers[MAX_TIMERS] = {
|
||||
TIM1, TIM2, TIM3, TIM4
|
||||
};
|
||||
|
||||
static uint8_t channels[CC_CHANNELS_PER_TIMER] = {
|
||||
TIM_Channel_1, TIM_Channel_2, TIM_Channel_3, TIM_Channel_4
|
||||
};
|
||||
|
||||
typedef struct timerConfig_s {
|
||||
timerCCCallbackPtr *callback;
|
||||
uint8_t channel;
|
||||
TIM_TypeDef *tim;
|
||||
uint8_t reference;
|
||||
} timerConfig_t;
|
||||
|
||||
timerConfig_t timerConfig[MAX_TIMERS * CC_CHANNELS_PER_TIMER];
|
||||
|
||||
static uint8_t lookupTimerIndex(const TIM_TypeDef *tim)
|
||||
{
|
||||
uint8_t timerIndex = 0;
|
||||
while (timers[timerIndex] != tim) {
|
||||
timerIndex++;
|
||||
}
|
||||
return timerIndex;
|
||||
}
|
||||
|
||||
static uint8_t lookupChannelIndex(const uint8_t channel)
|
||||
{
|
||||
uint8_t channelIndex = 0;
|
||||
while (channels[channelIndex] != channel) {
|
||||
channelIndex++;
|
||||
}
|
||||
return channelIndex;
|
||||
}
|
||||
|
||||
static timerConfig_t *findTimerConfig(TIM_TypeDef *tim, uint8_t channel)
|
||||
{
|
||||
uint8_t timerConfigIndex = (lookupTimerIndex(tim) * MAX_TIMERS) + lookupChannelIndex(channel);
|
||||
return &(timerConfig[timerConfigIndex]);
|
||||
}
|
||||
|
||||
static void timCCxHandler(TIM_TypeDef *tim)
|
||||
{
|
||||
uint16_t capture;
|
||||
timerConfig_t *timerConfig;
|
||||
|
||||
if (TIM_GetITStatus(tim, TIM_IT_CC1) == SET) {
|
||||
TIM_ClearITPendingBit(tim, TIM_IT_CC1);
|
||||
|
||||
timerConfig = findTimerConfig(tim, TIM_Channel_1);
|
||||
capture = TIM_GetCapture1(tim);
|
||||
} else if (TIM_GetITStatus(tim, TIM_IT_CC2) == SET) {
|
||||
TIM_ClearITPendingBit(tim, TIM_IT_CC2);
|
||||
|
||||
timerConfig = findTimerConfig(tim, TIM_Channel_2);
|
||||
capture = TIM_GetCapture2(tim);
|
||||
} else if (TIM_GetITStatus(tim, TIM_IT_CC3) == SET) {
|
||||
TIM_ClearITPendingBit(tim, TIM_IT_CC3);
|
||||
|
||||
timerConfig = findTimerConfig(tim, TIM_Channel_3);
|
||||
capture = TIM_GetCapture3(tim);
|
||||
} else if (TIM_GetITStatus(tim, TIM_IT_CC4) == SET) {
|
||||
TIM_ClearITPendingBit(tim, TIM_IT_CC4);
|
||||
|
||||
timerConfig = findTimerConfig(tim, TIM_Channel_4);
|
||||
capture = TIM_GetCapture4(tim);
|
||||
}
|
||||
|
||||
if (!timerConfig->callback) {
|
||||
return;
|
||||
}
|
||||
timerConfig->callback(timerConfig->reference, capture);
|
||||
}
|
||||
|
||||
void TIM1_CC_IRQHandler(void)
|
||||
{
|
||||
timCCxHandler(TIM1);
|
||||
}
|
||||
|
||||
void TIM2_IRQHandler(void)
|
||||
{
|
||||
timCCxHandler(TIM2);
|
||||
}
|
||||
|
||||
void TIM3_IRQHandler(void)
|
||||
{
|
||||
timCCxHandler(TIM3);
|
||||
}
|
||||
|
||||
void TIM4_IRQHandler(void)
|
||||
{
|
||||
timCCxHandler(TIM4);
|
||||
}
|
||||
|
||||
void timerNVICConfig(uint8_t irq)
|
||||
{
|
||||
NVIC_InitTypeDef NVIC_InitStructure;
|
||||
|
||||
NVIC_InitStructure.NVIC_IRQChannel = irq;
|
||||
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
|
||||
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
|
||||
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
|
||||
NVIC_Init(&NVIC_InitStructure);
|
||||
}
|
||||
|
||||
void configureTimerInputCaptureCompareChannel(TIM_TypeDef *tim, const uint8_t channel)
|
||||
{
|
||||
switch (channel) {
|
||||
case TIM_Channel_1:
|
||||
TIM_ITConfig(tim, TIM_IT_CC1, ENABLE);
|
||||
break;
|
||||
case TIM_Channel_2:
|
||||
TIM_ITConfig(tim, TIM_IT_CC2, ENABLE);
|
||||
break;
|
||||
case TIM_Channel_3:
|
||||
TIM_ITConfig(tim, TIM_IT_CC3, ENABLE);
|
||||
break;
|
||||
case TIM_Channel_4:
|
||||
TIM_ITConfig(tim, TIM_IT_CC4, ENABLE);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void configureTimerChannelCallback(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *callback)
|
||||
{
|
||||
uint8_t timerConfigIndex = (lookupTimerIndex(tim) * MAX_TIMERS) + lookupChannelIndex(channel);
|
||||
timerConfig[timerConfigIndex].callback = callback;
|
||||
timerConfig[timerConfigIndex].channel = channel;
|
||||
timerConfig[timerConfigIndex].reference = reference;
|
||||
}
|
||||
|
||||
void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *callback)
|
||||
{
|
||||
configureTimerChannelCallback(timerHardwarePtr->tim, timerHardwarePtr->channel, reference, callback);
|
||||
configureTimerInputCaptureCompareChannel(timerHardwarePtr->tim, timerHardwarePtr->channel);
|
||||
}
|
|
@ -0,0 +1,20 @@
|
|||
#pragma once
|
||||
|
||||
typedef void timerCCCallbackPtr(uint8_t port, uint16_t capture);
|
||||
|
||||
typedef struct {
|
||||
TIM_TypeDef *tim;
|
||||
GPIO_TypeDef *gpio;
|
||||
uint32_t pin;
|
||||
uint8_t channel;
|
||||
uint8_t irq;
|
||||
uint8_t outputEnable;
|
||||
} timerHardware_t;
|
||||
|
||||
extern const timerHardware_t timerHardware[];
|
||||
|
||||
void timerNVICConfig(uint8_t irq);
|
||||
|
||||
void configureTimerInputCaptureCompareChannel(TIM_TypeDef *tim, const uint8_t channel);
|
||||
void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *callback);
|
||||
void configureTimerChannelCallback(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *callback);
|
|
@ -68,7 +68,7 @@ serialPort_t *serialUSART2(uint32_t baudRate, portmode_t mode)
|
|||
// USART2_TX PA2
|
||||
// USART2_RX PA3
|
||||
gpio.speed = Speed_2MHz;
|
||||
gpio.pin = GPIO_Pin_2;
|
||||
gpio.pin = Pin_2;
|
||||
gpio.mode = Mode_AF_PP;
|
||||
if (mode & MODE_TX)
|
||||
gpioInit(GPIOA, &gpio);
|
||||
|
|
Loading…
Reference in New Issue