PCM开发板模块实验指导--6轴姿态融合实验

模块推荐：https://item.taobao.com/item.htm?ft=t&id=671629736762

        47---6轴姿态融合实验：

                硬件需求：PC机，PCM模块一个，microUSB线，6轴传感器MPU6050,导线若干；

                软件需求：PCM编译软件,电脑端串口调试助手；

                供电方式：USB线接PC机供电；

                连接电路：如图70（6轴传感器实验）

                        在实验45--6轴传感器实验中，已经介绍了如何输出6轴传感器的原始数据，本实验

                介绍如何使用fusion指令把3轴加速度数据与3轴角速度数据融合成3个姿态角

                ROLL,PITCH,YAW;

                        本实验使用的fusion指令为姿态融合输出指令，指令带2个参数，参数1指定6轴数据

                的起始寄存器地址，存储顺序为：AX,AY,AZ,GX,GY,GZ；参数2指定3个姿态角的输出起

                始寄存器地址，存储顺序为：ROLL,PITCH,YAW;

                        实验程序如下：

                1 intpro imu6int 

                2         fusion f0 f10 //姿态融合，输入ax=f0,ay=f1,az=f2,gx=f3,gy=f4,gz=f5,

                                                //输出ROLL,PITCH,YAW角度分别到f10,f11,f12

                3 endint

                4 subpro outdata //数据输出子程序

                5         prints "roll:%f pitch:%f yaw:%f\r\n" f10 f11 f12 //输出roll pitch yaw角度

                6 endsub

                7 main 

                8         iic 0 400000 //初始化IIC0接口，速度为400000HZ

                9         intinit s0 imu6int //开启中断

                10         imuinit 0 f0 s0 //启动6轴测量，ax,ay,az,gx,gy,gz分别保存到f0,f1,f2,f3,f4,f5;

                                                //更新周期0.004S，输出后中断

                11         while 1 //无限循环

                12                 delay 1000 //延时1000ms

                13                 call outdata //调用子程序，输出融合后的姿态角：ROLL,PITCH,YAW

                14         endwhile

                15 endmain

                        程序中第1-3行，为6轴数据输出中断程序，中断周期固定为4ms,用户不可更改；

                        第2行，为姿态融合指令，F0为6轴数据的起始寄存器地址，

                即F0=AX,F1=AY,F2=AZ,F3=GX,F4=GY,F5=GZ, F10为姿态融合输出的起始寄存器地

                址，F10=ROLL,F11=PITCH,F12=YAW,单位为度；

                        第4-6行为姿态输出子程序，通过串口打印输出3个姿态角；

                        第7-15行，为主程序，此程序与6轴传感器实验差不多，只是在11-14行循环体内每

                秒输出一次姿态角；

                        用PCM编译软件编译以上程序，并下载到模块，电脑端使用串口调试助手接收数

                据，下图73为姿态角的输出，显示了ROLL,PITCH,YAW，3个角度。

        图73         6轴姿态融合实验