四轴飞行器1.4 姿态解算和Matlab实时姿态显示

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

MPU6050数据读取出来后，经过一个星期的努力，姿态解算和在matlab上的实时显示姿态终于完成了。

1：完成matlab的串口，并且实时通过波形显示数据

2：添加RTT查看CPU使用率的扩展功能，MPU6050读取数据的优化

3：四元素表示的坐标变化，四元素与欧拉角的关系和Madgwick的IMUupdate算法

4：飞控数据采集线程和数据处理线程的安排，类似于生产者与消费者的关系。

先放个效果视频。。。

如果看不了视频，请打开视屏网址：http://v.youku.com/v_show/id_XNzU3MTk0MTAw.html

1：matlab串口初始化还是比较简单的，网上的资料也很多，这里就直接贴初始化代码了。

 % --- Executes on button press in pb_OpenSerialPort.
 function pb_OpenSerialPort_Callback(hObject, eventdata, handles)
 % hObject    handle to pb_OpenSerialPort (see GCBO)
 % eventdata  reserved - to be defined in a future version of MATLAB
 % handles    structure with handles and user data (see GUIDATA)
 %
 global o_SerialPort;
 %______________________________________________
 %GUI全局变量
 
 %---------------------串口初始化-----------------------
 %%%COM端口初始化
 int_Index_COM=get(handles.pop_SerialPort,'Value');
 string_COM=get(handles.pop_SerialPort,'String');
 string_Select_COM=string_COM{int_Index_COM};
 o_SerialPort=serial(string_Select_COM);
 %%%Baud初始化
 int_Index_Baud=get(handles.pop_BaudRate,'Value');
 string_Baud=get(handles.pop_BaudRate,'String');
 string_Select_Baud=string_Baud{int_Index_Baud};
 double_Baud=str2double(string_Select_Baud);
 set(o_SerialPort,'BaudRate',double_Baud);
 %%%设置数据长度
 int_Index_DataBit=get(handles.pop_DataBit,'Value');
 string_DataBit=get(handles.pop_DataBit,'String');
 string_Select_DataBit=string_DataBit(int_Index_DataBit);
 double_DataBit=str2double(string_Select_DataBit);
 set(o_SerialPort,'DataBits',double_DataBit);
 %%%设置停止位长度
 int_Index_StopBits=get(handles.pop_StopBits,'Value');
 string_StopBits=get(handles.pop_StopBits,'String');
 string_Select_StopBits=string_StopBits(int_Index_StopBits);
 double_StopBits=str2double(string_Select_StopBits);
 set(o_SerialPort,'StopBits',double_StopBits);
 %%%设置输入缓冲区大小为1M
 set(o_SerialPort,'InputBufferSize',);
 %%%串口事件回调设置
 
         set(o_SerialPort,'BytesAvailableFcnMode','terminator');
         set(o_SerialPort,'terminator','!');     %!标识结束符结束，方便处理和读取数据
 
         o_SerialPort.BytesAvailableFcn={@EveBytesAvailableFcn,handles};
 % ----------------------打开串口-----------------------
 fopen(o_SerialPort);

matlab串口我们采用回调函数，类似于中断方式哈，但是mtalb的串口十分的不好用哈，没有多线程，而我们在中断里面需要进行波形显示，四元素旋转等各种数据操作，是需要花费点时间的，这就导致我们的数据平率不能很高。。当上传的速率达到100hz以后，就会出错了。。50hz也不稳定。。这个实在是有点。。。担心以后的系统辨识和惯性导航的数据处理了。。头疼。。。

