int motorsGetRatio(int id)
{
switch(id) {
case MOTOR_LEFT:
return C_BITS_TO_16(TIM_GetCapture3(MOTORS_GPIO_TIM_M3_4));
case MOTOR_REAR:
return C_BITS_TO_16(TIM_GetCapture4(MOTORS_GPIO_TIM_M3_4));
case MOTOR_RIGHT:
return C_BITS_TO_16(TIM_GetCapture3(MOTORS_GPIO_TIM_M1_2));
case MOTOR_FRONT:
return C_BITS_TO_16(TIM_GetCapture4(MOTORS_GPIO_TIM_M1_2));
}
return -1;
}
void TIM3_IRQHandler(void) {
GPIO_SetBits(GPIOD, GPIO_Pin_13);
static uint16_t total_len = 0;
static uint16_t pulse = 0;
static uint16_t capture = 0;
static uint8_t updown = 0;
static uint8_t channel = 0;
static uint8_t mchannel = 0;
if (TIM_GetITStatus(TIM3, TIM_IT_CC3) != RESET) {
TIM_ClearITPendingBit(TIM3, TIM_IT_CC3);
capture = TIM_GetCapture3(TIM3);
if (GPIO_ReadInputDataBit(PPM1_OUTPUT_PORT, PPM1_OUTPUT_PIN) != 0) {
updown = 1;
} else {
updown = 0;
}
if (updown == 0) {
pulse = 300;
TIM_SetCompare3(TIM3, capture + pulse * PPM_SCALE);
total_len += pulse;
} else {
if (channel < 8) {
pulse = (1000 - 300 + ppm_channel_values[channel]);
TIM_SetCompare3(TIM3, capture + pulse * PPM_SCALE);
total_len += pulse;
channel++;
} else {
TIM_SetCompare3(TIM3, capture + (7000) * PPM_SCALE);
channel = 0;
total_len = 0;
}
}
}
}
/**
* @brief This function handles TIM3 global interrupt request.
* @param None
* @retval None
*/
void TIM3_IRQHandler(void)
{
/* TIM3_CH1 toggling with frequency = 585.9 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC1 );
capture = TIM_GetCapture1(TIM3);
TIM_SetCompare1(TIM3, capture + CCR1_Val );
}
/* TIM3_CH2 toggling with frequency = 1171.8 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC2) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
capture = TIM_GetCapture2(TIM3);
TIM_SetCompare2(TIM3, capture + CCR2_Val);
}
/* TIM3_CH3 toggling with frequency = 2343.75 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC3) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC3);
capture = TIM_GetCapture3(TIM3);
TIM_SetCompare3(TIM3, capture + CCR3_Val);
}
/* TIM3_CH4 toggling with frequency = 4687.5 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC4) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC4);
capture = TIM_GetCapture4(TIM3);
TIM_SetCompare4(TIM3, capture + CCR4_Val);
}
}
/**
* @brief This function handles TIM3 global interrupt request.
* @param None
* @retval None
*/
void TIM3_IRQHandler(void)
{
/* TIM3_CH1 toggling with frequency = 183.1 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC1 );
capture = TIM_GetCapture1(TIM3);
TIM_SetCompare1(TIM3, capture + CCR1_Val );
}
/* TIM3_CH2 toggling with frequency = 366.2 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC2) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
capture = TIM_GetCapture2(TIM3);
TIM_SetCompare2(TIM3, capture + CCR2_Val);
}
/* TIM3_CH3 toggling with frequency = 732.4 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC3) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC3);
capture = TIM_GetCapture3(TIM3);
TIM_SetCompare3(TIM3, capture + CCR3_Val);
}
/* TIM3_CH4 toggling with frequency = 1464.8 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC4) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC4);
capture = TIM_GetCapture4(TIM3);
TIM_SetCompare4(TIM3, capture + CCR4_Val);
}
}
uint16_t Servo::readMicroseconds() const {
if (!this->attached()) {
return 0;
}
uint16_t SERVO_TIM_CCR = 0x0000;
if(PIN_MAP[this->pin].timer_ch == TIM_Channel_1)
{
SERVO_TIM_CCR = TIM_GetCapture1(PIN_MAP[this->pin].timer_peripheral);
}
else if(PIN_MAP[this->pin].timer_ch == TIM_Channel_2)
{
SERVO_TIM_CCR = TIM_GetCapture2(PIN_MAP[this->pin].timer_peripheral);
}
else if(PIN_MAP[this->pin].timer_ch == TIM_Channel_3)
{
SERVO_TIM_CCR = TIM_GetCapture3(PIN_MAP[this->pin].timer_peripheral);
}
else if(PIN_MAP[this->pin].timer_ch == TIM_Channel_4)
{
SERVO_TIM_CCR = TIM_GetCapture4(PIN_MAP[this->pin].timer_peripheral);
}
//pulseWidth = (SERVO_TIM_CCR * 1000000) / ((SERVO_TIM_ARR + 1) * SERVO_TIM_PWM_FREQ);
return SERVO_TIM_CCR;
}
/**
* @brief This function handles TIM3 global interrupt request.
* @param None
* @retval None
*/
void TIM3_IRQHandler(void)
{
/* TIM3_CH1 toggling with frequency = 256.35 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC1 );
uhCapture = TIM_GetCapture1(TIM3);
TIM_SetCompare1(TIM3, uhCapture + uhCCR1_Val );
}
/* TIM3_CH2 toggling with frequency = 512.7 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC2) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
uhCapture = TIM_GetCapture2(TIM3);
TIM_SetCompare2(TIM3, uhCapture + uhCCR2_Val);
}
/* TIM3_CH3 toggling with frequency = 1025.4 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC3) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC3);
uhCapture = TIM_GetCapture3(TIM3);
TIM_SetCompare3(TIM3, uhCapture + uhCCR3_Val);
}
/* TIM3_CH4 toggling with frequency = 2050.78 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC4) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC4);
uhCapture = TIM_GetCapture4(TIM3);
TIM_SetCompare4(TIM3, uhCapture + uhCCR4_Val);
}
}
/**
* @brief This function handles timer 8 capture/compare interrupt request.
* @retval None
*/
void TIM8_CC_IRQHandler(void) {
if (TIM_GetITStatus(TIM8, TIM_IT_CC1) != RESET) {
TIM_ClearITPendingBit(TIM8, TIM_IT_CC1);
if(TIM_Handler[TIM8CH1] != NULL) {
TIM_Handler[TIM8CH1]();
}
TIM_SetCompare1(TIM8, TIM_GetCapture1(TIM8) + TIM_IRQ_period[TIM8CH1]);
}
else if (TIM_GetITStatus(TIM8, TIM_IT_CC2) != RESET) {
TIM_ClearITPendingBit(TIM8, TIM_IT_CC2);
if(TIM_Handler[TIM8CH2] != NULL) {
TIM_Handler[TIM8CH2]();
}
TIM_SetCompare2(TIM8, TIM_GetCapture2(TIM8) + TIM_IRQ_period[TIM8CH2]);
}
else if (TIM_GetITStatus(TIM8, TIM_IT_CC3) != RESET) {
TIM_ClearITPendingBit(TIM8, TIM_IT_CC3);
if(TIM_Handler[TIM8CH3] != NULL) {
TIM_Handler[TIM8CH3]();
}
TIM_SetCompare3(TIM8, TIM_GetCapture3(TIM8) + TIM_IRQ_period[TIM8CH3]);
}
else if (TIM_GetITStatus(TIM8, TIM_IT_CC4) != RESET) {
TIM_ClearITPendingBit(TIM8, TIM_IT_CC4);
if(TIM_Handler[TIM8CH4] != NULL) {
TIM_Handler[TIM8CH4]();
}
TIM_SetCompare4(TIM8, TIM_GetCapture4(TIM8) + TIM_IRQ_period[TIM8CH4]);
}
}
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 TIM4_IRQHandler(void) {
#ifdef PWM_IRQ_TIM4_CH1
if (TIM_GetITStatus(TIM4, TIM_IT_CC1) == SET) {
int8_t pwmPort = PWM_IRQ_TIM4_CH1;
TIM_ClearITPendingBit(TIM4, TIM_IT_CC1);
pwmData[pwmPort].callback(TIM_GetCapture1(TIM4), GPIO_ReadInputDataBit((GPIO_TypeDef *)pwmPorts[pwmPort], (uint16_t)pwmPins[pwmPort]));
}
#endif
#ifdef PWM_IRQ_TIM4_CH2
if (TIM_GetITStatus(TIM4, TIM_IT_CC2) == SET) {
int8_t pwmPort = PWM_IRQ_TIM4_CH2;
TIM_ClearITPendingBit(TIM4, TIM_IT_CC2);
pwmData[pwmPort].callback(TIM_GetCapture2(TIM4), GPIO_ReadInputDataBit((GPIO_TypeDef *)pwmPorts[pwmPort], (uint16_t)pwmPins[pwmPort]));
}
else
#endif
#ifdef PWM_IRQ_TIM4_CH3
if (TIM_GetITStatus(TIM4, TIM_IT_CC3) == SET) {
int8_t pwmPort = PWM_IRQ_TIM4_CH3;
TIM_ClearITPendingBit(TIM4, TIM_IT_CC3);
pwmData[pwmPort].callback(TIM_GetCapture3(TIM4), GPIO_ReadInputDataBit((GPIO_TypeDef *)pwmPorts[pwmPort], (uint16_t)pwmPins[pwmPort]));
}
#endif
#ifdef PWM_IRQ_TIM4_CH4
if (TIM_GetITStatus(TIM4, TIM_IT_CC4) == SET) {
int8_t pwmPort = PWM_IRQ_TIM4_CH4;
TIM_ClearITPendingBit(TIM4, TIM_IT_CC4);
pwmData[pwmPort].callback(TIM_GetCapture4(TIM4), GPIO_ReadInputDataBit((GPIO_TypeDef *)pwmPorts[pwmPort], (uint16_t)pwmPins[pwmPort]));
}
#endif
}
void timerISR(uint8_t timer) {
TIM_TypeDef *TIMx = TIMER_MAP[timer].TIMx;
TimerInfo *cfg = &TIMER_MAP[timer];
// Update for rollover
if (TIM_GetITStatus(TIMx, TIM_IT_Update) != RESET) {
TIM_ClearITPendingBit(TIMx, TIM_IT_Update);
cfg->numRollovers++;
}
// CCx for pwmIn
if (TIM_GetITStatus(TIMx, TIM_IT_CC1) != RESET) {
TIM_ClearITPendingBit(TIMx, TIM_IT_CC1);
timerCCxISR(TIMx, &cfg->channelData[0], TIM_GetCapture1(TIMx), cfg->numRollovers);
}
if (TIM_GetITStatus(TIMx, TIM_IT_CC2) != RESET) {
TIM_ClearITPendingBit(TIMx, TIM_IT_CC2);
timerCCxISR(TIMx, &cfg->channelData[1], TIM_GetCapture2(TIMx), cfg->numRollovers);
}
if (TIM_GetITStatus(TIMx, TIM_IT_CC3) != RESET) {
TIM_ClearITPendingBit(TIMx, TIM_IT_CC3);
timerCCxISR(TIMx, &cfg->channelData[2], TIM_GetCapture3(TIMx), cfg->numRollovers);
}
if (TIM_GetITStatus(TIMx, TIM_IT_CC4) != RESET) {
TIM_ClearITPendingBit(TIMx, TIM_IT_CC4);
timerCCxISR(TIMx, &cfg->channelData[3], TIM_GetCapture4(TIMx), cfg->numRollovers);
}
}
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);
} else {
return; // avoid uninitialised variable dereference
}
if (!timerConfig->callback) {
return;
}
timerConfig->callback(timerConfig->reference, capture);
}
/*******************************************************************************
* Function Name : TIM3_IRQHandler
* Description : This function handles TIM3 global interrupt request.
* Input : None
* Output : None
* Return : None
*******************************************************************************/
void TIM3_IRQHandler(void)
{
vu16 capture = 0; /* 当前捕获计数值局部变量 */
printf("SR = %x\r\n", TIM3->SR);
/*
* TIM2 时钟 = 72 MHz, 分频数 = 7299 + 1, TIM2 counter clock = 10KHz
* CC1 更新率 = TIM2 counter clock / CCRx_Val
*/
if(TIM_GetITStatus(TIM3, TIM_IT_CC1) != RESET)
{
//GPIO_WriteBit(GPIOA, GPIO_Pin_4, (BitAction)(1 - GPIO_ReadOutputDataBit(GPIOA, GPIO_Pin_4)));
PA_PIN_NOT(4);
/* 读出当前计数值 */
capture = TIM_GetCapture1(TIM3);
//printf("capture1 = %d\r\n", capture);
/* 根据当前计数值更新输出捕获寄存器 */
TIM_SetCompare1(TIM3, capture + 40000);
TIM_ClearITPendingBit(TIM3, TIM_IT_CC1);
}
else if(TIM_GetITStatus(TIM3, TIM_IT_CC2) != RESET)
{
//GPIO_WriteBit(GPIOA, GPIO_Pin_5, (BitAction)(1 - GPIO_ReadOutputDataBit(GPIOA, GPIO_Pin_5)));
PA_PIN_NOT(5);
capture = TIM_GetCapture2(TIM3);
//printf("capture2 = %d\r\n", capture);
TIM_SetCompare2(TIM3, capture + 20000);
TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
}
else if(TIM_GetITStatus(TIM3, TIM_IT_CC3) != RESET)
{
//GPIO_WriteBit(GPIOA, GPIO_Pin_6, (BitAction)(1 - GPIO_ReadOutputDataBit(GPIOA, GPIO_Pin_6)));
PA_PIN_NOT(6);
capture = TIM_GetCapture3(TIM3);
//printf("capture3 = %d\r\n", capture);
TIM_SetCompare3(TIM3, capture + 10000);
TIM_ClearITPendingBit(TIM3, TIM_IT_CC3);
}
else if(TIM_GetITStatus(TIM3, TIM_IT_CC4) != RESET)
{
//GPIO_WriteBit(GPIOA, GPIO_Pin_7, (BitAction)(1 - GPIO_ReadOutputDataBit(GPIOA, GPIO_Pin_7)));
PA_PIN_NOT(7);
capture = TIM_GetCapture4(TIM3);
//printf("capture4 = %d\r\n", capture);
TIM_SetCompare4(TIM3, capture + 5000);
TIM_ClearITPendingBit(TIM3, TIM_IT_CC4);
}
else if(TIM3->SR & 0x0001)
{
//capture = TIM_GetCapture4(TIM3);
//printf("capture5 = %d\r\n", capture);
//printf("SR = %x\r\n", TIM3->SR);
TIM3->SR &= ~(1 << 0); // 清除中断标志位
}
}
void TIM3_IRQHandler(void)
{
uint16_t val =0,i=0;
for(i=0;i<4;i++){
if (Channels[i+2].tim == TIM3 && (TIM_GetITStatus(TIM3, Channels[i+2].cc) == SET)){
TIM_ClearITPendingBit(TIM3, Channels[i+2].cc);
switch (Channels[i+2].channel)
{
case TIM_Channel_1:
val = TIM_GetCapture1(TIM3);
break;
case TIM_Channel_2:
val = TIM_GetCapture2(TIM3);
break;
case TIM_Channel_3:
val = TIM_GetCapture3(TIM3);
break;
case TIM_Channel_4:
val = TIM_GetCapture4(TIM3);
break;
}
if (Inputs[i+2].state == 0)
Inputs[i+2].rise = val;
else
Inputs[i+2].fall = val;
if (Inputs[i+2].state == 0) {
Inputs[i+2].state = 1;
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
TIM_ICInitStructure.TIM_Channel = Channels[i+2].channel;
TIM_ICInit(TIM3, &TIM_ICInitStructure);
}else{
if(Inputs[i+2].fall > Inputs[i+2].rise ){
val = (Inputs[i+2].fall - Inputs[i+2].rise);
if(val < 2100 && val>900)
Inputs[i+2].capture = val;
}else{
val = ((0xffff - Inputs[i+2].rise) + Inputs[i+2].fall);
if(val < 2100 && val>900)
Inputs[i+2].capture = val;
}
Inputs[i+2].state = 0;
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_Channel = Channels[i+2].channel;
TIM_ICInit(TIM3, &TIM_ICInitStructure);
}
}
}
rc_in.thr=Inputs[0].capture;
rc_in.roll=Inputs[1].capture;
rc_in.pitch=Inputs[2].capture;
rc_in.yaw=Inputs[3].capture;
rc_in.mode=Inputs[4].capture;
rc_in.serve=Inputs[5].capture;
}
/**
* @brief Return the angle value related to the specified servo.
* @param servo: pointer to a ausbeeServo structure that contains the
* configuration information for the specified servo.
* @retval angle: the servo angle
*/
uint8_t ausbeeGetAngleServo(ausbeeServo* servo)
{
if(servo->CHANx == TIM_Channel_1)
return (uint8_t)( (TIM_GetCapture1(servo->TIMx)-servo->minValue)*100/(servo->maxValue-servo->minValue));
else if(servo->CHANx == TIM_Channel_2)
return (uint8_t)( (TIM_GetCapture2(servo->TIMx)-servo->minValue)*100/(servo->maxValue-servo->minValue));
else if(servo->CHANx == TIM_Channel_3)
return (uint8_t)( (TIM_GetCapture3(servo->TIMx)-servo->minValue)*100/(servo->maxValue-servo->minValue));
else
return (uint8_t)( (TIM_GetCapture4(servo->TIMx)-servo->minValue)*100/(servo->maxValue-servo->minValue));
}
void TIM3_IRQHandler( void )
{
u16 capture = 0;
if( TIM_GetITStatus( TIM3, TIM_IT_CC3 ) != RESET )
{
capture = TIM_GetCapture3( TIM3 );
TIM_SetCompare3( TIM3, capture + CCR_Val + 1 );
TIM_ClearITPendingBit( TIM3, TIM_IT_CC3 );
}
}
uint16_t motor_get_speed(motor_chan_t motor) {
switch (motor) {
case MOTOR_1:
return TIM_GetCapture1(MOTOR_TIMER);
case MOTOR_2:
return TIM_GetCapture2(MOTOR_TIMER);
case MOTOR_3:
return TIM_GetCapture3(MOTOR_TIMER);
case MOTOR_4:
return TIM_GetCapture4(MOTOR_TIMER);
}
return 0;
}
void TIM2_IRQHandler(void) {
int8_t pwmPort;
if (TIM_GetITStatus(TIM2, TIM_IT_CC3) == SET) {
pwmPort = PWM_IRQ_TIM2_CH3;
TIM_ClearITPendingBit(TIM2, TIM_IT_CC3);
pwmData[pwmPort].callback(TIM_GetCapture3(TIM2), GPIO_ReadInputDataBit((GPIO_TypeDef *)pwmPorts[pwmPort], (uint16_t)pwmPins[pwmPort]));
}
else if (TIM_GetITStatus(TIM2, TIM_IT_CC4) == SET) {
pwmPort = PWM_IRQ_TIM2_CH4;
TIM_ClearITPendingBit(TIM2, TIM_IT_CC4);
pwmData[pwmPort].callback(TIM_GetCapture4(TIM2), GPIO_ReadInputDataBit((GPIO_TypeDef *)pwmPorts[pwmPort], (uint16_t)pwmPins[pwmPort]));
}
}
// Systick subroutines
void feedback (uint16_t time)
{
static uint16_t ticker = 0;
if ((uint16_t)(ticker+time)==GetTicker())
{
ticker = GetTicker();
fb_LFL_2 = +((int16_t)TIM_GetCapture1(TIM5)-phi0_LFL_2)/angle_step;
fb_LFL_1 = -((int16_t)TIM_GetCapture2(TIM5)-phi0_LFL_1)/angle_step;
fb_LFL_0 = -((int16_t)TIM_GetCapture3(TIM5)-phi0_LFL_0)/angle_step;
fb_LHL_2 = -((int16_t)TIM_GetCapture2(TIM2)-phi0_LHL_2)/angle_step;
fb_LHL_1 = +((int16_t)TIM_GetCapture3(TIM2)-phi0_LHL_1)/angle_step;
fb_LHL_0 = -((int16_t)TIM_GetCapture4(TIM2)-phi0_LHL_0)/angle_step;
fb_RFL_2 = -((int16_t)TIM_GetCapture1(TIM3)-phi0_RFL_2)/angle_step;
fb_RFL_1 = +((int16_t)TIM_GetCapture2(TIM3)-phi0_RFL_1)/angle_step;
fb_RFL_0 = -((int16_t)TIM_GetCapture3(TIM3)-phi0_RFL_0)/angle_step;
fb_RHL_2 = +((int16_t)TIM_GetCapture1(TIM4)-phi0_RHL_2)/angle_step;
fb_RHL_1 = -((int16_t)TIM_GetCapture2(TIM4)-phi0_RHL_1)/angle_step;
fb_RHL_0 = -((int16_t)TIM_GetCapture3(TIM4)-phi0_RHL_0)/angle_step;
}
}
/**
* @brief This function handles TIM4 global interrupt request.
* @param None
* @retval None
*/
void TIM4_IRQHandler(void)
{
uint16_t capture = 0;
/* TIM4_CH1 toggling */
if (TIM_GetITStatus(TIM4, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit(TIM4, TIM_IT_CC1 );
capture = TIM_GetCapture1(TIM4);
TIM_SetCompare1(TIM4, capture + Channel1Pulse );
}
/* TIM4_CH2 toggling */
if (TIM_GetITStatus(TIM4, TIM_IT_CC2) != RESET)
{
TIM_ClearITPendingBit(TIM4, TIM_IT_CC2);
capture = TIM_GetCapture2(TIM4);
TIM_SetCompare2(TIM4, capture + Channel2Pulse);
}
/* TIM4_CH3 toggling */
if (TIM_GetITStatus(TIM4, TIM_IT_CC3) != RESET)
{
TIM_ClearITPendingBit(TIM4, TIM_IT_CC3);
capture = TIM_GetCapture3(TIM4);
TIM_SetCompare3(TIM4, capture + Channel3Pulse);
}
/* TIM4_CH4 toggling */
if (TIM_GetITStatus(TIM4, TIM_IT_CC4) != RESET)
{
TIM_ClearITPendingBit(TIM4, TIM_IT_CC4);
capture = TIM_GetCapture4(TIM4);
TIM_SetCompare4(TIM4, capture + Channel4Pulse);
}
/* TIM_Update reset all */
if (TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET)
{
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
TIM_SetCompare1(TIM4, Channel1Pulse);
TIM_SetCompare2(TIM4, Channel2Pulse);
TIM_SetCompare3(TIM4, Channel3Pulse);
TIM_SetCompare4(TIM4, Channel4Pulse);
}
}
/**
* @brief This function handles TIM3 global interrupt request.
* @param None
* @retval None
*/
void TIM3_IRQHandler(void)
{
if (TIM_GetITStatus(TIM3, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC1);
/* LED1 toggling with frequency = 73.24 Hz */
STM_EVAL_LEDToggle(LED1);
capture = TIM_GetCapture1(TIM3);
TIM_SetCompare1(TIM3, capture + CCR1_Val);
}
else if (TIM_GetITStatus(TIM3, TIM_IT_CC2) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
/* LED2 toggling with frequency = 109.8 Hz */
STM_EVAL_LEDToggle(LED2);
capture = TIM_GetCapture2(TIM3);
TIM_SetCompare2(TIM3, capture + CCR2_Val);
}
else if (TIM_GetITStatus(TIM3, TIM_IT_CC3) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC3);
/* LED3 toggling with frequency = 219.7 Hz */
STM_EVAL_LEDToggle(LED3);
capture = TIM_GetCapture3(TIM3);
TIM_SetCompare3(TIM3, capture + CCR3_Val);
}
else
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC4);
/* LED4 toggling with frequency = 439.4 Hz */
STM_EVAL_LEDToggle(LED4);
capture = TIM_GetCapture4(TIM3);
TIM_SetCompare4(TIM3, capture + CCR4_Val);
}
}
static void timCCxHandler(TIM_TypeDef *tim)
{
uint16_t capture;
timerConfig_t *timerConfig;
uint8_t channelIndex = 0;
for (channelIndex = 0; channelIndex < CC_CHANNELS_PER_TIMER; channelIndex++) {
uint8_t channel = channels[channelIndex];
if (channel == TIM_Channel_1 && 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 (channel == TIM_Channel_2 && 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 (channel == TIM_Channel_3 && 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 (channel == TIM_Channel_4 && TIM_GetITStatus(tim, TIM_IT_CC4) == SET) {
TIM_ClearITPendingBit(tim, TIM_IT_CC4);
timerConfig = findTimerConfig(tim, TIM_Channel_4);
capture = TIM_GetCapture4(tim);
} else {
continue; // avoid uninitialised variable dereference
}
if (!timerConfig->callback) {
continue;
}
timerConfig->callback(timerConfig->reference, capture);
}
}
void RHL_0_execute(uint16_t time) // time = time*0.1 (ms)
{
static uint16_t ticker = 0;
if ((uint16_t)(ticker+time)==GetTicker())
{
ticker = GetTicker();
CCR_now=TIM_GetCapture3(TIM4);
if (phi_RHL_0>CCR_now)
{
CCR_now++;
if ((SysTicker % k_RHL_0) ==0)
TIM_SetCompare3(TIM4, CCR_now);
}
else if (phi_RHL_0<CCR_now)
{
CCR_now--;
if ((SysTicker % k_RHL_0) ==0)
TIM_SetCompare3(TIM4, CCR_now);
}
}
}
void LHL_1_execute(uint16_t time) // time = time*0.1 (ms)
{
static uint16_t ticker = 0;
if ((uint16_t)(ticker+time)==GetTicker())
{
ticker = GetTicker();
CCR_now=TIM_GetCapture3(TIM2);
if (phi_LHL_1>CCR_now)
{
CCR_now++;
if ((SysTicker % k_LHL_1) ==0)
TIM_SetCompare3(TIM2, CCR_now);
}
else if (phi_LHL_1<CCR_now)
{
CCR_now--;
if ((SysTicker % k_LHL_1) ==0)
TIM_SetCompare3(TIM2, CCR_now);
}
}
}
/**
* @brief This function handles TIM3 global interrupt request.
* @param None
* @retval None
*/
void TIM3_IRQHandler(void)
{
if (TIM_GetITStatus(TIM3, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC1);
/* LED4 toggling with frequency = 4.57 Hz */
STM_EVAL_LEDToggle(LED4);
capture = TIM_GetCapture1(TIM3);
TIM_SetCompare1(TIM3, capture + CCR1_Val);
}
else if (TIM_GetITStatus(TIM3, TIM_IT_CC2) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
/* LED3 toggling with frequency = 9.15 Hz */
STM_EVAL_LEDToggle(LED3);
capture = TIM_GetCapture2(TIM3);
TIM_SetCompare2(TIM3, capture + CCR2_Val);
}
else if (TIM_GetITStatus(TIM3, TIM_IT_CC3) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC3);
/* LED5 toggling with frequency = 18.31 Hz */
STM_EVAL_LEDToggle(LED5);
capture = TIM_GetCapture3(TIM3);
TIM_SetCompare3(TIM3, capture + CCR3_Val);
}
else
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC4);
/* LED6 toggling with frequency = 36.62 Hz */
STM_EVAL_LEDToggle(LED6);
capture = TIM_GetCapture4(TIM3);
TIM_SetCompare4(TIM3, capture + CCR4_Val);
}
}
/**
* @brief This function handles TIM2 global interrupt request.
* @param None
* @retval None
*/
void TIM2_IRQHandler(void)
{
if (TIM_GetITStatus(TIM2, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit(TIM2, TIM_IT_CC1);
/* Pin PC.06 toggling with frequency = 73.24 Hz */
GPIO_WriteBit(GPIOC, GPIO_Pin_6, (BitAction)(1 - GPIO_ReadOutputDataBit(GPIOC, GPIO_Pin_6)));
capture = TIM_GetCapture1(TIM2);
TIM_SetCompare1(TIM2, capture + CCR1_Val);
}
else if (TIM_GetITStatus(TIM2, TIM_IT_CC2) != RESET)
{
TIM_ClearITPendingBit(TIM2, TIM_IT_CC2);
/* Pin PC.07 toggling with frequency = 109.8 Hz */
GPIO_WriteBit(GPIOC, GPIO_Pin_7, (BitAction)(1 - GPIO_ReadOutputDataBit(GPIOC, GPIO_Pin_7)));
capture = TIM_GetCapture2(TIM2);
TIM_SetCompare2(TIM2, capture + CCR2_Val);
}
else if (TIM_GetITStatus(TIM2, TIM_IT_CC3) != RESET)
{
TIM_ClearITPendingBit(TIM2, TIM_IT_CC3);
/* Pin PC.08 toggling with frequency = 219.7 Hz */
GPIO_WriteBit(GPIOC, GPIO_Pin_8, (BitAction)(1 - GPIO_ReadOutputDataBit(GPIOC, GPIO_Pin_8)));
capture = TIM_GetCapture3(TIM2);
TIM_SetCompare3(TIM2, capture + CCR3_Val);
}
else
{
TIM_ClearITPendingBit(TIM2, TIM_IT_CC4);
/* Pin PC.09 toggling with frequency = 439.4 Hz */
GPIO_WriteBit(GPIOC, GPIO_Pin_9, (BitAction)(1 - GPIO_ReadOutputDataBit(GPIOC, GPIO_Pin_9)));
capture = TIM_GetCapture4(TIM2);
TIM_SetCompare4(TIM2, capture + CCR4_Val);
}
}
/**
* @brief This function handles TIM2 global interrupt request.
* @param None
* @retval None
*/
void TIM2_IRQHandler(void)
{
if (TIM_GetITStatus(TIM2, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit(TIM2, TIM_IT_CC1);
/* Pin PC.00 toggling with frequency = 390 Hz */
GPIO_WriteBit(GPIOC, GPIO_Pin_0, (BitAction)(1 - GPIO_ReadOutputDataBit(GPIOC, GPIO_Pin_0)));
capture = TIM_GetCapture1(TIM2);
TIM_SetCompare1(TIM2, capture + CCR1_Val);
}
else if (TIM_GetITStatus(TIM2, TIM_IT_CC2) != RESET)
{
TIM_ClearITPendingBit(TIM2, TIM_IT_CC2);
/* Pin PC.01 toggling with frequency = 585 Hz */
GPIO_WriteBit(GPIOC, GPIO_Pin_1, (BitAction)(1 - GPIO_ReadOutputDataBit(GPIOC, GPIO_Pin_1)));
capture = TIM_GetCapture2(TIM2);
TIM_SetCompare2(TIM2, capture + CCR2_Val);
}
else if (TIM_GetITStatus(TIM2, TIM_IT_CC3) != RESET)
{
TIM_ClearITPendingBit(TIM2, TIM_IT_CC3);
/* Pin PC.02 toggling with frequency = 1.17 KHz */
GPIO_WriteBit(GPIOC, GPIO_Pin_2, (BitAction)(1 - GPIO_ReadOutputDataBit(GPIOC, GPIO_Pin_2)));
capture = TIM_GetCapture3(TIM2);
TIM_SetCompare3(TIM2, capture + CCR3_Val);
}
else
{
TIM_ClearITPendingBit(TIM2, TIM_IT_CC4);
/* Pin PC.03 toggling with frequency = 2.34 KHz */
GPIO_WriteBit(GPIOC, GPIO_Pin_3, (BitAction)(1 - GPIO_ReadOutputDataBit(GPIOC, GPIO_Pin_3)));
capture = TIM_GetCapture4(TIM2);
TIM_SetCompare4(TIM2, capture + CCR4_Val);
}
}
uint16_t HAL_Servo_Read_Pulse_Width(uint16_t pin)
{
uint16_t SERVO_TIM_CCR = 0x0000;
if(PIN_MAP[pin].timer_ch == TIM_Channel_1)
{
SERVO_TIM_CCR = TIM_GetCapture1(PIN_MAP[pin].timer_peripheral);
}
else if(PIN_MAP[pin].timer_ch == TIM_Channel_2)
{
SERVO_TIM_CCR = TIM_GetCapture2(PIN_MAP[pin].timer_peripheral);
}
else if(PIN_MAP[pin].timer_ch == TIM_Channel_3)
{
SERVO_TIM_CCR = TIM_GetCapture3(PIN_MAP[pin].timer_peripheral);
}
else if(PIN_MAP[pin].timer_ch == TIM_Channel_4)
{
SERVO_TIM_CCR = TIM_GetCapture4(PIN_MAP[pin].timer_peripheral);
}
//pulseWidth = (SERVO_TIM_CCR * 1000000) / ((SERVO_TIM_ARR + 1) * SERVO_TIM_PWM_FREQ);
return SERVO_TIM_CCR;
}
void TIM2_IRQHandler(void)
{
uint32_t current[4];
TIM_ICInitTypeDef TIM_ICInitStructure;
TIM_ICStructInit(&TIM_ICInitStructure);
if (TIM_GetITStatus(TIM2, TIM_IT_CC1) == SET) {
/* Clear TIM1 Capture compare interrupt pending bit */
TIM_ClearITPendingBit(TIM2, TIM_IT_CC1);
if ( inc[INC6].status == RISING ) {
TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
/* Get the Input Capture value */
inc[INC6].prev_value = TIM_GetCapture1(TIM2);
inc[INC6].status = FALLING;
} else {
TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
//Get the Input Capture value
current[0] = TIM_GetCapture1(TIM2);
if (current[0] > inc[INC6].prev_value)
inc[INC6].curr_value = current[0] - inc[INC6].prev_value ;
else if (current[0] < inc[INC6].prev_value)
inc[INC6].curr_value = 0xFFFF - inc[INC6].prev_value + current[0] ;
inc[INC6].status = RISING;
}
TIM_ICInit(TIM2, &TIM_ICInitStructure);
}
if (TIM_GetITStatus(TIM2, TIM_IT_CC2) == SET) {
/* Clear TIM1 Capture compare interrupt pending bit */
TIM_ClearITPendingBit(TIM2, TIM_IT_CC2);
if (inc[INC5].status == RISING) {
TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
/* Get the Input Capture value */
inc[INC5].prev_value = TIM_GetCapture2(TIM2);
inc[INC5].status = FALLING;
} else {
TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
/* Get the Input Capture value */
current[1] = TIM_GetCapture2(TIM2);
if (current[1] > inc[INC5].prev_value)
inc[INC5].curr_value = current[1] - inc[INC5].prev_value ;
else if (current[1] < inc[INC5].prev_value)
inc[INC5].curr_value = 0xFFFF - inc[INC5].prev_value + current[1] ;
inc[INC5].status = RISING;
}
TIM_ICInit(TIM2, &TIM_ICInitStructure);
}
if (TIM_GetITStatus(TIM2, TIM_IT_CC3) == SET) {
/* Clear TIM1 Capture compare interrupt pending bit */
TIM_ClearITPendingBit(TIM2, TIM_IT_CC3);
if (inc[INC2].status == RISING) {
TIM_ICInitStructure.TIM_Channel = TIM_Channel_3;
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
/* Get the Input Capture value */
inc[INC2].prev_value = TIM_GetCapture3(TIM2);
inc[INC2].status = FALLING;
} else {
TIM_ICInitStructure.TIM_Channel = TIM_Channel_3;
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
/* Get the Input Capture value */
current[2] = TIM_GetCapture3(TIM2);
if (current[2] > inc[INC2].prev_value)
inc[INC2].curr_value= current[2] - inc[INC2].prev_value;
else if (current[2] < inc[INC2].prev_value)
inc[INC2].curr_value = 0xFFFF - inc[INC2].prev_value + current[2] ;
inc[INC2].status = RISING;
}
TIM_ICInit(TIM2, &TIM_ICInitStructure);
}
if (TIM_GetITStatus(TIM2, TIM_IT_CC4) == SET) {
/* Clear TIM1 Capture compare interrupt pending bit */
TIM_ClearITPendingBit(TIM2, TIM_IT_CC4);
if (inc[INC1].status == RISING) {
TIM_ICInitStructure.TIM_Channel = TIM_Channel_4;
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
/* Get the Input Capture value */
inc[INC1].prev_value = TIM_GetCapture4(TIM2);
inc[INC1].status = FALLING;
} else {
TIM_ICInitStructure.TIM_Channel = TIM_Channel_4;
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
/* Get the Input Capture value */
current[3] = TIM_GetCapture4(TIM2);
if (current[3] > inc[INC1].prev_value)
inc[INC1].curr_value = current[3] - inc[INC1].prev_value;
else if (current[3] < inc[INC1].prev_value)
inc[INC1].curr_value = 0xFFFF - inc[INC1].prev_value+ current[3] ;
inc[INC1].status = RISING;
}
TIM_ICInit(TIM2, &TIM_ICInitStructure);
}
}
/*********************************************************************************
*Function : void TIM3_Tone_Interrupt_Handler(void)
*Description : set duty cycle
*Input : none
*Output : none
*Return : none
*author : lz
*date : 6-December-2013
*Others :
**********************************************************************************/
void TIM3_Tone_Interrupt_Handler(void)
{
uint16_t capture;
if (TIM_GetITStatus(TIM3, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC1 );
capture = TIM_GetCapture1(TIM3);
TIM_SetCompare1(TIM3, capture + PIN_MAP[36].timer_ccr);
if(PIN_MAP[36].user_property != -1)
{
if (PIN_MAP[36].user_property > 0)
{
PIN_MAP[36].user_property -= 1;
}
else
{
noTone(36);
}
}
}
if (TIM_GetITStatus(TIM3, TIM_IT_CC2) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
capture = TIM_GetCapture2(TIM3);
TIM_SetCompare2(TIM3, capture + PIN_MAP[37].timer_ccr);
if(PIN_MAP[37].user_property != -1)
{
if (PIN_MAP[37].user_property > 0)
{
PIN_MAP[37].user_property -= 1;
}
else
{
noTone(37);
}
}
}
if (TIM_GetITStatus(TIM3, TIM_IT_CC3) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC3);
capture = TIM_GetCapture3(TIM3);
TIM_SetCompare3(TIM3, capture + PIN_MAP[38].timer_ccr);
if(PIN_MAP[38].user_property != -1)
{
if (PIN_MAP[38].user_property > 0)
{
PIN_MAP[38].user_property -= 1;
}
else
{
noTone(38);
}
}
}
if (TIM_GetITStatus(TIM3, TIM_IT_CC4) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC4);
capture = TIM_GetCapture4(TIM3);
TIM_SetCompare4(TIM3, capture + PIN_MAP[39].timer_ccr);
if(PIN_MAP[39].user_property != -1)
{
if (PIN_MAP[39].user_property > 0)
{
PIN_MAP[39].user_property -= 1;
}
else
{
noTone(39);
}
}
}
}
/*********************************************************************************
*Function : void TIM2_Tone_Interrupt_Handler(void)
*Description : set duty cycle
*Input : none
*Output : none
*Return : none
*author : lz
*date : 6-December-2013
*Others :
**********************************************************************************/
void TIM2_Tone_Interrupt_Handler(void)
{
uint16_t capture;
if (TIM_GetITStatus(TIM2, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit(TIM2, TIM_IT_CC1 );
capture = TIM_GetCapture1(TIM2);
TIM_SetCompare1(TIM2, capture + PIN_MAP[30].timer_ccr);
if(PIN_MAP[30].user_property != -1)
{
if (PIN_MAP[30].user_property > 0)
{
PIN_MAP[30].user_property -= 1;
}
else
{
noTone(30);
}
}
}
if (TIM_GetITStatus(TIM2, TIM_IT_CC2) != RESET)
{
TIM_ClearITPendingBit(TIM2, TIM_IT_CC2);
capture = TIM_GetCapture2(TIM2);
TIM_SetCompare2(TIM2, capture + PIN_MAP[31].timer_ccr);
if(PIN_MAP[31].user_property != -1)
{
if (PIN_MAP[31].user_property > 0)
{
PIN_MAP[31].user_property -= 1;
}
else
{
noTone(31);
}
}
}
if (TIM_GetITStatus(TIM2, TIM_IT_CC3) != RESET)
{
TIM_ClearITPendingBit(TIM2, TIM_IT_CC3);
capture = TIM_GetCapture3(TIM2);
TIM_SetCompare3(TIM2, capture + PIN_MAP[33].timer_ccr);
if(PIN_MAP[33].user_property != -1)
{
if (PIN_MAP[33].user_property > 0)
{
PIN_MAP[33].user_property -= 1;
}
else
{
noTone(33);
}
}
}
if (TIM_GetITStatus(TIM2, TIM_IT_CC4) != RESET)
{
TIM_ClearITPendingBit(TIM2, TIM_IT_CC4);
capture = TIM_GetCapture4(TIM2);
TIM_SetCompare4(TIM2, capture + PIN_MAP[32].timer_ccr);
if(PIN_MAP[32].user_property != -1)
{
if (PIN_MAP[32].user_property > 0)
{
PIN_MAP[32].user_property -= 1;
}
else
{
noTone(32);
}
}
}
}