fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 00:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

df_mpu9250_wrapper: calibration after rotation

Browse Source 
Same as for the df_hmc5883_wrapper.
This commit is contained in:
Julian Oes
2016-08-06 21:01:37 +02:00
committed by Lorenz Meier
parent 1dd2c94949
commit 68986604e9
1 changed files with 8 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+8 -3
View File
@@ -588,13 +588,18 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
vec_integrated_unused, vec_integrated_unused,
integral_dt_unused); integral_dt_unused);
math::Vector<3> gyro_val((data.gyro_rad_s_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale, math::Vector<3> gyro_val(data.gyro_rad_s_x,
(data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale, data.gyro_rad_s_y,
(data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale); data.gyro_rad_s_z);
// apply sensor rotation on the gyro measurement // apply sensor rotation on the gyro measurement
gyro_val = _rotation_matrix * gyro_val; gyro_val = _rotation_matrix * gyro_val;
// Apply calibration after rotation.
gyro_val(0) = (gyro_val(0) - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
gyro_val(1) = (gyro_val(1) - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
gyro_val(2) = (gyro_val(2) - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
_gyro_int.put_with_interval(data.fifo_sample_interval_us, _gyro_int.put_with_interval(data.fifo_sample_interval_us,
gyro_val, gyro_val,
vec_integrated_unused, vec_integrated_unused,