matlab采用符号‘！’为结束符，碰到这个符号matlab就会调用回调函数，中间的数据都是逗号隔开的，数据顺序一次为accex,accey,accez,temp,gyrox,gyroy,gyroz,cpu_major,q0,q1,q2,q3发送，数据通过sprintf进行格式化，然后通过rt_kprintf函数发送，

   sprintf(buffer,"\n%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7d,%7.2f,%7.2f,%7.2f,%7.2f!", \
             mpu6050_data_tf->acce_x,mpu6050_data_tf->acce_y,mpu6050_data_tf->acce_z,\
             mpu6050_data_tf->temp,mpu6050_data_tf->gyro_x,mpu6050_data_tf->gyro_y,\
             mpu6050_data_tf->gyro_z,(s8)cpu_major,q0,q1,q2,q3);
           rt_kprintf("%s",buffer);

temp是MPU6050读出的温度数，cpu_major是CPU使用率，q0,q1,q2,q3分别对应四元素的四个参数，q0是实数，其他分别对应i,j,k的参数。

matlab数据处理:收到数据后，其实标准的处理方式是用matlab的regexp函数，用正则表达式将数据读取出来，我们没有用这个，上传数据格式我们自己可以控制，所以处理起来很简单，没必要用到复杂的正则表达式，而且正则表达式处理时间应该比我们自己简单的处理方法的时间要长，所以采用简单的处理方法。处理方法是先将数据中的空格去掉，然后去掉结束符感叹号，最后把数据中的间隔福逗号去掉，去掉后调用str2num函数将字符串转换为数字就行了。

     StringIn(StringIn==' ')=[];   %先去掉空格
     StringIn(StringIn=='!')=[];   %去掉感叹号
     StringIn(StringIn==',')=' ';  %逗号编程空格
     SourceData=str2num(StringIn);

调用plot函数就可以绘制波形了。。这个比较简单，不过还是解释下四元素在这里的用处。

