四轴飞行器1.8 通讯协议拟定、协议验证与飞控输出验证

原创文章，欢迎转载，转载请注明出处。

        上个周末其实通讯协议就已经拟定完成了，这一个星期主要成了通讯协议的解析，然后通过通讯协议的实现，加入遥控器的控制和飞控信息的传递，从飞控传到遥控器，再从遥控器传到电脑上，通过matlab现实姿态信息和电机输出控制信息。这章会一步一步介绍实现的过程。

1：遥控和飞控之间的通讯。

2：通讯协议的拟定。

3：通讯协议的实现与解析。

4：通过遥控控制飞控并且飞控姿态通过nrf上传数据。

  

先上通过遥控控制飞控并且飞控姿态通过nrf上传数据的视频哈，看看效果，解释在后面；

视频地址http://v.youku.com/v_show/id_XNzg3NjcwMDc2.html

 

 

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，应该可以飞了。。