PX4中IMU传感器的数据经过了哪些处理后被使用的？

注：所用PX4的代码版本为当前master最新版9e309f62a9b1731caae96000b824aa96661e67ad
2019年11月所写，新版本有所不同，尚未更新。

IMU的数据：加速度、角速度发布后被两个地方所使用：1，导航算法；2，控制算法

以加速度数据为例说明IMU到导航算法ekf2经过了哪些处理

ekf2使用了sensor_combined中的数据进行导航解算。

此主题在src/modules/sensors/sensors.cpp中publish。

_sensor_pub.publish(raw);

以mpu6000中的加速度数据为例

1， 启动传感器

boards/px4/fmu-v5/init/rc.board_sensors

mpu6000的start

mpu6000 -R 12 -S -T 20689 start

导入了rotation，记这里的rotation为（rotation_R1）

src/drivers/imu/mpu6000/MPU6000.cpp

2 ，mpu6000把rotation传给内部的类_px4_accel

.

_px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation),

src/drivers/imu/mpu6000/MPU6000.cpp

3， 首先按照rotation进行数据旋转

   // Apply rotation (before scaling)

   rotate_3f(_rotation, x, y, z);

记这里的rotation为（rotation_R1）

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

4，然后应用范围缩放系数和校准的系数和偏移

   // Apply range scale and the calibrating offset/scale

   const matrix::Vector3f val_calibrated{(((raw * report.scaling) - _calibration_offset).emult(_calibration_scale))};

这里的_calibration_scale和_calibration_offset就是应用加速度校准的旋转和温度校准的混合数据，记这里的rotation为（rotation_R2）；

未经校准之前scale都是1，offset都是0。

那种外置的单独的imu相对于FMU板的安装方位都在start的时候自己设置好（即是地1步中的 -R选项）。先跟FMU板保持一致，再应用板子的旋转（记板子的rotation为（rotation_R3））。

校准的数据从哪里来的呢：

PX4Accelerometer::ioctl(ACCELIOCSSCALE,……)传入accel_calibration_s类型的数据

而在VotedSensorsUpdate.cpp中应用校准VotedSensorsUpdate::applyAccelCalibration:

h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal);

而调用校准的函数是VotedSensorsUpdate::parametersUpdate()（它又被函数Sensors::parametersUpdate()调用→Sensors::parameter_update_poll(bool forced)->Sensors::run() ）中将参数：

CAL_ACC%u_ZSCALE和CAL_ACC%u_ZOFF等写入ascale结构体中，然后调用。

所以校准过的参数数据被sensors.cpp进行参数更新，输入到传感器驱动中。

5，应用低通滤波器LowPassFilter2pVector3f，参数：IMU_ACCEL_CUTOFF

   // Filtered values

   const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)};

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

这里的滤波器是src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp，所利用的参数就是IMU_ACCEL_CUTOFF

//PX4Accelerometer类初始化的时候：

 configure_filter(_param_imu_accel_cutoff.get());

6，给数据积分_integrator.put(timestamp, val_calibrated, integrated_value, integral_dt)

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

传感器的采集频率是1000hz，但是ekf2使用数据的频率只有250hz。出4个数才用1个数，浪费。PX4用的方法是将采集到的数据进行积分，4000us后输出acc_integral到结构体中。

sensor_combined 与 sensor_accel_数字是大体一致的。在敲击情况下sensor_combined 表现的更加尖锐，而这是更加真实的数据。

而角速度的积分后再微分就是与原数据相同。

7，publish(ORB_ID(sensor_accel) )数据_sensor_accel_pub.update()

sensor_accel中的x_raw、y_raw、z_raw是加速度计输出的原始整形数据，只经过了传感器本身的_R旋转。而其中的x，y，z是经过校准和滤波的数据

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

8，传感器数据表决选取

在accel_poll()函数中

src/modules/sensors/parameters.cpp中更新了参数：SENS_BOARD_X_OFF、SENS_BOARD_Y_OFF、SENS_BOARD_Z_OFF到board_offset[]数组中。

然后在VotedSensorsUpdate::parametersUpdate()中把offset的旋转矩阵R3与板子的安装方向相乘得到rotation_RR

在VotedSensorsUpdate::accelPoll(struct
sensor_combined_s
&raw)中获取到sensor_accel主题的数据后与rotation_RR相乘，输送给表决器。

在Sensors::run()中poll各种传感器的数据，

VotedSensorsUpdate::sensorsPoll()

调用accelPoll()，其中再调用get_best()函数获得最好的传感器数据，并publish（发布 sensor_combined_s类型的数据。

9，ekf2中获取 sensor_combined_s类型的数据进行计算

角速度数据到ekf2的路径类似加速度，不再赘述。

IMU到多旋翼角速度控制算法经过了哪些处理
PX4的控制算法中只用到了角速度数据，所以只传入gyro数据。

MulticopterAttitudeControl::control_attitude_rates()

而uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this,
ORB_ID(vehicle_angular_velocity)};

所以角速度控制器中使用的是vehicle_angular_velocity主题的角速度

继续寻找到src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

看里面的VehicleAngularVelocity::Run()函数

SensorCorrectionsUpdate()

更新每个传感器的_sensor_correction_sub，包括offset和scale，构成传感器的board_rotation_offset

然后再与_param_sens_board_rot形成的 board_rotation相乘，得到了_board_rotation

sensor_correction中的数据应该是同参数相同的。

如果_sensor_control_available

获取sensor_gyro_control_s主题的数据

rates{(Vector3f{sensor_data.xyz} - _offset).emult(_scale)};

_board_rotation * rates;

rates -= _bias;来自于_sensor_bias_sub{ORB_ID(sensor_bias)}

给到vehicle_angular_velocity_s并publish。

否则

rates{(val - _offset).emult(_scale)};correction的

_board_rotation * rates;

rates -= _bias;来自于_sensor_bias_sub{ORB_ID(sensor_bias)}

给到vehicle_angular_velocity_s并publish。

sensor_bias是src/modules/ekf2/ekf2_main.cpp发布的。这个就不了解了。。。

也就是说，得到数据后先进行offset和scale的校准，然后应用rotation，再减去运行中ekf2计算出的bias，然后就可以发布了。不同之处是数据来源。第二种好说，传感器数据。

我们再来看第一种sensor_gyro_control_s从哪里来的

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)

从sensor_gyro_s主题获取数据，是原始整形数据。

然后应用rotation、offset和scale的校准、滤波器滤波（LowPassFilter2pVector3f，参数IMU_GYRO_CUTOFF）。

然后根据参数IMU_GYRO_RATEMAX决定的时间间隔进行publish。

10 ，总结

这个数据与送给ekf2的数据的相同点和不同点：

以下2021年12月22日更新