话说我们最开始的时候写了一个通过yaw pitch roll现实姿态的函数，写着写着我们发现了用方向余弦的方法，而现在我们直接使用四元素进行坐标变化，简单暴力，几行代码搞定。具体代码如下：

     q0=SourceData();
     q1=SourceData();
     q2=SourceData();
     q3=SourceData();
 
     %创建四元素矩阵
    q = [-*(q2^+q3^)        *(q1*q2-q0*q3)        *(q0*q2+q1*q3);
         *(q1*q2+q0*q3)        -*(q1^+q3^)        *(q2*q3-q0*q1);
         *(q1*q3-q0*q2)        *(q2*q3+q0*q1)        -*(q1^+q2^)];
 
     %c初始化三角形的三个坐标点
     xd=[ -1.2735; -1.2735];
     yd=[  1.3474;  -1.3474];
     zd=[ ; ];
 
     %坐标变换
     temp = [xd(,) yd(,) zd(,);
             xd(,) yd(,) zd(,);
             xd(,) yd(,) zd(,)];
     temp = temp*q;
     xd = [temp(:,)';temp(1,1),temp(3,1)];
     yd = [temp(:,)';temp(1,2),temp(3,2)];
     zd = [temp(:,)';temp(1,3),temp(3,3)];
         

首先成功数据中提取四元素的四个参数，然后创建四元素的旋转矩阵，最后对三角形的三个坐标旋转下就可以了。。真是暴汗。。之前那程序写了一天。。。。。。

matlab界面如下，后期我们还需要添加控制四个电机的pwm数值现实和pid控制器中yaw pitch roll目标值的显示，这样就可以看到PID的控制效果和对齐进行调整了。

左边中间两个方框，左边那个33.76是mpu6050读出来的温度数值，右边的7是代表CPU使用率为7%。

2：为了观察cpu的使用情况，我想找个像UCOS里面的一个变量，可以查看CPU使用率的参数，可是RTT并没有包涵在标准的系统中，而需要单独添加，在RTT系统目录下的examples\kernel中，我们可以找到一个叫做cpuusage.c文件，将这个文件添加到我们自己的工程中，然后调用void cpu_usage_init()函数，这个函数是初始化RTT IDLE线程的一个钩子函数，在空闲期间统计CPU的使用率。。函数代码如下：

 void cpu_usage_init()
 {
     /* set idle thread hook */
     rt_thread_idle_sethook(cpu_usage_idle_hook);
 }

具体统计方式我们就不多说了哈。。这一说又要说一大段话。。初始化后怎么获取CPU的使用率，我们调用void cpu_usage_get(rt_uint8_t *major, rt_uint8_t *minor)函数就可以获取CPU的使用率，分别为整数部分和小数部分，我们值用了整数部分。。哈。。懒得处理了。。整数部分已经可以反映CPU的使用率了。

使用率一出来，吓了一跳，在1000hz采样率和100hz姿态解算下，使用率高达43%，而且我们还没有进行姿态解算。。指示简单的上传数据到matlab。。。先分析原因，我们在读取MPU6050数据的时候，因为I2C是可能可以重入的函数，使用了RTT零界区的管理函数，rt_enter_critical(void)和rt_exit_critical(void) 进行处理，I2C读取数据的时间也是相当可观的，可能是因为这个导致CPU使用率很高，当时用零界区也是想偷懒，I2C虽然是一个可重入的函数，但是我们使用的是模拟I2C，也就是说I2C是可以被打断的，所以其实我们是可以试用互斥量来处理的，本来共享资源就是应该使用互斥量来处理的哈。。额。。用互斥量，可以保证I2C不被重入，但是可以被打断，这样不回影响更高优先级任务的运行。。。

MPU6050读数据还有一个需要处理，上一章我们也说了，为了偷懒，避免大端小端的转换，我们通过共用体的方式读取处理从而避免转换，之前我们也说了，这个会带来读取数据的时候效率底下的问题，我们来算算有多底下。。。我们需要读取accex,accey,accez,temp,gyrox,gyroy,gyroz，7个变量，都是2字节的，需要读取14次，每次通过I2C读取的过程是写MPU6050的地址，然后的到ack，然后写寄存器地址，然后写MPU6050读取的命令，然后读数据，然后返回noack命令，返回stop命令，中间通讯过程中的应答帧需要的事件我们忽略掉，单看读一个字节，我们需要写三个字节读取一个字节，中间传输了4个字节，14*4=56个字节，也就是读取14个字节的数据我们中间传输了56个字节。。相当的浪费，所以还是改成连读吧。。如果我们使用硬件I2C或者使用DMA方式，这还不那么明显，读取的时候CPU可以做其他事情，指示读取获得数据的时间长了点。。哎。。模拟的生不起啊。。看看连读，连读下我们只需要写入MPU6050地址，开始寄存器，然后写读命令，然后读啊读，读完14个字节就可以了（这几个寄存器在MPU6050里面内部地址是连起来的哈），也就是3+14=17，也就是说读14个字节，我们中间只传了17个字节，效率相当的客观。经过这样处理,CPU的使用率可以降低15%左右。。。。

 3：四元素表示的坐标变化和四元素与欧拉角的关系

说起四元素，不懂的时候感觉那是相当的高深，这是个姿态表达式而已，就和欧拉角一样的，换个表达方式。。

其实回到最开始的地方，就是我们获取MPU6050的数据后怎么处理。一个是加速度，一个是角速度。。有了加速度我们就可以算出pitch和roll了，可是可是。。。。。可是灰机是动态的，会动的，灰机动的过程中自己本身机体也会产生加速度，那么我们就需要分辨因为地球重力产生的重力加速度和机体的加速度才能算出集体的pitch和roll了，这个分辨的过程会比较麻烦，不过后面用惯性导航算法的时候，这个是接触，否则没有分辨出机体的加速度怎么可以计算出灰机的轨迹呢。。对吧。。不过这里有个问题哈，加速度在震动情况下输出的值是波动很大的，而陀螺在动态下输出就要好很多。。。可是可是再可是。。。陀螺输出的是角速度，我们用时间乘以这个角速度就可以的到角度，每次积分的角度和上次的角度相加，就可以达到集体的xyz三个轴的角度了，可是可是再可是。。。。你采样频率足够高吗？解算频率足够高吗？在你解算的过程中，角速度是恒定的吗？保证不了吧？那就意味着长时间的对陀螺仪积分出来的角度误差会越来越大。。。虽然积分的时候可以使用龙格-库塔积分方法，这个方法还是比较简单的哈，但是角速度积分误差还是无可避免的。。。这个时候加速度就派上用场了。。。用加速度校正陀螺仪角度积分的误差啊。。。。对，没错，就是短期相信陀螺仪，然后长期相信加速度。Madgwick的IMUupdate算法的总体思路就是这样的哈。。。

返回来说四元素。。。四元素其实不难的哈，我参照 邓正隆的惯性技术的四元素部分学习了下，讲的还是比较详细的，看不懂？对，开始是看不怎么太懂，，哈。怎么办？做题啊。。。按照书上讲的，按照他的推导过程，自己推倒一遍。。瞬间明白了有木有？虽然理解深度可能还不深，但是我直到怎么用了。。哈哈。。

推到过程自己用笔推下就懂了。。

四元素还可以参考这个网址http://www.cnblogs.com/Mrt-02/archive/2011/10/15/2213656.html

四元素的一些公式：

坐标转换公式，我们matlab里面画图的坐标转化公式就是这个哈。。

其中w,x,y,z就是四元素的四个元素，W为实数部分，xyz对应ijk的三个变量。

四元素的微分方程：

这个公式在IMUupdate有用到哈，看了这个公式应该直到halfT是怎么来的了吧。。四元素其实知识还挺多的哈。。还是要自己算算才会哈。。看没用的。。

4：飞控数据采集线程和数据处理线程的安排，类似于生产者与消费者的关系。

采集数据的频率可以很高，采集足够多的数据才好处理吗。哈。。可是CPU的性能考虑，姿态解算频率太高并不划算。。。

我们有两个线程，一个采集数据线程，采集频率500hz，一个姿态解算线程，将采集过来的数据进行解算，通过解算的结果再控制电机。这其中数据采集线程就是生产者，姿态解算线程就是消费者。。。。他们之间需要通过怎样的协调工作才能是效率高并且数据安全呢？这里我们有三钟方法：

（1） 生产者只管采集数据，消费者只管消费数据，这就有个麻烦，生产者不知道消费者什么时候会消费数据，所以他要时刻将数据准备好，让消费者随时可以消费数据，并且数据是要最新的。那么滤波怎么办？每个数据都要是最新的且滤波好的，那就要用平滑滤波了，应该是这么叫，然后窗口多大呢？不知道，额。。希望是低耦合高内聚的代码哈。如果需要解决窗口大大小，有两种方法，实现预定好，或者消费者消费的时候告知生产者，我已经消费，那么生产者可以重新设定窗口。那么，这中间不可避免的消费者和生产者是要交流的，而且消费数据的时候数据是共享资源，需要互斥量。。也就是说，要实现我们需要互斥量，然后要平滑滤波，生产者每次生产数据都要进行滤波运算。。。效率并不高         

（2） 生产者采集N个数据，通知消费者消费。这里有个问题，消费者处在阻塞状态，等生产者生产好了通知消费者，消费者才消费，可是，系统运行会出现各种各样的问题，无法保证100%生产者的task不出问题，所以这样的系统怎么说呢，安全不好保证，同是也不利于我们到时候统计各个任务的状态，通过统计各个任务的状态我们可以实现类似于硬件看门狗的功能，任务出问题了，可以对任务进行删除再重启的操作等。。

（3） 生产者只管采集数据，消费者消费数据，但是我们这中间增加一个协调员。我们有两个变量来同步这两个任务。。互斥量和计数。。。生产者只管统计数据，将数据进行累加，同时技术，消费者消费数据，但是需要做些处理，拿到数据后对数据均值滤波，就是除以统计的次数，同时将原来的数值请0。。。。互斥量的存在，可以保证数据操作的同步和安全性。最后，我们是采用第三种方法。