完善资料让更多小伙伴认识你,还能领取20积分哦, 立即完善>
一、简介
航模接收机的输出信号,乍看起来是PWM波,但严谨的看法是高电平脉冲的宽度。不同厂家接收机设计的信号周期不一样相同,甚至同一个接收机使用时信号周期也不一定相同。所谓接收机类型中的PWM型、SBUS型、PPM型,本质上只是对脉冲宽度按不同的方式编码 通常脉宽变化范围是1000us~2000us,一般航模遥控器中: (1)方向、油门之类的双向控制,中间值是1500us,两个方向的极值分别是1000us和2000us; (2)模式切换按钮,分别在1000us和2000us之间切换。 (3)无刷电调没有方向,在1000us时油门为0,2000us时油门最大。 官方介绍中,匿名飞控支持PPM,SUBS和PWM三种遥控信号输入格式,默认为PPM,但实际上,TM4C主控版本的飞控在程序上只支持PPM,SUBS两种模式(我2019/6/27拿到的版本是这样的),stm32版本的飞控支持PWM。 在匿名上位机“飞控设置”功能页面,然后打开“参数设置”,可以修改接收机模式 这里实际上是用USB串口和TM4C通讯,相关设置会存贮在板载EEPROM中,掉电后也可以保留 二、PWM信号模式 1、介绍 PWM型接收机,会把接收到的每一个通道的脉宽值,都转换为PWM波的形式,用一个针脚单独输出,基本上所有接收机都支持这种输出形式,是使用范围最广的。 解码时,需要每个通道单独用一个IO口进行输入捕获,记录其脉宽。因此这种形式接收机接线复杂,资源占用多 2、硬件连接 匿名飞控的板子上其实做了PWM接收机的信号接口,如下,这个框为无用的接口就是PWM接收机信号输入,这是一个8pin插头,对应8个通道 查看一下原理图 这一看就是32版本的原理图啊,TM4C一个定时器只能做两路输入捕获,而且命名也不是这样的。。。推测TM4C版飞控的这个接口应该没有内部连接,但原理图还是给的32版本的 三、PPM信号模式 1、PPM介绍 PPM信号是把多路PWM波压缩到一路中的编码方式,通常20ms为一个周期,用一系列高电平脉冲之间的间隔时间表示每一路PWM波的脉宽,见下图 注意: 各个通道的高电平信号是一个挨着一个的,而不是每个通道固定分配2ms的时间 两个相邻通道间不一定是紧挨着的(表示前一个通道结束的脉冲,和表示下一个通道开始的脉冲,不一定是完全重合的),其间间隔时间也不一定 单路信号最长是2000us,周期20ms,所以理论上可以容纳10路。而由于需要进行同步,实际上遥控器最多只能容纳9路信号,每个周期的最后有一段较长时间的低电平(至少2ms),单片机通过它判断一个PPM周期结束 2、硬件连接 可以看到直接连接到PC7,一个IO口做输入捕获即可 3、代码分析 (1)初始化部分 在程序最开始,调用Drv_BspInit();对整机的传感器和外设等进行初始化,其中调用Remote_Control_Init函数进行遥控接收配置,程序如下 //初始化遥控接受 static u8 RC_IN_MODE; void Remote_Control_Init() { RC_IN_MODE = Ano_Parame.set.pwmInMode; if(RC_IN_MODE == SBUS) { Drv_SbusInit(); //SBUS-》UART3 } else { Drv_PpmInit(); //PPM-》timer1B输入捕获 // PWM_IN_Init(RC_IN_MODE); } } Ano_Parame.set结构体中存储了所有将被设置的参数,包括pid参数、起飞降落速度等,其中pwmInMode为接收机模式 Ano_Parame是一个共用体,set和一个2048字节的uint8_t数组byte公用空间,上电后程序先从EEPROM中读取2048字节数据到共用体Ano_Parame的byte中,从而对共用内存空间的set结构体赋值。可见遥控模式设定pwmInMode是保存在ROM中的,设置一次后就一直保持 Ano_Parame.set.pwmInMode有三种取值,如下。根据Remote_Control_Init函数中的if-else语句,可见TM4C版飞控中PWM模式其实也是PPM,二者的处理是相同的 enum pwminmode_e { PWM = 0, PPM, SBUS, }; 查看PPM接收的外设初始化函数Drv_PpmInit(); /******************************************************************************************************************* *函 数 名: Drv_PpmInit *功能说明: 初始化ppm解码,配置PC7为输入捕获引脚,定时器5B为捕获上升沿,边沿加计时模式,计数范围0-0xffffff,频率80M *形 参: 无 *返 回 值: 无 *******************************************************************************************************************/ void Drv_PpmInit(void) { /*启动GPIOC*/ ROM_SysCtlPeripheralEnable(PPM_SYSCTL); /*启动wide timer 1 (32/64 bit)*/ ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_WTIMER1); /*GPIOC配置为定时器捕获模式*/ ROM_GPIOPinTypeTimer(PPM_PORTS, PPM_PIN); ROM_GPIOPinConfigure(PPM_FUNCTION); /*配置定时器5B为捕获上升沿,边沿加计时模式*/ ROM_TimerConfigure( WTIMER1_BASE ,TIMER_CFG_SPLIT_PAIR | TIMER_CFG_B_CAP_TIME_UP ); ROM_TimerControlEvent(WTIMER1_BASE,TIMER_B,TIMER_EVENT_POS_EDGE); /*计时范围0-0xffffff,计数频率同系统频率80M*/ ROM_TimerLoadSet( WTIMER1_BASE , TIMER_B , 0xffff ); //注意这个函数,这个8位的预分频寄存器,只有单次或周期的定时是真正的预分频寄存器,这里做输入捕获,其实是把这8位当成WTIMER1B的高8位了,从而扩展了捕获时的计数范围到24位 ROM_TimerPrescaleSet( WTIMER1_BASE , TIMER_B , 0xff ); /*开启定时器中断*/ TimerIntRegister(WTIMER1_BASE, TIMER_B , PPM_Decode); ROM_IntPrioritySet( INT_WTIMER1B , USER_INT6); ROM_TimerIntEnable( WTIMER1_BASE , TIMER_CAPB_EVENT); ROM_TimerEnable( WTIMER1_BASE, TIMER_B ); ROM_IntEnable( INT_WTIMER1B ); } 可以看到,这里把PC7配置为输入捕获,捕获模式为上升沿加计时,计时频率为80MHz,计数范围0-0xffffff, 注意中断的配置,当发生捕获事件(检测到上升沿)时,执行中断服务函数PPM_Decode (2)解码 /******************************************************************************************************* *函 数 名: PPM_Decode *功能说明: PPM解码,把每个PWM脉宽分离出来 *形 参: 无 *返 回 值: 无 *关于 PPM:PPM信号把多路PWM信号调制到一路通道上,标准刷新率是50Hz(周期20ms),每路脉宽变化范围是1000us~2000us。 1. 方向、油门之类的双向控制,中间值是1500us,两个方向的极值分别是1000us和2000us; 2. 模式切换按钮,分别在1000us和2000us之间切换。 3. 无刷电调没有方向,在1000us时油门为0,2000us时油门最大 ******************************************************************************************************/ static void PPM_Decode(void) { static uint32_t PeriodVal1,PeriodVal2 = 0; static uint32_t PulseHigh; /* 清除中断标志 */ ROM_TimerIntClear( WTIMER1_BASE , TIMER_CAPB_EVENT ); /* 获取捕获值 */ PeriodVal1 = ROM_TimerValueGet( WTIMER1_BASE , TIMER_B ); //PeriodVal1是高电平到来时的计数值 /* 计算高电平持续时间,1对应1us,注意过零点处理 */ if( PeriodVal1 》 PeriodVal2 ) PulseHigh = (PeriodVal1 - PeriodVal2) /80; else PulseHigh = (PeriodVal1 - PeriodVal2 + 0xffffff)/80; PeriodVal2 = PeriodVal1; PPM_Cal(PulseHigh); } /********************************************************************************************************** *函 数 名: PPM_Cal *功能说明: PPM通道数据计算 *形 参: 无 *返 回 值: 无 **********************************************************************************************************/ static void PPM_Cal(uint32_t PulseHigh) { static uint8_t Chan = 0; //通道标记 /* 脉宽高于一定值(5000us)说明一帧数据已经结束 */ if(PulseHigh 》 5000) Chan = 0; /* 脉冲高度正常 */ else { if (PulseHigh 》 PULSE_MIN && PulseHigh 《 PULSE_MAX) { if(Chan 《 16) { /* 通道读取正常,给通道Chan喂狗 */ ch_watch_dog_feed(Chan); RC_PPM.Captures[Chan++] = PulseHigh; } } } } PPM_Decode()是一个中断服务函数,当PPM输入引脚捕获到高电平脉冲上升沿时触发,在这里检查计数器计数值,并和上次进中断的计数值做差,从而获得一个通道对应的脉宽(单位us),注意一下过零点处理就好 前面说过,PPM信号周期最后有一个长时间的低电平,而且两个相邻通道间不一定是紧挨着的。因此PPM_Decode()直接获得的脉宽不一定是通道数据,需要调用PPM_Cal函数进一步确认。 PPM_Cal中首先判断脉宽是否超过5000us(标志一个PPM周期结束),超过了就准备从0通道开始重新记录各通道脉宽;否则,进一步判断脉宽是否在合理范围内(PULSE_MIN~PULSE_MAX),如果都没问题,说明读取到的确实是一个通道数据,给通道看门狗喂狗ch_watch_dog_feed(Chan),并把脉宽记录到RC_PPM.Captures数组中,等待进一步处理。 关于通道看门狗的内容稍后再讲 四、SBUS信号模式 1、SBUS介绍 简单说,SBUS信号就是一种特殊的UART信号,相比于普通UART信号,SBUS信号经过硬件取反后可以直接用UART控制器处理,注意必须要进行硬件取反,软件取反不行 接受串口配置:波特率100k,8位数据位,偶校验,2位停止位,无控流(硬件流控制),25个字节、 SBUS规定了一个数据传输协议,格式为 [startbyte] [data1][data2]…[data22][flags][endbyte](帧头、帧尾、标志+22个数据字节,一帧共25字节) 22个数据字节对应16个通道(ch1-ch16),每个通道11bit(22x8=16x11=176),数据范围在0-2047之间,基本上是1102~1927,中值为1500; 2、硬件连接 3、代码分析 (1)初始化部分 /******************************************************************************************************************* *函 数 名: Drv_SbusInit *功能说明: 初始化***us解码,配置PC6复用UART3功能,UART频率为系统频率80M,波特率100k,8位数据位,2位停止位,偶校验,FIFO深度1/8(16*1/8=2) *形 参: 无 *返 回 值: 无 *******************************************************************************************************************/ void Drv_SbusInit(void) { ROM_SysCtlPeripheralEnable(SBUS_SYSCTL); ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_UART3); /*GPIO的UART模式配置*/ ROM_GPIOPinConfigure(UART3_RX); ROM_GPIOPinTypeUART( UART3_PORT ,UART3_PIN_RX ); /*配置串口的波特率和时钟源*/ ROM_UARTConfigSetExpClk( SBUS_UART ,SysCtlClockGet(), SBUS_BAUDRATE ,UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_TWO | UART_CONFIG_PAR_EVEN ); /*FIFO设置*/ ROM_UARTFIFOLevelSet( SBUS_UART , UART_FIFO_TX1_8 , UART_FIFO_RX1_8 ); ROM_UARTFIFOEnable(SBUS_UART); /*使能串口*/ ROM_UARTEnable( SBUS_UART ); /*串口中断配置与使能*/ UARTIntRegister( SBUS_UART , Sbus_IRQHandler ); ROM_IntPrioritySet( INT_UART3 , USER_INT6 ); ROM_UARTIntEnable( SBUS_UART , UART_INT_RX | UART_INT_OE ); } 配置PC6复用UART3功能,UART频率为系统频率80M,波特率100k,8位数据位,2位停止位,偶校验,FIFO深度1/8(16*1/8=2) 打开了串口接收中断 & 溢出错误中断 (2)解码 static void Sbus_IRQHandler(void) { uint8_t com_data; /*获取中断标志 原始中断状态 屏蔽中断标志*/ uint32_t flag = ROM_UARTIntStatus(SBUS_UART,1); /*清除中断标志*/ ROM_UARTIntClear(SBUS_UART,flag); ROM_UARTRxErrorClear( SBUS_UART ); /*判断FIFO是否还有数据*/ while(ROM_UARTCharsAvail(SBUS_UART)) { com_data=UART3-》DR; Sbus_Decode(com_data); } } u16 Rc_Sbus_In[16]; //SBUS接受的数据 u8 ***us_flag; /* ***us flags的结构如下所示: flags: bit7 = ch17 = digital channel (0x80) bit6 = ch18 = digital channel (0x40) bit5 = Frame lost, equivalent red LED on receiver (0x20) bit4 = failsafe activated (0x10) b: 0001 0000 bit3 = n/a bit2 = n/a bit1 = n/a bit0 = n/a */ static void Sbus_Decode(uint8_t data) { static uint8_t i; static uint8_t DataCnt = 0; static uint8_t SUBS_RawData[25]; /*接收数据*/ SUBS_RawData[DataCnt++]=data; /*每帧数据长度为25*/ if(DataCnt 》= 25) { /*判断帧头帧尾是否正确 只要有一个不正确就退出函数*/ if(SUBS_RawData[0] == 0x0F && SUBS_RawData[24] == 0) { DataCnt = 0; Rc_Sbus_In[0] = (s16)(SUBS_RawData[2] & 0x07) 《《 8 | SUBS_RawData[1]; //data[2]低三位+data[1]八位 Rc_Sbus_In[1] = (s16)(SUBS_RawData[3] & 0x3f) 《《 5 | (SUBS_RawData[2] 》》 3); //data[3]低六位+data[2]高五位 Rc_Sbus_In[2] = (s16)(SUBS_RawData[5] & 0x01) 《《 10 | ((s16)SUBS_RawData[4] 《《 2) | (SUBS_RawData[3] 》》 6); Rc_Sbus_In[3] = (s16)(SUBS_RawData[6] & 0x0F) 《《 7 | (SUBS_RawData[5] 》》 1); Rc_Sbus_In[4] = (s16)(SUBS_RawData[7] & 0x7F) 《《 4 | (SUBS_RawData[6] 》》 4); Rc_Sbus_In[5] = (s16)(SUBS_RawData[9] & 0x03) 《《 9 | ((s16)SUBS_RawData[8] 《《 1) | (SUBS_RawData[7] 》》 7); Rc_Sbus_In[6] = (s16)(SUBS_RawData[10] & 0x1F) 《《 6 | (SUBS_RawData[9] 》》 2); Rc_Sbus_In[7] = (s16)SUBS_RawData[11] 《《 3 | (SUBS_RawData[10] 》》 5); Rc_Sbus_In[8] = (s16)(SUBS_RawData[13] & 0x07) 《《 8 | SUBS_RawData[12]; Rc_Sbus_In[9] = (s16)(SUBS_RawData[14] & 0x3f) 《《 5 | (SUBS_RawData[13] 》》 3); Rc_Sbus_In[10] = (s16)(SUBS_RawData[16] & 0x01) 《《 10 | ((s16)SUBS_RawData[15] 《《 2) | (SUBS_RawData[14] 》》 6); Rc_Sbus_In[11] = (s16)(SUBS_RawData[17] & 0x0F) 《《 7 | (SUBS_RawData[16] 》》 1); Rc_Sbus_In[12] = (s16)(SUBS_RawData[18] & 0x7F) 《《 4 | (SUBS_RawData[17] 》》 4); Rc_Sbus_In[13] = (s16)(SUBS_RawData[20] & 0x03) 《《 9 | ((s16)SUBS_RawData[19] 《《 1) | (SUBS_RawData[18] 》》 7); Rc_Sbus_In[14] = (s16)(SUBS_RawData[21] & 0x1F) 《《 6 | (SUBS_RawData[20] 》》 2); Rc_Sbus_In[15] = (s16)SUBS_RawData[22] 《《 3 | (SUBS_RawData[21] 》》 5); //标志位,低四位保留,位7:6表示两个开关通道(通道17和18) //位5表示帧丢失,接收机红色LED亮起,我的理解是,如果这一位为1,表示这一帧信号出问题了,接收机红色LED亮起。 //位4表示故障保护激活,意思应该是说如果这一位为1,激活接受方故障保护(失控标记) ***us_flag = SUBS_RawData[23]; /*一帧数据解析完成*/ //user if(***us_flag & 0x10) { //如果有数据且能接收到有失控标记,则不处理,转嫁成无数据失控。 } else { //否则有数据就喂狗 for(u8 i = 0;i 《 8;i++)//原RC接收程序只设计了8个通道 { ch_watch_dog_feed(i); } } } /*帧头或帧尾出错,数据整体前移1位,下一位数据将加入在数组最后再次凑足一帧(25byte)*/ else { for( i=0; i《24;i++) SUBS_RawData[i] = SUBS_RawData[i+1]; DataCnt = 24; } } } UART3的FIFO深度配置为2,接收数据时稍微缓冲了一下,然后进入中断服务函数Sbus_IRQHandler 在中断函数中,取出收到的每一个字节,调用Sbus_Decode进行解码。在Sbus_Decode中,先把数据填入SUBS_RawData数组,凑足一帧长度后才能尝试解码。 解码时,先判断帧头帧尾,如果不匹配,就把数据整体前移,在末尾添加新数据,直到匹配为止。匹配后就按照SBUS协议进行数据拼接,把数据暂存在Rc_Sbus_In中等待进一步处理,并且给通道看门狗喂狗ch_watch_dog_feed(Chan) = |
|
|
|
关于通道看门狗的内容稍后再讲
*小结 到此为止吗,关于PPM和SBUS的接受解码已经讲完了,得到的摇杆数据分别为RC_PPM.Captures和Rc_Sbus_In 在带入pid控制前,还要进行两个步骤,一是把取值范围不同的两组数据归一化到同一个范围内,这样在修改接收机模式时不会影响其他参数的选择。二是提高数据的可靠性,毕竟是多旋翼飞行器,一但使用了错误的遥控数据,将会发生灾难性的炸鸡,这是不可接受的,匿名针对这个问题的解决方案是利用“通道看门狗”。 五、通道看门狗 所谓通道看门狗,其实和通常指的看门狗功能差不多,就是检查遥控器通道数据读取是否正常,如果数据有问题,就拒绝,从而保证错误的数据不会被带入pid控制 这个通道看门狗并不是真正的看门狗,不需要使用看门狗定时器,也不能进行系统复位,只是因为二者功能都是守卫程序正常运行,所以这样称呼而已 1、遥控数据读取和处理任务 //遥控器数据读取和处理 void RC_duty_task(u8 dT_ms) //建议2ms调用一次 { if(flag.start_ok) { /获得通道数据 // if(RC_IN_MODE == PWM) // { // for(u8 i=0;i《CH_NUM;i++) // { // if(chn_en_bit & (1《《i))//(Rc_Pwm_In[i]!=0)//该通道有值,==0说明该通道未插线(PWM) // { // CH_N[i] = 1.25f *((s16)Rc_Pwm_In[i] - 1500); //1100 -- 1900us,处理成大约+-500摇杆量 // } // else // { // CH_N[i] = 0; // } // CH_N[i] = LIMIT(CH_N[i],-500,500);//限制到+—500 // } // } // else if(RC_IN_MODE == PPM) if(RC_IN_MODE == PPM || RC_IN_MODE == PWM) { for(u8 i=0;i《CH_NUM;i++) { if(chn_en_bit & (1《《i))//(Rc_Ppm_In[i]!=0)//该通道有值 { CH_N[i] = ((s16)RC_PPM.Captures[i] - 1500); //1000 -- 2000us,处理成大约+-500摇杆量 } else { CH_N[i] = 0; } CH_N[i] = LIMIT(CH_N[i],-500,500);//限制到+—500 } } else//***us { for(u8 i=0;i《CH_NUM;i++) { if(chn_en_bit & (1《《i))//该通道有值 { CH_N[i] = 0.65f *((s16)Rc_Sbus_In[i] - 1024); //248 --1024 --1800,处理成大约+-500摇杆量 } else { CH_N[i] = 0; } CH_N[i] = LIMIT(CH_N[i],-500,500);//限制到+—500 } } /// //解锁监测 unlock(dT_ms); //摇杆触发功能监测 stick_function(dT_ms); //通道看门狗 ch_watch_dog(dT_ms); //失控保护检查 fail_safe_check(dT_ms);//3ms } } 这里是所有遥控解码函数的入口,同时也进行归一化处理,RC_PPM.Captures和Rc_Sbus_In都被限制到-500~500之间,处理后的摇杆值存入CH_N数组中,这是将被真正带人pid使用的参数值 可以看到PWM模式和PPM模式实际上是一致的 RC_duty_task()是一个轮询线程,利用系统嘀嗒定时器轮询,每11ms执行一次,参数dT_ms即为此任务执行周期。这里和任务调度相关的以后再说 chn_en_bit是一个bit-pack格式的字节,每位标志一个通道的值是否正常(是否有值) 注意这里调用了看门狗函数ch_watch_dog(dT_ms),参数是遥控解码任务周期时长 2、通道看门狗 static u16 cwd_cnt[10] ; //通道看门狗计时 u8 chn_en_bit = 0; //标志哪些通道正常 //给通道看门狗喂狗 void ch_watch_dog_feed(u8 ch_n) { ch_n = LIMIT(ch_n,0,7); cwd_cnt[ch_n] = 0; } //检查通道看门狗,dT_ms是调用时间间隔(调用周期,这里为11ms) static void ch_watch_dog(u8 dT_ms)//如果是PPM/SBUS模式,也只检测前8通道 { for(u8 i = 0;i《8;i++) { //看门狗计数小于500(数据更新周期小于500ms),认为通道接收正常,计时增加,给chn_en_bit中对应位标记正常 if(cwd_cnt[i]《500) { cwd_cnt[i] += dT_ms; chn_en_bit |= 0x01《《i; } //否则认定通道不正常,chn_en_bit中对应位标记0 else { chn_en_bit &= ~(0x01《《i); // Rc_Pwm_In[i] = 0; //把捕获值复位 // Rc_Ppm_In[i] = 0; // Rc_Sbus_In[i] = 0; } } } 程序注释已经写得比较清楚了,这个看门狗的原理,就是每次调用解码任务时把周期时间dT_ms加到每个通道对应的看门狗计时器cwd_cnt中,代表此通道的值多久没更新了(注意在解码部分,每个通道解码成功后都会调用ch_watch_dog_feed喂狗,清空对应通道的看门狗计时器)。如果未更新时间小于500ms,认为值没问题,给chn_en_bit对应位置1;否则认为此通道无值,给chn_en_bit对应位置0,。 以上是我目前的理解,可能有误,欢迎交流 |
|
|
|
只有小组成员才能发言,加入小组>>
调试STM32H750的FMC总线读写PSRAM遇到的问题求解?
1752 浏览 1 评论
X-NUCLEO-IHM08M1板文档中输出电流为15Arms,15Arms是怎么得出来的呢?
1611 浏览 1 评论
1052 浏览 2 评论
STM32F030F4 HSI时钟温度测试过不去是怎么回事?
721 浏览 2 评论
ST25R3916能否对ISO15693的标签芯片进行分区域写密码?
1666 浏览 2 评论
1924浏览 9评论
STM32仿真器是选择ST-LINK还是选择J-LINK?各有什么优势啊?
711浏览 4评论
STM32F0_TIM2输出pwm2后OLED变暗或者系统重启是怎么回事?
560浏览 3评论
583浏览 3评论
stm32cubemx生成mdk-arm v4项目文件无法打开是什么原因导致的?
544浏览 3评论
小黑屋| 手机版| Archiver| 电子发烧友 ( 湘ICP备2023018690号 )
GMT+8, 2024-12-18 12:25 , Processed in 0.869499 second(s), Total 80, Slave 63 queries .
Powered by 电子发烧友网
© 2015 bbs.elecfans.com
关注我们的微信
下载发烧友APP
电子发烧友观察
版权所有 © 湖南华秋数字科技有限公司
电子发烧友 (电路图) 湘公网安备 43011202000918 号 电信与信息服务业务经营许可证:合字B2-20210191 工商网监 湘ICP备2023018690号