原创文章,欢迎转载,转载请出名出处。
上个周末其实通讯协议就已经拟定完成了,这一个星期主要成了通讯协议的解析,然后通过通讯协议的实现,加入遥控器的控制和飞控信息的传递,从飞控传到遥控器,再从遥控器传到电脑上,通过matlab现实姿态信息和电机输出控制信息。这章会一步一步介绍实现的过程。
1:遥控和飞控之间的通讯。
2:通讯协议的拟定。
3:通讯协议的实现与解析。
4:通过遥控控制飞控并且飞控姿态通过nrf上传数据。
先上通过遥控控制飞控并且飞控姿态通过nrf上传数据的视频哈,看看效果,解释在后面;
视频发布中,发布了再加进来。
视频地址
1:遥控和飞控之间的通讯
有了上章的环形缓冲和通讯的基础,现在就验证下环形缓冲和通讯结合起来的工作效果。
有了环形缓冲,我们实现发送和接收就比较简单了,需要发送数据的时候,我们只要把数据push到缓冲里面,接收数据的时候在接受中断了里将数据push进缓冲。我们有一个专门通讯管理的task,用来判断缓冲里面是否有数据,如果发送缓冲里面有数据,就读取数据,发送,发送后然后将数据pop,如果接收缓冲里面有数据,就读取数据,并且根据协议解析,然后pop。
根据上面的说明,我们在遥控上面发送数据,然后飞控接到数据后会返回数据给遥控,如此往复。。具体通讯效果如下图。
上图左边是遥控器的串口输出信息,右边是飞控输出的信息。图中txds为收到ACK包,txed为发送完成,rxed为接受完成,trmax为重试了设定的次数还没收到ACK包,意味着发送失败。发送失败的还是有的哈,不过暂时没有做发送失败的处理,也没有判断。。我们可以失败了不pop,稍微等待个几毫秒再试着发送一次。。
2:通讯协议的拟定
通讯协议需要简单,尽量少的数据传输尽量多的信息。。。
首先我们将协议进行了分类,分为控制类,查询类,和主动上报类,一共三类,所以需要占用2位,一个字节里面占2位后还有6位,有64种组合,目前看应该够我们用了,然后后面就是根据各个命令来传输数据了。协议大致如下图:
根据协议我们定义了如下结构体:
1 typedef union 2 { 3 u8 mode; //飞行模式的mode参数 4 Com_Att_t Att; //姿态信息 5 u32 Height; //高度参数 6 Com_PID_t Pid; //PID信息 7 u8 RetPara; //返回信息 8 Com_Sensor_t Sensor; //传感器数据 9 u8 Battery; //电池电量 10 }Comm_Para_tu; 11 12 typedef struct 13 { 14 u8 Command:6; //0~5位为命令 15 u8 ComType:2; //6~7位为命令类型 16 }Order_t; 17 18 19 typedef struct 20 { 21 u8 Len; //数据长度 22 Order_t Order; //命令类型 23 Comm_Para_tu Para; //参数 24 }Comm_Data_ts;
Comm_Data_ts是我们通讯用到的结构体,根据协议,里面有数据长度,然后有命令类型和对应的命令,后面就跟着参数。发送数据只需要对这个结构体赋值,接收数据后解析只需要根据这个机构体的Order找到对应的命令,然后根据对应的参数读取数据就好了,这样在上层操作的时候,我们不需要管通讯协议的哪个字节存什么数据,只需要对结构体进行操作。
3:通讯协议的实现与解析
为了方便代码的实现,我们对每个命令写了一个解析函数。那我们是怎样找到这个函数的呢?我们创建一个结构体,里面有命令和命令对应的处理函数的函数指针,我们只需要进行命令匹配,匹配好后,就调用对应的函数指针调用对应的函数,这样就完成了解析。
结构体如下:
1 /*命令回调结构*/ 2 typedef struct 3 { 4 void (*Comm_CallBack)(Comm_Data_ts *msg); 5 u8 command; 6 } SubComm_Fun; 7 8 9 typedef struct 10 { 11 const SubComm_Fun *SubComm; //指向子命令 12 u8 ComType; //命令类型 13 u8 Len; //子命令个数,优化搜索时间 14 } Comm_Fun;
因为我们命令分两层,所以先对命令类型解析,然后搜索下一层命令,然后再运行对应的函数指针,打个比方,上面结构体的部分初始化如下:
1 //查询类命令于对应函数定义 2 //注意,一定要注意,一定要修改FLY_QUERY_FUN_LEN宏定义 3 const SubComm_Fun Fly_Query_Fun[]= 4 { 5 { 6 Fly_Query_Sensor, 7 FLY_QUERY_SENSOR 8 }, 9 10 { 11 Fly_Query_Attitude, 12 FLY_QUERY_ATTITUDE 13 }, 14 15 { 16 Fly_Query_System_Sta, 17 FLY_QUERY_SYSTEM_STA 18 }, 19 20 { 21 Fly_Query_Battery, 22 FLY_QUERY_BATTERY 23 }, 24 }; 25 #define FLY_QUERY_FUN_LEN 4 26 27 28 //命令类型对子命令数定义 29 //注意,一定要注意,一定要修改Comm_Fun_Len宏定义 30 const Comm_Fun Fly_Fun[]= 31 { 32 { 33 Fly_Ctrl_Fun, 34 FLY_CTRL, 35 FLY_CTRL_FUN_LEN 36 }, 37 38 { 39 Fly_Query_Fun, 40 FLY_QUERY, 41 FLY_QUERY_FUN_LEN 42 }, 43 44 { 45 Fly_Report_Fun, 46 FLY_REPORT, 47 FLY_REPORT_FUN_LEN 48 }, 49 };
初始化上面的机构体后,我们就根据命令来解析,解析代码如下:
1 void Comm_Decode(Comm_Data_ts *msg) 2 { 3 u8 i,k; 4 5 for(i = 0 ; i < COMM_FUN_LEN; i++) 6 { 7 /*查询消息命令*/ 8 if(msg->Order.ComType == Fly_Fun[i].ComType) 9 { 10 for(k = 0 ;k < Fly_Fun[i].Len; k++) 11 { 12 if(Fly_Fun[i].SubComm[k].command == msg->Order.Command) 13 { 14 Fly_Fun[i].SubComm[k].Comm_CallBack(msg); 15 break; 16 } 17 } 18 break; 19 } 20 21 } 22 23 }
这样一是代码重用率很高,而且增加和删减命令很简单。不过我们可以将命令类型改成枚举类型,可以避免出现错误的命令可能。
解析的效果如下图:
上图左边为遥控器的nrf的debug信息,右边为遥控器接收到数据后的解析的debug信息。遥控器通过手柄的及格按键发送不同的命令进行测试,通过实际观察,运行良好。
4:通过遥控控制飞控并且飞控姿态通过nrf上传数据
将上面功能全部结合起来,同时实现无线姿态采集,都是通过协议实现的。
先说明下我们的运行方式:飞控定时给遥控器发送传感器信息和姿态信息,遥控通过串口发给matlab,并且进行实时现实。这里我们上传的信息添加了电机的控制信息,包括总油门和每个电机的单独油门,通过这个我们可以观察飞控对桨的操作逻辑是否正确。同时遥控可以发送遥控命令,命令飞控的目标姿态。效果图图下:
左边的柱状图12345分别为moto1,moto2,moto3,moto4和油门。
电机位置示意如下:
字很丑哈,随便画的当时,看不太明白的可以到如下网址看,电机的方位和角度的定义有点不同哈,但是原理是一样的。
网址:http://bbs.21ic.com/icview-605405-1-1.html
最后联机测试的视频见顶端,通过测试,电机输出逻辑正常,放心了哈。。后面加电调控制进去,调下PID,应该可以飞了。。