详细资料见Q群:531905626
对该项目感兴趣的同志,想要一起深入开发拓展可以联系本人邮箱:2560198277@qq.com。限于本人精力,在PCB设计、3D建模、嵌入式开发方面都有较大的需求。
书籍总目录:http://t.csdnimg.cn/YDe8m
3.4 注意!核心在这儿——航模遥控器
3.4.1 航模遥控器简介
入门航模的话六通道的航模遥控器基本就够了比如六通道MC6C迈克遥控器,但进行嵌入式无人机的开发最好使用八通道的遥控器,本作品使用乐迪生产的蓝牙版T8FB八通道遥控器以及配套的R8FM接收机。
图3.4.1.1使用到的T8FB遥控器系统
(1)遥控器与接收机参数
遥控器:
项目: | 参数: |
---|---|
通道数 | 8 通道 |
遥控距离 | 空中 2000 米,地面 500 米(实际操控距离与环境有关) |
供电电压 | 4.8~18V |
供电方式 | 航模锂电池或普通五号电池 |
支持接收机 | R8EF, R8XM, R8SM, R8FM, R8F, R8FG, R7FG, R6FG, R6F, R4FGM, R4F(参考:https://www.radiolink.com/receivers) |
帧传输周期(通道值刷新频率?) | 15ms/每帧,约66Hz |
接收机:
项目: | 参数: |
---|---|
尺寸: | 30*17.5mm |
重量: | 2.5g |
通道: | 8通道 |
信号: | SBUS&PPM |
遥控距离: | 空中 2000 米,地面 500 米(实际操控距离与环境有关) |
工作电压: | 3-6V |
工作电流: | 38-45mA |
(2)遥控器功能
图3.4.1.2遥控器硬件
如图3.4.1.2所示,T8FB遥控器拥有八通道输出,包括四个操纵杆通道,一个二段拨码开关通道,一个三段拨码开关通道,两个旋扭开关通道。同时操纵杆的每个通道都有与之对应的微调开关与通道取反开关。其中取反开关上字母对应的意思如下:
图3.4.1.3
上述功能在之后的飞控程序中都可以进行验证,而除了上述基本功能外,该遥控器还具有如下独特的功能:
(1)可拆卸的油门操纵杆回中组件:
图3.4.1.4
这个组件可以说十分有用,不装该组件时,遥控器的油门操纵杆处于浮动状态,常用于固定航模与穿越机的控制。而拆开遥控器安装上该组件后,油门操纵杆变为与其他操纵杆相同的自动回路状态,常用于控制安装有飞控、具有定高功能的固定翼、四旋翼与多旋翼无人机。
(2)手机与电脑调参软件
遥控器具有用于调参的软件,其中手机APP使用蓝牙连接,电脑软件使用Micro USB接头连接。
图3.4.1.5
使用这两个软件可以查看遥控器各通道值、进行混控(用于控制三角翼与V尾等航模)设置、设置通道行程等。不过作者没有试过,详细参考“4、硬件资料\蓝牙版T8FB中文说明书-R8EF 2023.9.14.pdf”或乐迪官网https://www.radiolink.com/t8fb_bt_app
(3)接收机的对码与输出信号切换
(3.1)接收机的对码
每一台遥控器都有独立的 ID 编码,开始使用设备前,接收机必须与遥控器对码,对码完成后,ID 编码则储存在接收机内,且不需要再次对码,除非接收机再次与另外一台遥控器配套使用。当您购买了新的 T8FB兼容接收机,必须要重新对码,否则接收机将无法正常使用。
对码步骤:
① 将遥控器和接收机保持 50 厘米间距水平放置。
② 打开遥控器电源开关,给接收机通电。
③ 按下接收机侧面的(ID SET)开关 1 秒钟以上,接收机指示灯开始闪烁松开,接收机指示灯由闪烁变成常亮代表对码成功。
④ 如果对码没有成功,接收机指示灯会发出慢闪提示。此时需要重复上述步骤再次对码,直至接收机指示灯常亮。
也可以点击下面的链接查看对码教程:https://www.bilibili.com/video/BV1Lt4y19713
参考:“4、硬件资料\蓝牙版T8FB中文说明书-R8EF 2023.9.14.pdf\1.3.3 对码”
(3.2)输出信号切换
R8FM 接收机支持PPM与SBUS 两种信号输出两种。短按接收机侧面的对码键(ID SET)两次(一秒内),完成PPM与SBUS切换。当接收机亮红灯时输出 PPM 信号,亮蓝灯时输出 SBUS 信号。
PPM 信号输出模式:接收机指示灯为红色,R8EM 接收机信号引脚输出 PPM 信号;
SBUS 信号输出模式: 接收机指示灯为蓝色,R8EM 接收机信号引脚输出 SBUS 信号。
3.4.2 接收机SBUS信号协议简介
航模遥控器对应的接收机一般会输出三种常见的信号用于控制无刷电机、舵机或供飞控使用,即PWM、PPM与SBUS。三种信号中PWM通常用于控制无刷电机的电调、舵机,PPM与SBUS则通常供飞控使用。本小节的飞控程序便使用了接收机的SBUS信号,需要提前将接收机与遥控器对码并切换为SBUS输出模式。
(1)PWM信号
由于选择的R8FM接收机不具有PWM输出功能,这里使用了MC6C六通道遥控器进行了实验。
PWM的通信协议广泛用于接收机与舵机、电调的控制,网络上关于其具体的形式描述不一,这里以遥控器的实测波形为准,来介绍“一种” “通用”的PWM控制协议。
首先如图1硬件接线,用示波器来看通道3(油门遥杆)的波形:
图3.4.2.1
可以看到波形图2:
图3.4.2.2
发现图2波形为普通PWM波,周期约18ms,频率约55Hz,可以近似看作20ms、50Hz。
将其中一个波形放大如图3所示:
图3.4.2.3
图中高电平时间为1ms,拔动遥杆发现高电平时间会随之改变,如下视频:https://live.csdn.net/v/254345
实际测量得到高电平的变化范围在12ms之间,域宽1ms。则这1ms的差值可以控制舵机0180度转动,也可以控制电机静止到满油门的转速。
根据作者的经验许多无刷电机电调、舵机是支持更高控制频率的PWM控制信号的,只要保证2ms的高电平区间与一点点的低电平区间,一般可用周期为2.5ms的PWM信号用于控制,即控制频率400Hz。当然也有特殊情况,比如电调比较老或者是自制的一些电调,只支持50Hz左右的控制频率也是有的。
遥控器控制舵机的实验参考:
https://blog.csdn.net/qq_53043199/article/details/127755306
(2)PPM信号
PPM信号类似多路PWM信号的叠加,详细参考:
https://blog.csdn.net/nicekwell/article/details/53866204
(3)SBUS信号
SBUS协议与串口通信协议极其相似。实际使用时可以通过电平反向电路将其转化为标准串口信号,这样,就可以用单片机的一个串口来与接收机直接通信啦!
(3.1)SUBS信号的波形
使用示波器来显示接收机SBUS信号的波形,探测结果如图2.1、2.2所示。SBUS原始信号发送的频率约为70Hz。
图3.4.2.4 SBUS原始串口数据帧
图3.4.2.5 SBUS原始串口数据帧2
(3.2)转化为标准串口信号
其实原理很简单。如图2.3.1为需要的电平反向电路,原始的SBUS信号经过 该电路转换,咻!就变成了标准的串口通信信号。其实物就长图2.3.2的样子。
图3.4.2.6 示波器接线
电平反向电路其实就是一个简单的三极管通断控制电路,其中SBUS接接收机的SBUS信号输出引脚,UART1_RX接飞控(这里接示波器)
图3.4.2.7 电平反向电路
此时用示波器检测输出信号,得到结果如图3.4.2.8、3.4.2.9
图3.4.2.8 SUBS标准串口数据帧
图3.4.2.9 SBUS标准串口数据帧2
到这里,我们就可以使用串口与接收机正常通信了。这里先用上位机来进行显示。如图3.4.2.10, 注意,经处理后的SBUS标准串口信号 的参数为:
波特率:100000(5个零哦)
数据位:8位
停止位:2位
偶校验
无硬件流控制
能同时设置辣么多参数的上位机不好找,这里用到了野火的串口助手,结果如图图3.4.2.11
图3.4.2.10
图3.4.2.11
(3.3)SBUS协议帧
当然,接收完成SBUS数据帧后不要进一步进行解析,从而得到每个通道的值。
SBUS协议的第一帧数据包含25个字节,具体格式如下:
首部(1字节)+ 数据(22字节)+ 标志位(1字节)+ 结束符(1字节)
首部:起始字节 =0000 1111b (0x0f)
数据:22 字节的数据,分别代表16个通道的数据,即每个通道的值用了 11 位来表示,22x8/16 = 11
这样,每个通道的取值范围为 0~2047,低位在前、高位在后
标志位:1字节,高四位从高到低依次表示:
bit7:CH17数字通道
bit6:CH16数字通道
bit5:帧丢失(Frame lost)
bit4:安全保护(Failsafe):失控保护激活位(0x10)判断飞机是否失控
bit3~bit0:低四位不用
结束符:0x00
将协议帧解析为的通道值的方法如下图:
图3.4.2.12
理论上来说,解析出来的每个通道的通道值范围在1000~2000之间,会随着遥控器操纵杆、拨码开关的移动而变化。实际上则可能会超过这个范围一点点,如980、2008等。
此处T8FB遥控器只用到了八个通道,SBUS协议帧解析为通道值的参考代码如下:
void Sbus_Data_Count(uint8_t *buf)
{
// 数据0 的 8位 + 数据1 的低3 位 (SBUS信号:低位在前)
CH[ 0] = ((int16_t)buf[ 1] >> 0 | ((int16_t)buf[ 2] << 8 )) & 0x07FF;
CH[ 1] = ((int16_t)buf[ 2] >> 3 | ((int16_t)buf[ 3] << 5 )) & 0x07FF;
CH[ 2] = ((int16_t)buf[ 3] >> 6 | ((int16_t)buf[ 4] << 2 ) | (int16_t)buf[5] << 10 ) & 0x07FF;
CH[ 3] = ((int16_t)buf[ 4] >> 1 | ((int16_t)buf[ 6] << 7 )) & 0x07FF;
CH[ 4] = ((int16_t)buf[ 5] >> 4 | ((int16_t)buf[ 7] << 4 )) & 0x07FF;
CH[ 5] = ((int16_t)buf[ 7] >> 7 | ((int16_t)buf[ 8] << 1 ) | (int16_t)buf[9] << 9 ) & 0x07FF;
CH[ 6] = ((int16_t)buf[ 9] >> 2 | ((int16_t)buf[ 10] << 6)) & 0x07FF;
CH[ 7] = ((int16_t)buf[ 10] >> 5| ((int16_t)buf[ 11] << 3)) & 0x07FF;
Touch_flag = (buf[23]&0x04) >> 2; //使用第3位 丢帧位 判断遥控器是否连接(1:丢帧 0:正常)
}
3.4.3 AT32定时器寄存器与库函数
SBUS协议的解析使用到了UART与TMR5通用定时器外设,其中UART在前面介绍过,这里介绍一下AT32与定时器中断相关的寄存器与库函数。注意,开关标号代表其在参考手册或固件手册的位置。
AT32定时器寄存器
-
14.2.4.12 TMR2到TMR5周期寄存器(TMRx_PR)
- 位 31:16 PR
周期值(Period value)
当 TMR2 或 TMR5 开启增强模式时(TMR_CTRL1 中的
PMEN 位),PR 被扩展为 32 位。 - 位 15:0 PR
周期值(Period value)
定时器计数的周期值。当周期值为 0 时,定时器不工作。
- 位 31:16 PR
-
14.2.4.11 TMR2到TMR5分频系数(TMRx_DIV)
- 位 15:0 DIV
分频系数 (Divider value)
计数器时钟频率 fCK_CNT = fTMR_CLK /(DIV[15:0]+1)。
DIV 为溢出事件发生时写入的分频系数。
- 位 15:0 DIV
定时器的计数器时钟频率计算是一个相当有意思的问题,根据手册中提供的求计数器时钟频率(fCK_CNT)公式:
fCK_CNT = fTMR_CLK /(DIV[15:0]+1)
可以看出fCK_CNT与两个参数有关,一个是这里介绍的分频系数寄存器(TMRx_DIV),另一个就是fTMR_CLK,即定时器的时钟源。
那fTMR_CLK的频率是多少呢?
“6、AT32官方资料\(1)教程与文档(1)AT32F435_437固件库BSP&Pack应用指南.pdf”中的“4.1.3 外设时钟”有说明:
定时器使用 APB1/2 作为时钟,特别地,当 APB 预分频系数是 1 时,定时器的时钟频率等于 APB1/2 的
时钟频率;当 APB 预分频系数不为 1 时,定时器的时钟频率等于 APB1/2 时钟频率的 2 倍。
AT32时钟树与定时器相关的部分如图3.4.3.1:
图3.4.3.1
而在程序初始化过程调用的system_clock_config()函数中有:
/* config apb2clk */
crm_apb2_div_set(CRM_APB2_DIV_2);
/* config apb1clk */
crm_apb1_div_set(CRM_APB1_DIV_2);
即将APB1、APB2总线的预分频系数都设为了2分频,故默认定时器的时钟源是其对应APB1、APB2总线时钟频率的二倍。
本小节用到了TMR5,而TMR5挂载在APB1总线上,则其时钟频率是APB1时钟频的2倍,而APB1时钟的频率要通过外部晶振的频率进行计算,这都已经写进了system_clock_config()函数中,我们只需要调用对应的变量就行。
- 14.2.4.6 TMR2到TMR5软件事件寄存器(TMRx_SWEVT)
- 位 0 OVFSWTR
软件触发溢出事件(Overflow event triggered by
software)
通过软件触发一个溢出事件。
0:无作用;
1:制造一个溢出事件。
- 位 0 OVFSWTR
在程序中需要将其赋1触发溢出事件以立即重新加载pr值和div值
(可以不进行此步吗?)
-
14.2.4.1 TMR2到TMR5控制寄存器1(TMRx_CTRL1)
- 位 4 OWCDIR
单向对齐计数方向 (One-way count direction)
0:向上;
1:向下。 - 位 0 TMREN
使能定时器(TMR enable)
0:关闭;
1:开启。
- 位 4 OWCDIR
-
14.2.4.4 TMR2到TMR5 DMA/中断使能寄存器(TMRx_IDEN)
- 位 0 OVFIEN
溢出中断使能(overflow interrupt enable)
0:关闭;
1:开启。
- 位 0 OVFIEN
-
14.2.4.5 TMR2到TMR5中断状态寄存器(TMRx_ISTS)
- 位 0 OVFIF
溢出中断标记 (Overflow interrupt flag)
当溢出事件发生时由硬件置’1’,由软件清’0’。
0:无溢出事件发生;
1:发生溢出事件,若 TMRx_CTRL1 的 OVFEN=0、
OVFS=0 时:
− 当 TMRx_SWEVE 寄存器的 OVFG=1 时产生溢出事
件;
− 当计数值 CVAL 被触发事件重初始化时产生溢出事
件。
- 位 0 OVFIF
AT32定时器相关库函数
-
5.23.6 函数 tmr_base_init
- 函数原型
void tmr_base_init(tmr_type* tmr_x, uint32_t tmr_pr, uint32_t tmr_div); - 功能描述
初始化 TMR 周期、分频 - 输入参数 1
tmr_x:所选择的 TMR 外设,该参数可以选取自其中之一:
TMR1, TMR2, TMR3, TMR4, TMR5, TMR6, TMR7, TMR8, TMR9, TMR10,TMR11, TMR12, TMR13, TMR14, TMR20 - 输入参数 2
tmr_pr:定时器周期值,16 位定时器可取 0x0000~0xFFFF,32 位定时器可取
0x0000_0000~0xFFFF_FFFF - 输入参数 3
tmr_div:定时器分频值,0x0000~0xFFFF
- 函数原型
-
5.23.8 函数 tmr_cnt_dir_set
- 函数原型
void tmr_cnt_dir_set(tmr_type *tmr_x, tmr_count_mode_type tmr_cnt_dir) - 功能描述
设置 TMR 计数器计数方向 - 输入参数 1
tmr_x:所选择的 TMR 外设,该参数可以选取自其中之一:
TMR1, TMR2, TMR3, TMR4, TMR5, TMR8, TMR9, TMR10, TMR11, TMR12,
TMR13, TMR14, TMR20 - 输入参数 2
tmr_cnt_dir:定时器计数方向
- 函数原型
-
5.23.43 函数 tmr_interrupt_enable
- 函数原型
void tmr_interrupt_enable(tmr_type *tmr_x, uint32_t tmr_interrupt, confirm_state
new_state); - 功能描述
启用或禁用 TMR 中断 - 输入参数 1
tmr_x:所选择的 TMR 外设,该参数可以选取自其中之一:
TMR1, TMR2, TMR3, TMR4, TMR5, TMR6, TMR7, TMR8, TMR9, TMR10,
TMR11, TMR12, TMR13, TMR14, TMR20 - 输入参数 2
tmr_interrupt:将要配置的 TMR 中断 - 输入参数 3
new_state:将要配置的 TMR 中断状态,可选择启用(TRUE)或禁用(FALSE)
- 函数原型
-
5.23.2 函数 tmr_counter_enable
- 函数原型
void tmr_counter_enable(tmr_type *tmr_x, confirm_state new_state); - 功能描述
启用或禁用 TMR 计数器 - 输入参数 1
tmr_x:所选择的 TMR 外设,该参数可以选取自其中之一:
TMR1, TMR2, TMR3, TMR4, TMR5, TMR6, TMR7, TMR8, TMR9, TMR10,
TMR11, TMR12, TMR13, TMR14, TMR20 - 输入参数 2
new_state:将要配置的计数器状态,可选择启用(TRUE)或禁用(FALSE)
- 函数原型
-
5.23.44 函数 tmr_flag_get
- 函数原型
flag_status tmr_flag_get(tmr_type *tmr_x, uint32_t tmr_flag); - 功能描述
获取标志位状态 - 输入参数 1
tmr_x:所选择的 TMR 外设,该参数可以选取自其中之一:
TMR1, TMR2, TMR3, TMR4, TMR5, TMR6, TMR7, TMR8, TMR9, TMR10,
TMR11, TMR12, TMR13, TMR14, TMR20 - 输入参数 2
tmr_flag:需要获取状态的标志选择
- 函数原型
-
5.23.45 函数 tmr_flag_clear
- 函数原型
void tmr_flag_clear(tmr_type *tmr_x, uint32_t tmr_flag); - 功能描述
清除标志位 - 输入参数 1
tmr_x:所选择的 TMR 外设,该参数可以选取自其中之一:
TMR1, TMR2, TMR3, TMR4, TMR5, TMR6, TMR7, TMR8, TMR9, TMR10,
TMR11, TMR12, TMR13, TMR14, TMR20 - 输入参数 2
tmr_flag:待清除的标志选择
- 函数原型
3.4.4 硬件设计
本小节例程功能:使用ANO显示航模遥控器各通道波形与连接状态
用到的硬件资源有:
- UART1_RX_PA10引脚,用于连接接收机
- TMR5定时器中断
- UART3串口,用于连接蓝牙模块
USART3_TX_PC4
USART3_RX_PC5 - 蓝牙版T8FB乐迪八通道遥控器,以及配套R8FM接收机
- 一对汇承蓝牙串口模块
图3.4.4.1 接收机引脚原理图
图3.4.4.2 接收机引脚原理图2
接收机与飞控的连接,可以直接使用上表面的焊盘,也可以使用GH1.25端子(不过还是要从上表面引出一个5V电源线)。这里使用了GH1.25引出的UART1_RX引脚,同时蓝牙使用到的UART3的两个引脚也来自这里,如图3.4.4.3:
图3.4.4.3
3.4.5 软件设计
(1)遥控器初始化
打开“2、飞控例程\4_Remoter”下的工程,在main.c的开始任务中有如下代码:
void START_task_function(void *pvParameters) //开始任务
{
/****************************************** 进入临界区,原子操作 ***************************************/
taskENTER_CRITICAL();
/****************************************** 地面站串口初始化 *****************************************/
printfSerialInit();
ANO_init_usart(230400);
/****************************************** LED初始化 ************************************************/
My_LED_init();
/****************************************** 遥控器初始化 ************************************************/
Remoter_init();
#ifdef QY_BY_CORE_BOARD //AT32核心板
#else //AT32飞控
#endif
/* 创建其他任务 */
qy_tasks_creat();
vTaskDelete(START_handler); //删除开始任务
/* 退出临界区 */
taskEXIT_CRITICAL();
}
其中15行调用了遥控器初始化的函数Remoter_init();
该函数定义在“qy_drivers\remoter.c”中,原型为:
void Remoter_init(void)
{
SBUS_uart_init(100000);
Timer_pit_init();
//清零数据
for(uint8 i=0; i<CH_NUM; ++i)
{
CH[i] = 0; //遥控器原始数据
Rc_mid[i] = 0; //遥控器中值数据(-500~500)
Rc_pos[i] = 0; //遥控器通道位置状态
Rc_unit[i] = 0; //遥控器数据加死区后单位化
}
}
可以看到该函数调用了SBUS_uart_init()、Timer_pit_init()函数用于初始化串口与定时器中断,同时也清零了一些数组变量。
(1.1)SBUS_uart_init()函数
原型为:
void SBUS_uart_init(uint32_t baudrate)
{
gpio_init_type gpio_init_struct;
/* enable the uart and gpio clock */
crm_periph_clock_enable(REMOTER_UART_CRM_CLK, TRUE);
crm_periph_clock_enable(REMOTER_UART_RX_GPIO_CRM_CLK, TRUE);
gpio_default_para_init(&gpio_init_struct);
/* configure the uart tx and rx pin */
gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER;
gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL;
gpio_init_struct.gpio_mode = GPIO_MODE_MUX;
gpio_init_struct.gpio_pins = REMOTER_UART_RX_PIN;
gpio_init_struct.gpio_pull = GPIO_PULL_NONE;
gpio_init(REMOTER_UART_RX_GPIO, &gpio_init_struct);
gpio_pin_mux_config(REMOTER_UART_RX_GPIO, REMOTER_UART_RX_PIN_SOURCE, REMOTER_UART_RX_PIN_MUX_NUM);
/* config usart nvic interrupt */
nvic_priority_group_config(NVIC_PRIORITY_GROUP_4); //优先级组 4(4 位用于抢占优先级(0~15),0 位用于响应优先级)
nvic_irq_enable(REMOTER_UART_IRQn, REMOTER_IRQ_Priority, 0); //NVIC 中断使能及优先级配置(抢占优先级, 响应优先级)
/* configure uart param */
usart_init(REMOTER_UART, baudrate, USART_DATA_8BITS, USART_STOP_2_BIT); //两个停止位
usart_parity_selection_config(REMOTER_UART, USART_PARITY_EVEN); //偶检验
usart_transmitter_enable(REMOTER_UART, TRUE);
usart_receiver_enable(REMOTER_UART, TRUE);
usart_interrupt_enable(REMOTER_UART, USART_RDBF_INT, TRUE); //使能接收中断
usart_enable(REMOTER_UART, TRUE); //使能串口
}
行3~19:
进行时钟使能、GPIO初始化等
行21~23:
设置串口接收中断的优先级,“REMOTER_UART_IRQn”是与中断索引有关的宏,“REMOTER_IRQ_Priority”宏代表串口接收中断的优先级。遥控器设置有关的宏都在remoter.h中定义:
#define REMOTER_UART USART1
#define REMOTER_UART_CRM_CLK CRM_USART1_PERIPH_CLOCK
#define REMOTER_UART_RX_PIN GPIO_PINS_10
#define REMOTER_UART_RX_GPIO GPIOA
#define REMOTER_UART_RX_GPIO_CRM_CLK CRM_GPIOA_PERIPH_CLOCK
#define REMOTER_UART_RX_PIN_SOURCE GPIO_PINS_SOURCE10
#define REMOTER_UART_RX_PIN_MUX_NUM GPIO_MUX_7
#define REMOTER_UART_IRQn USART1_IRQn //串口中断
#define REMOTER_IRQ_Priority 3 //中断优先级(比FreeRTOS的5优先)
#define REMOTER_IRQ_Function USART1_IRQHandler //串口中断函数
行26~33:
串口初始化,遥控器的串口初始化与之前小节提到的普通串口初始化类似,只是一些参数有区别。对应SBUS串口的参数,行27设置串口波特率100000(为输入参数),8个数据位,2个停止位;行28设置为偶校验。
其后就是中断、接收各种使能了。
(1.2)Timer_pit_init()函数
SBUS协议帧(共25个字节)是有首部与结束符字节的,可以通过识别这两个字节来判断是否接收完成一帧数据。而另一方面,由于SBUS信号的频率在70Hz左右,两个协议帧之间会有约十几ms的时间间隔,考虑到可靠性和接收效率,更好的方法是便是通过判断接收数据之间的时间间隔来判断协议帧接收完成与否。一般接收完一字节数据后3ms仍没有接收到数据,则判断一帧数据接收完成,而判断这3ms的时间,就要用到定时器中断了。
该函数也定义在remoter.c中,原型为:
/**
* @brief 定时器初始化
* @param 无
* @retval 无
*/
void Timer_pit_init(void)
{
crm_clocks_freq_type crm_clocks_freq_struct = {0};
float Remoter_pit_time = 3; //3ms定时器中断
/* get system clock */
crm_clocks_freq_get(&crm_clocks_freq_struct);
/* enable tmr1 clock */
crm_periph_clock_enable(CRM_TMR5_PERIPH_CLOCK, TRUE);
/* tmr1 configuration */
/* time base configuration */
tmr_base_init(TMR5, Remoter_pit_time*10, (crm_clocks_freq_struct.apb1_freq/10000 * 2 - 1));
tmr_cnt_dir_set(TMR5, TMR_COUNT_UP);
/* overflow interrupt enable */
tmr_interrupt_enable(TMR5, TMR_OVF_INT, TRUE);
/* tmr1 hall interrupt nvic init */
nvic_priority_group_config(NVIC_PRIORITY_GROUP_4);
nvic_irq_enable(TMR5_GLOBAL_IRQn, 3, 0);
/* enable tmr1 */
tmr_counter_enable(TMR5, FALSE);
}
行13:
得到各系统与和总线时钟,并保存在crm_clocks_freq_struct变量中,这是一个crm_clocks_freq_type类型的变量,而crm_clocks_freq_type数据类型定义为:
/**
* @brief crm clocks freqency structure
*/
typedef struct
{
uint32_t sclk_freq; /*!< system clock frequency */
uint32_t ahb_freq; /*!< ahb bus clock frequency */
uint32_t apb2_freq; /*!< apb2 bus clock frequency */
uint32_t apb1_freq; /*!< apb1 bus clock frequency */
} crm_clocks_freq_type;
行16:
使能定时器5的时钟。
行20~21:
设置定时器重装载值、分频值等,用到了之前讲到的公式:
fCK_CNT = fTMR_CLK /(DIV[15:0]+1)
行24~31:
设置与使能定时器中断、使能定时器,这里将定时器5的溢出中断优先级设置为3,高于FreeRTOS的Systick
(2)SBUS协议帧解析的过程
SBUS协议帧的解析主要依靠上述初始化的串口接收中断、定时器溢出中断,如下:
/**
* @brief 遥控器中断(TMR5)处理函数
* @param 无
* @retval 无
*/
void TMR5_GLOBAL_IRQHandler(void)
{
if(tmr_flag_get(TMR5, TMR_OVF_FLAG) == SET) //TMR8溢出中断标志被置位
{
tmr_flag_clear(TMR5, TMR_OVF_FLAG);; //清空中断标志
usart_enable(REMOTER_UART, FALSE);; //失能串口4
tmr_counter_enable(TMR5, FALSE);; //失能定时器5
if(rec_cnt==25)
{
Sbus_Data_Count(rec_buff); //SBUS解析
}
else
{
RecErr_Flag++;
}
rec_cnt=0;
usart_enable(REMOTER_UART, TRUE);; //使能串口4
}
}
/**
* @brief REMOTER串口中断回调函数
* @param 无
* @retval 无
*/
void REMOTER_IRQ_Function(void)
{
if(REMOTER_UART->sts_bit.rdbf) /********** 为串口接收中断 **********/
{
rec_buff[rec_cnt] = REMOTER_UART->dt; //得到 数据寄存器(USART_DT) 的值
rec_cnt++;
tmr_counter_value_set(TMR5, 0); //归零定时器计数值
tmr_counter_enable(TMR5, TRUE); //使能定时器5
}
}
行37~47:
为串口接收中断,第接收到一个数据,一方面存储在数据中,另一方面归零定时器计数值,并使能定时器。
行6~28:
定时器溢出中断,当串口接收完成一字节数据后3ms仍无其它数据初接收时,便会进入该中断。
行10~13:
进入定时器中断后,先清零中断标志,并失能串口、定时器,防止以下程序执行时被打断。
行15~24:
这便是具体的SBUS协议帧解析部分了,主要是使用Sbus_Data_Count()函数来完成的。
行26:
使能串口接收中断,进入新一轮循环。
SBUS协议帧解析流程如下图:
图3.4.5.1
Sbus_Data_Count()函数也定义在remoter.c中,原型为:
//S-BUS解算
void Sbus_Data_Count(uint8_t *buf)
{
// 数据0 的 8位 + 数据1 的低3 位 (SBUS信号:低位在前?)
CH[ 0] = ((int16_t)buf[ 1] >> 0 | ((int16_t)buf[ 2] << 8 )) & 0x07FF;
CH[ 1] = ((int16_t)buf[ 2] >> 3 | ((int16_t)buf[ 3] << 5 )) & 0x07FF;
CH[ 2] = ((int16_t)buf[ 3] >> 6 | ((int16_t)buf[ 4] << 2 ) | (int16_t)buf[5] << 10 ) & 0x07FF;
CH[ 3] = ((int16_t)buf[ 4] >> 1 | ((int16_t)buf[ 6] << 7 )) & 0x07FF;
CH[ 4] = ((int16_t)buf[ 5] >> 4 | ((int16_t)buf[ 7] << 4 )) & 0x07FF;
CH[ 5] = ((int16_t)buf[ 7] >> 7 | ((int16_t)buf[ 8] << 1 ) | (int16_t)buf[9] << 9 ) & 0x07FF;
CH[ 6] = ((int16_t)buf[ 9] >> 2 | ((int16_t)buf[ 10] << 6)) & 0x07FF;
CH[ 7] = ((int16_t)buf[ 10] >> 5| ((int16_t)buf[ 11] << 3)) & 0x07FF;
Touch_flag = (buf[23]&0x04) >> 2; //使用第3位 丢帧位 判断是否连接(1:丢帧 0:正常)
Channel_value_update(); //通道数据处理
Channel_Action(); //通道数据功能
}
可以看到,接收的方法与之前提到的SBUS协议帧的格式相同:
首部(1字节)+ 数据(22字节)+ 标志位(1字节)+ 结束符(1字节)
首部:起始字节 =0000 1111b (0x0f)
数据:22 字节的数据,分别代表16个通道的数据,即每个通道的值用了 11 位来表示,22x8/16 = 11
这样,每个通道的取值范围为 0~2047,低位在前、高位在后
标志位:1字节,高四位从高到低依次表示:
bit7:CH17数字通道
bit6:CH16数字通道
bit5:帧丢失(Frame lost)
bit4:安全保护(Failsafe):失控保护激活位(0x10)判断飞机是否失控
bit3~bit0:低四位不用
结束符:0x00
行16~17
调用了两个函数,Channel_Action()函数定义为空,而Channel_value_update()函数的原型为:
//通道数据处理
void Channel_value_update(void)
{
uint8_t i;
for(i=0; i<CH_NUM; ++i)
{
Rc_mid[i] = 1.25f *(CH[i] - 1000)/2; //处理成+-500摇杆量,1.21是为遥控器做的适配
Rc_mid[i] = (int16_t)limit(Rc_mid[i],-500,500); //限制到+—500
if(Rc_mid[i] < (-300))
{
Rc_pos[i] = LO; //处理通道 档位
}
else if(Rc_mid[i] > (300))
{
Rc_pos[i] = HI; //高档
}
else
{
Rc_pos[i] = CE; //中档
}
}
}
其主要作用是将通道原始值转化为-500~500范围内的值,并判断和操纵杆、拨码开关位置。
(3)遥控器任务与ANO示波
遥控器有关的任务定义在“qy_tasks/qy_Remoter_task.c”中,原型为:
Remoter_post_s Remoter_post; //遥控器驿站
/* Remoter任务函数 */
void Remoter_task_function(void *pvParameters)
{
while(1)
{
Remoter_post.Touch_flag = !Touch_flag; //连接标志(1:连接 0:断开)
Remoter_post.ROL_pos = Rc_pos[ROL];
Remoter_post.PIT_pos = Rc_pos[PIT];
Remoter_post.THR_pos = Rc_pos[THR];
Remoter_post.YAW_pos = Rc_pos[YAW];
Remoter_post.ROL_unit = data_to_unit_dead(Rc_mid[ROL], 40); /*遥控数据单位化和加死区(左负, 右正; 上正, 下负;如xy坐标)*/
Remoter_post.PIT_unit = data_to_unit_dead(Rc_mid[PIT], 40);
Remoter_post.THR_unit = data_to_unit_dead(Rc_mid[THR], 50);
Remoter_post.YAW_unit = data_to_unit_dead(Rc_mid[YAW], 100);
Remoter_post.ENABLE_pos = -Rc_pos[AUX3];
Remoter_post.MODE_pos = -Rc_pos[AUX1];
Remoter_post.Left_unit = data_to_unit_dead(Rc_mid[AUX4]+500, 0)/2;
Remoter_post.Right_unit = data_to_unit_dead(-Rc_mid[AUX2]+500, 0)/2;
vTaskDelay(10);
}
}
主要作用是将通道值进一步处理成常用的格式,如我们一般需要和操纵杆的位置信息用于解锁、上锁,真要操纵杆与旋钮开关的归一化值(-1~1)便于与对应电机控制量等直接相乘等。
而上述信息处理后都存储在一个Remoter_post_s类型的全局变量Remoter_post中,便于管理与调用。Remoter_post称之为数据驿站,存储任务处理过后的数据,与任务搭配使用,对程序的模块化十分有用。
Remoter_post_s是一个结构体数据类型,定义在“qy_Remoter_task.h”中:
typedef struct //遥控器驿站 数据类型
{
uint8 Touch_flag;
//四遥杆位置
CH_Pos ROL_pos;
CH_Pos PIT_pos;
CH_Pos THR_pos;
CH_Pos YAW_pos;
//四遥杆单位化值(左负, 右正)
float ROL_unit;
float PIT_unit;
float THR_unit;
float YAW_unit;
//使能小杆位置(对应实际, -1:下, 1:上)
CH_Pos ENABLE_pos;
//模式小杆位置(对应实际, -1:下, 0:中, 1:上)
CH_Pos MODE_pos;
//左右旋钮单位化值(0~1 外:0, 内:1)
float Left_unit;
float Right_unit;
}Remoter_post_s;
在任务中执行好后,就可以通过ANO将通道的值在电脑上通过波形的方式显示了,在
#define ANO_tx_type_7
void Uart_to_Computer(void)
{
……
#ifdef ANO_tx_type_7
static uint16 num = 0;
var[0] = Remoter_post.ROL_unit*1000; /*遥控数据单位化和加死区(左正, 右负; 上正, 下负)*/
var[1] = Remoter_post.PIT_unit*1000;
var[2] = Remoter_post.THR_unit*1000;
var[3] = Remoter_post.YAW_unit*1000;
var[4] = Remoter_post.ENABLE_pos*1000;
var[5] = Remoter_post.MODE_pos*1000;
var[6] = Remoter_post.Left_unit*1000;
var[7] = Remoter_post.Right_unit*1000;
var[9] = Remoter_post.Touch_flag; //连接标志(1:连接 0:断开)
#endif
GFP_usart_senddata1(var); //虚拟示波器函数
}
3.4.6 下载验证
进入“2、飞控例程\4_Remoter”,打开工程并编译。
按住BOOT按键,给飞控上电,进入Bootloader(同时进入USB的DFU模式)。编译例程,双击“Download_tool_ISP”文件夹下的“1_Download_by_usb.bat”,开始下载程序。
该例程的功能为:使用ANO显示航模遥控器各通道波形与连接状态。
连接好蓝牙模块、确认遥控器与接收机已经连接,打开ANO软件,设置好波特率与串口后打开连接,然后打开波形显示界面。
此时可以看到随和操纵杆的移动,对应通道的通道值会发生变化:
图3.4.6.1
拨支AIL等取反按键,对应通道值也会取反:
图3.4.6.2
由于程序中对操纵杆的值有死区处理,小的微调显示不出,但微调开关调的大了仍能看出效果:
图3.4.6.3