(一)原理图:
PX4飞控的PPM输入捕获由协处理器完成,接在A8引脚,对应Timer1的通道1。
(二)PPM协议:
PPM的每一帧数据间隔为20ms,用两个上升沿之间的时间间隔表示一个通道的值,如图所示是一个6通道信号。在每一帧的结束会有较长时间的持续低电平,两次上升沿时间间隔要大于5ms,可以用于辨别下一帧数据的到来。
(三)相关代码
定时器1配置:
/* timer1 config */
void Bsp_Timer1_Config(void)
{
TIM_ICInitTypeDef TIM_ICInitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
NVIC_InitTypeDef NVIC_InitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
/* Clock Enable */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
/* Enable global Interrupt */
NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
/* TIM1 channel 1 pin (PA.08) configuration */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
/* Time Base configuration, 1us every tick */
TIM_TimeBaseStructure.TIM_Prescaler = SystemCoreClock / 1000000;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
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_ICInit(TIM1, &TIM_ICInitStructure);
/* TIM enable counter */
TIM_Cmd(TIM1, ENABLE);
/* Enable the CC1 Interrupt Request */
TIM_ITConfig(TIM1, TIM_IT_CC1, ENABLE);
}
PPM处理程序:
#define GET_GAP(x, y) (x > y ? (x - y) : (0xFFFF - y + x))
ST_PPM machine_param;
static uint8_t PPM_Recv = 0; /* ppm received flag */
static uint8_t PPM_Send_Freq = 0; /* freq of sending ppm to fmu */
/* status turn to the next one */
static void ppm_next_status(ST_PPM *param)
{
static uint8_t max_ch = STA_EDGE_9 + 1;
param->status = (param->status + 1) % max_ch;
}
/* reset the ppm machine state */
void reset_machine_state(ST_PPM *param)
{
param->status = STA_EDGE_1;
param->bad_frame = 0;
param->last_ic = 0;
}
/* the function will be called back by the timer1 interrupt */
void App_PPM_Status_Machine(uint16_t ic)
{
static uint16_t temp_val[8];
uint16_t gap;
gap = GET_GAP(ic, machine_param.last_ic);
/* each ppm frame has a 5.5ms start puls, each tick is 1us */
if (gap > 5000)
{
reset_machine_state(&machine_param);
}
else if (gap < 800 || gap > 2200)
{
/* bad signal, drop frame */
machine_param.bad_frame = 1;
}
if (machine_param.status == STA_EDGE_1)
{
/* do nothing */
}
else if (machine_param.status == STA_EDGE_9)
{
uint8_t i;
temp_val[STA_EDGE_9 - 1] = gap;
/* we know next we have a 5.5ms gap, so we can do some time-cost task here */
/* If it's bad frame, we drop it */
if (!machine_param.bad_frame)
{
for (i = 0; i < 8; i++)
{
machine_param.ppm_val[i] = temp_val[i];
}
PPM_Recv = 1;
}
}
else
{
temp_val[machine_param.status - 1] = gap;
}
machine_param.last_ic = ic;
ppm_next_status(&machine_param);
}
/* received a wholed ppm frame */
uint8_t App_PPM_Ready(void)
{
static uint32_t prev_time = 0;
uint32_t time = Sys_Tick_Time;
if (!PPM_Send_Freq)
{
return 0;
}
if (time - prev_time > 1000 / PPM_Send_Freq && PPM_Recv)
{
prev_time = time;
return 1;
}
return 0;
}
/* copy the ppm frame to the buff when the ppm frame will be used */
void App_Get_PPM_Value(uint16_t ppm[8])
{
memcpy(ppm, machine_param.ppm_val, 8);
PPM_Recv = 0;
}
/* set the frequency of sending ppm buff to fmu */
uint8_t App_Set_PPM_Send_Freq(uint8_t freq)
{
if (freq <= 50)
{
PPM_Send_Freq = freq;
return 1;
}
else
{
return 0;
}
} (x > y ? (x - y) : (0xFFFF - y + x))
ST_PPM machine_param;
static uint8_t PPM_Recv = 0; /* ppm received flag */
static uint8_t PPM_Send_Freq = 0; /* freq of sending ppm to fmu */
/* status turn to the next one */
static void ppm_next_status(ST_PPM *param)
{
static uint8_t max_ch = STA_EDGE_9 + 1;
param->status = (param->status + 1) % max_ch;
}
/* reset the ppm machine state */
void reset_machine_state(ST_PPM *param)
{
param->status = STA_EDGE_1;
param->bad_frame = 0;
param->last_ic = 0;
}
/* the function will be called back by the timer1 interrupt */
void App_PPM_Status_Machine(uint16_t ic)
{
static uint16_t temp_val[8];
uint16_t gap;
gap = GET_GAP(ic, machine_param.last_ic);
/* each ppm frame has a 5.5ms start puls, each tick is 1us */
if (gap > 5000)
{
reset_machine_state(&machine_param);
}
else if (gap < 800 || gap > 2200)
{
/* bad signal, drop frame */
machine_param.bad_frame = 1;
}
if (machine_param.status == STA_EDGE_1)
{
/* do nothing */
}
else if (machine_param.status == STA_EDGE_9)
{
uint8_t i;
temp_val[STA_EDGE_9 - 1] = gap;
/* we know next we have a 5.5ms gap, so we can do some time-cost task here */
/* If it's bad frame, we drop it */
if (!machine_param.bad_frame)
{
for (i = 0; i < 8; i++)
{
machine_param.ppm_val[i] = temp_val[i];
}
PPM_Recv = 1;
}
}
else
{
temp_val[machine_param.status - 1] = gap;
}
machine_param.last_ic = ic;
ppm_next_status(&machine_param);
}
/* received a wholed ppm frame */
uint8_t App_PPM_Ready(void)
{
static uint32_t prev_time = 0;
uint32_t time = Sys_Tick_Time;
if (!PPM_Send_Freq)
{
return 0;
}
if (time - prev_time > 1000 / PPM_Send_Freq && PPM_Recv)
{
prev_time = time;
return 1;
}
return 0;
}
/* copy the ppm frame to the buff when the ppm frame will be used */
void App_Get_PPM_Value(uint16_t ppm[8])
{
memcpy(ppm, machine_param.ppm_val, 8);
PPM_Recv = 0;
}
/* set the frequency of sending ppm buff to fmu */
uint8_t App_Set_PPM_Send_Freq(uint8_t freq)
{
if (freq <= 50)
{
PPM_Send_Freq = freq;
return 1;
}
else
{
return 0;
}
}
定时器1中断程序:
/* timer1 interrupt */
void TIM1_CC_IRQHandler(void)
{
uint16_t ic = 0;
if (TIM_GetITStatus(TIM1, TIM_IT_CC1) == SET)
{
/* Clear TIM1 Capture compare interrupt pending bit */
TIM_ClearITPendingBit(TIM1, TIM_IT_CC1);
ic = TIM_GetCapture1(TIM1);
App_PPM_Status_Machine(ic);
}
}
版权声明:本文为arris1992原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。