Pixhawk的传感器数据（陀螺、加计等）流程

文/mike

一、总体流程

先由驱动层drive，再到中间层sensor，再到应用层ekf2,最后发布数据给其他应用。控制系统最重要的是带宽，位置环的带宽，到速度环的带宽，再到传感器的更新率，所以，做控制，看程序的时候一定要注意几个时间的概念。举个例子，位置环50hz，速度环200hz，传感器数据的更新率一定要高于200hz。如果自己写程序，里面的这几个时间一定要打印出来看一看，或者翻转一个电平用示波器观察以下。这是一个控制系统的基本要素，带宽的概念。
为什么位置环是50hz呢，速度环是200hz呢？这个问题，我们可以再开一篇文章进行讨论，这里说个大概，具体的控制频率是取决于应用环境的环境变化速率。比如飞机在空中悬停着，我们就需要对自然环境给飞机绕动进行建模。比如飞机的机体低频震动（高频需要滤波，带宽做高成本较大），比如一阵风吹过来，比如手动打舵的更新率等。一般控制带宽要高于使用环境绕动或给定带宽的3到5倍。不要问我为什么，这是经验公式，一定要有理论出处，那就是采样定理，采样定理说，只要2倍于一个信号带宽就可以还原这个信号，但是我们一般都在做工程，一是要留一点冗余量，二是采样频率越高，对信号还原的失真度越低。大家一定还记得我们开始学控制原理的时候，是用一个pid去跟踪几个基本信号，正弦信号，斜坡信号，阶越信号等。这里可以反向看作是对这几种信号的采样还原。
如果再需要理论化一点，一般一套控制系统，开始需要建模，对各种扰动信号进行建模，建立了系统的控制模型后，在matlab里面有个工具，叫simulink，里面有一些基础模型，另外，这个工具可以对建立的模型进行分析，自控里面的两大分析方法，根轨迹法和频率分析法。一般做大型工程是需要按套路从理论到软仿，到半实物仿真，到原理样机实测，一步步来的。但是消费电子，为快不破，特别是现在很多开源的工程，前面的基础工作已经有人做过了，论证过了，更何况有些项目已经被无数爱好者试验过N多次了。我们就可以直接站在巨人的肩膀上了。有些年轻人，一进来就要去研究高深的理论算法，系统仿真。仿佛不说几个牛逼的理论，不足以证明自己也很牛逼似的，结果扎如了浩瀚的推导和公司，久久不能出局。就像笔者开始研究linux的时候，一头扎进了系统内核的各种函数，仿佛进入了一片浩瀚的海洋，找不到方向，久而久之消磨了热情。
能够按照控制系统工程，从头开始干一遍的，不是军工企业就是国有企业，如果都不是，一定是牛逼的上市公司，大型公司。他们有足够的资金来搭建这些系统，软件系统，硬件系统，半实物仿真系统，人员系统，人员系统尤其昂贵，做仿真的可能就是一个科室几十号人，另外做研究做到这个层面基本都是硕士以上了，并且普通高校培养的人员可能技术层面上还略有欠缺，所以可以按照整个控制系统工程来做一遍的企业一定不简单。有点扯远了，还是来说我们的pixhawk。对于这个世界上的大多数工程和应用，人类凭借自己的经验就可以成功，比如生产一把菜刀，绝壁没有人来对菜刀进行有限元应力分析，在多大削砍速度下，砍在什么样的材料上，刀身各处的强度分布是怎么样的……，什么情况下会卷韧会折断……，有功夫算这些东西的，别人已经基于人类菜刀几千年的经验传承，做了一把刀把生活中要砍之物全部砍了个遍。说到这里，好像讲到了哲学里面的两个派系，经验派，逻辑派，哈哈，也有点像笑傲江湖里面剑宗和气宗。又扯远了，这里我其实想说的是，对于大多数的工程应用级别的，我们做好应用就行了，侧重点放在框架，流程，接口，全貌，弄清楚了这些，还有精力的话，可以深入研究下里面的部分算法，这就叫行有余力而学文。让哪些研究所，开源机构，大型公司去做理论分析。你看，google弄好的的牛逼的AI引擎tensorflow不也开源了吗？保持对这个世界好奇，找准自己的定位，什么人做什么事，争取到不惑之年真达到孔夫子讲的那个境界。本来想讲个传感器数据的流向问题（一个飞控用传感器的数据，怎么产生，然后它经历了什么，最后到了哪里，发挥了什么作用，这个数据的整个生命周期多长，有哪几个关键的阶段，每个阶段大概多久），结果手指随心所欲，想到哪里键盘就敲到了哪里，各位凑合这看吧。有问题，我们可以在交流。
陀螺的数据，定时器回调函数自动的采集，采集之后会进行积分，积分之后在sensor.cpp 里面会根据积分时间在进行平均，又得到了角速度，然后有一个get_best的函数，会从几组陀螺仪里面，选出最好的一组速据，然后发布在sensor_combine主题里面。

                          /*
			 * Using data that has been integrated in the driver before downsampling is preferred
			 * becasue it reduces aliasing errors. Correct the raw sensor data for scale factor errors
			 * and offsets due to temperature variation. It is assumed that any filtering of input
			 * data required is performed in the sensor driver, preferably before downsampling.
			*/

