基于S698和MPU9150的无人机姿态识别

季宏祥 耿相铭



基于S698和MPU9150的无人机姿态识别

季宏祥 耿相铭

上海交通大学电子信息与电气工程学院，上海200240

S698是一款由珠海欧比特控制工程股份有限公司研制出的32位嵌入式处理器。采用S698的主控芯片平台，调用Motion- Fusion，其输出的四元数转换后的姿态角与无人机的姿态相符。介绍如何结合S698平台重构关键函数及底层驱动来实现调用MotionFusion来识别无人机的姿态。

S698；MPU9150；姿态；MotionFusion；四元素

引言

珠海欧比特工程研发的S698处理器，是国内首款基于SPARK V8架构的32为嵌入式处理器。由于国防、航空等领域的特殊性，国产的S698已被广泛的应用于这些领域[1]。

MPU9150的MotionFusion专利能够帮助客户减少在运动数据融合方面的工作，其目前主要支持TI-MSP430和ARM平台，对于其他平台，使用者需要结合Invensense官网提供的Motion Driver 5.1.2版本，重构部分函数进而实现运动数据的融合及四元数姿态输出。Motion Driver 5.1.2的限制在于它不能融合磁力计的三轴数据，但由于Motion Driver平台本身要求，若需要融合磁力计的数据，必须使用TI-MSP430或者ARM平台，本文中所讨论的是基于Motion Driver 5.1.2版本，而融合的数据也是六轴数据[2]。

1 Invensense的Motion Driver概述

Motion Driver是Invensense公司提供的传感器驱动层，以便用户能够配置并启动Invensense公司运动监测芯片的数字融合处理功能。Motion Driver的源代码是以C语言的方式编写的，方便进行移植。其融合的运动数据是可以根据用户的需求进行配置的，关于Motion Driver中各个函数具体的接口定义及含义参见Embeded Motion Driver v5.1.1 APIs Specification。Motion Driver的移植需要根据Invensense提供的Motion Driver 5.1.1 Tutorial进行。本指导书中关于具体的哪些函数需要重构，并不明确，需要使用者去阅读Motion Driver的源代码，并且需要结合所需要移植的控制平台对接口及部分底层驱动函数根据需要进行重构[3]。

2 S698中关键函数及驱动的重构

2.1 I2C总线驱动及接口函数重构

S698内部没有I2C总线接口，而使用S698的多功能口模拟I2C总线的关键在于模拟时钟SCL和根据SCL时钟。模拟SCL时钟有两种方式，一种是使用空指令，等待延时。另外一种非阻塞式的实现方式是通过定时器来实现。结合Motion Driver中的源代码，所有的涉及I2C的驱动调用均是阻塞式的方式，所以在S698平台上采用阻塞是方式的I2C。下面以I2C总线写操作为例说明实现过程[4]。

图1 I2C总线写操作为例说明实现过程

3.2 中断函数重构

MPU9150的中断输出功能在Motion Driver中被多次调用，其可被用于提醒主芯片其所需的数据或者运动数据的融合已经完成等。以上的功能的实现需要结合硬件平台上MPU9150中断引脚与S698的连接，以及软件对MPU9150的配置。由于在Motion Driver中具有专门的中断注册函数接口，用于关联具体的中断源于中断处理函数，在移植的过程中，需要根据S698平台重构此中断注册函数。主要的中断相关函数重构如下。

2.3 时钟函数重构

根据Motion Driver的要求，对于部分时钟函数也需要重构。采用S698通用定时器1来执行延时[5]。

图3 采用S698通用定时器1来执行延时

4 S698平台上配置并调用Motion Driver

配置和调用的主要步骤包括以下几步。（1）S698及MPU9150的初始化及自测。（2）配置加速度计及陀螺仪的量程。（3）加载数字运动处理程序（DMP）的镜像文件到MPU9150中。（4）将陀螺仪和加速度计的初始方向矩阵输入到DMP中。（5）配置具体需要DMP反馈的数据。（6）启动DMP。（7）配置DMP的数据输出的速率。

测试使用的是S698自带的开发平台Orion 4.0，使用的硬件测试平台为基于S698的旋翼无人机的飞行控制板。在测试过程中，根据以上七步，成功调用MPU9150内部的数字运算处理模块，输出六轴融合数据及传感器原始数据，同时将四元数转换成欧拉角，结果显示欧拉角的输出与无人机机体的姿态相符。

5 总结

本文给出了基于S698平台如何调用MPU9150内部的运动处理单元，并详述了其关键函数重构的方法。通过将MPU9150其输出的四元数转换后的姿态角与无人机的姿态相比较验证了该移植的有效性。需要指出的是，由于Invensense公司自身运动库的限制，若需要在S698平台上实现九轴数据的融合，则需要完全抛开Motion Driver，而这就不可以避免的涉及到各种融合算法。关于此项工作的研究，有待后续开展。

[1]冀亮，钱正洪，白茹.基于四元数的四轴无人机姿态的估计和控制[J].现代电子技术，2015（11）：112-116.

[2]任志明，李永树，何敬，等.基于姿态数据的无人机影像自动刺点功能研究[J].测绘通报，2011（4）：44-46.

[3]宋宇，翁新武，郭昕刚.基于四元数EKF算法的小型无人机姿态估计[J].吉林大学学报：理学版，2015（3）：511-518.

[4]何永勃，贾辉，姜坤，等.基于Android终端陀螺仪传感器的无人机飞行姿态控制[J].传感技术学报，2015（4）：474-478.

[5]赵亚鹏，曹彬才，吴帅.基于飞行姿态校正的无人机影像快速拼接[J].测绘科学与工程，2015（1）：47-54.

V249.12

A

1009-6434（2016）02-0018-02