转：：：：[单片机经验] 四轴飞行器1.4 姿态解算和Matlab实

oleilei

转自：http://bbs.elecfans.com/jishu_464398_1_1.html



MPU6050数据读取出来后，经过一个星期的努力，姿态解算和在matlab上的实时显示姿态终于完成了。
1：完成matlab的串口，并且实时通过波形显示数据
2：添加RTT查看CPU使用率的扩展功能，MPU6050读取数据的优化
3：四元素表示的坐标变化，四元素与欧拉角的关系和Madgwick的IMUupdate算法
4：飞控数据采集线程和数据处理线程的安排，类似于生产者与消费者的关系。
先放个效果视频。。。
http://v.youku.com/v_show/id_XNzU3MTk0MTAw.html
如果看不了视频，请打开视屏网址：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',1024000);
%%%串口事件回调设置
        
        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(9);
    q1=SourceData(10);
    q2=SourceData(11);
    q3=SourceData(12);
   
    %创建四元素矩阵
   q = [1-2*(q2^2+q3^2)        2*(q1*q2-q0*q3)        2*(q0*q2+q1*q3);
        2*(q1*q2+q0*q3)        1-2*(q1^2+q3^2)        2*(q2*q3-q0*q1);
        2*(q1*q3-q0*q2)        2*(q2*q3+q0*q1)        1-2*(q1^2+q2^2)];
     
    %c初始化三角形的三个坐标点
    xd=[3 -1.2735;3 -1.2735];
    yd=[0  1.3474;0  -1.3474];
    zd=[0 0;0 0];
   
    %坐标变换
    temp = [xd(1,1) yd(1,1) zd(1,1);
            xd(1,2) yd(1,2) zd(1,2);
            xd(2,2) yd(2,2) zd(2,2)];
    temp = temp*q;
    xd = [temp(1:2,1)';temp(1,1),temp(3,1)];
    yd = [temp(1:2,2)';temp(1,2),temp(3,2)];
    zd = [temp(1:2,3)';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使用率很高，当时用零界区


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




这个公式在IMUupdate有用到哈，看了这个公式应该直到halfT是怎么来的了吧。。四元素其实知识还挺多的哈。。还是要自己算算才会哈。。看没用的。。
4：飞控数据采集线程和数据处理线程的安排，类似于生产者与消费者的关系。
    采集数据的频率可以很高，采集足够多的数据才好处理吗。哈。。可是CPU的性能考虑，姿态解算频率太高并不划算。。。
    我们有两个线程，一个采集数据线程，采集频率500hz，一个姿态解算线程，将采集过来的数据进行解算，通过解算的结果再控制电机。这其中数据采集线程就是生产者，姿态解算线程就是消费者。。。。他们之间需要通过怎样的协调工作才能是效率高并且数据安全呢？这里我们有三钟方法：
（1） 生产者只管采集数据，消费者只管消费数据，这就有个麻烦，生产者不知道消费者什么时候会消费数据，所以他要时刻将数据准备好，让消费者随时可以消费数据，并且数据是要最新的。那么滤波怎么办？每个数据都要是最新的且滤波好的，那就要用平滑滤波了，应该是这么叫，然后窗口多大呢？不知道，额。。希望是低耦合高内聚的代码哈。如果需要解决窗口大大小，有两种方法，实现预定好，或者消费者消费的时候告知生产者，我已经消费，那么生产者可以重新设定窗口。那么，这中间不可避免的消费者和生产者是要交流的，而且消费数据的时候数据是共享资源，需要互斥量。。也就是说，要实现我们需要互斥量，然后要平滑滤波，生产者每次生产数据都要进行滤波运算。。。效率并不高
（2） 生产者采集N个数据，通知消费者消费。这里有个问题，消费者处在阻塞状态，等生产者生产好了通知消费者，消费者才消费，可是，系统运行会出现各种各样的问题，无法保证100%生产者的task不出问题，所以这样的系统怎么说呢，安全不好保证，同是也不利于我们到时候统计各个任务的状态，通过统计各个任务的状态我们可以实现类似于硬件看门狗的功能，任务出问题

shakencity

学习学习