上面是源码里面的注释，他说用那些在降采样之前的积分数据是非常好的，因为他减少了混跌误差，修正了原始数据的比例因子的误差还有由于温度振荡带来的漂移。

二、驱动层

在驱动层，以mpu6000为例（mpu6000.cpp），进入start函数，start在启动脚本（rcs）根据不同的硬件版本进行启动，此处不赘述。进入start函数后可以看到，传感器数据是通过定时器定时回调进行测量的。

	hrt_call_every(&_call,
		       1000,
		       _call_interval - MPU6000_TIMER_REDUCTION,
		       (hrt_callout)&MPU6000::measure_trampoline, this);

在measure_trampoline里面，通过spi总线读取了陀螺和加速度计的值，最后通过sensor_accel和sensor_gyro主题发布了读到的数据。

三、中间应用层

这层，主要是一个sensors.cpp，里面有陀螺，加速度计，气压，空速等传感器的值。这个函数对所有的传感器的原始值进行了滤波，然后发布。发布在sensor_combine主题里面，供其他函数调用。这个sensors.cpp里面关键的几个函数
_voted_sensors_update.sensors_poll(raw);
这个函数在拉取数据的同时，对数据进行了滤波，滤波就是上面提到的那个注释，其实这里还做了一个事情，就是机体坐标系的调整，也就是你的飞控在飞机里面的安装方向的问题。然后调用了下面的函数，选出几组传感器里面最好的数据。
_accel.voter.get_best(hrt_absolute_time(), &best_index);
最后通过
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
将所有传感器的数据发布出去了。

###四、应用层
为什么要把上面的sensors.cpp归类为中间层，我的理解是，他还是在为核心应用准备数据。核心应用层，这里以EKF2为例，进了主函数后，首先订阅了sensor_combined 数据。数据比较多，这里还是以陀螺和加速度计为例子，下面有一段很清晰的代码，外国兄弟把注释都写的很清楚的。
// push imu data into estimator float gyro_integral[3]; gyro_integral[0] = sensors.gyro_rad[0] * sensors.gyro_integral_dt; gyro_integral[1] = sensors.gyro_rad[1] * sensors.gyro_integral_dt; gyro_integral[2] = sensors.gyro_rad[2] * sensors.gyro_integral_dt; float accel_integral[3]; accel_integral[0] = sensors.accelerometer_m_s2[0] * sensors.accelerometer_integral_dt; accel_integral[1] = sensors.accelerometer_m_s2[1] * sensors.accelerometer_integral_dt; accel_integral[2] = sensors.accelerometer_m_s2[2] * sensors.accelerometer_integral_dt; _ekf.setIMUData(now, sensors.gyro_integral_dt * 1.e6f, sensors.accelerometer_integral_dt * 1.e6f, gyro_integral, accel_integral);
把陀螺和加速度的数据推入评估器。哈哈，推进去干什么呢？推进去当然是融合，怎么融合呢？说起来很简单，卡尔曼五步法，细节很复杂，我也没有完全弄透，关于卡尔曼的，一本书可以推荐（《最优状态估计》）。下面我们关心我们更想要的东西。那就是用来做控制用的ctrl_state。后面的所有的控制都会订阅这个的，程序在这里，最新发布版本1.6.3里面 ekf2_main.cpp的第775行。

if (_ekf.update()) { // integrate time to monitor time slippage if (start_time_us == 0) { start_time_us = now; } else if (start_time_us > 0) { integrated_time_us += (uint64_t)((double)sensors.gyro_integral_dt * 1.0e6); } matrix::Quaternion<float> q; _ekf.copy_quaternion(q.data()); ……………… orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
到这里，传感器的数据到处理，到融合到发布，基本都说完了。现在的控制状态的数据是可以直接拿去做控制的了。
最后说一点，如果你是一般的无人机公司，一般研究到这个程度，侧重于调试，以及结合实际的应用场景更改完善飞机的逻辑，修改程序的bug，是完全够用了。如果你是一家比较大的无人机公司，想加点自己的传感器，也不是问题（直接uart，spi对接fmu，还不行，写写底层驱动也不是什么大事，各种协议实现都是通的），如果你想把传感器的数据推入到卡尔曼里面，一起融合，那就要研究的更深入了，当然了没有什么过不去的坑，只是精力和时间的分配是否值得的问题。
下一篇，跟随笔者一起研究下，pixhawk里面的几个关键dt，直接影响控制精度的。关于这个dt的概念，通过笔者下一篇的介绍，大家就可以知道，pixhawk能不能用来做火箭，做导弹（导引头），太空飞船……，我不太喜欢这些规则的约束，太累，说不准下一篇，说点别的，后会有期。

QQ群：323525681
添加微信拉你入群 EzailPH3 (验证暗号：我要飞飞机)