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

df_lsm9ds1_wrapper: apply calibration after rotation

Browse Source
This commit is contained in:
lhc610github
2016-11-24 14:39:23 +01:00
committed by Beat Küng
parent afd6fd3896
commit f559b6ca88
1 changed files with 20 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp
+20 -18
View File
@@ -146,7 +146,6 @@ private:
} _mag_calibration; } _mag_calibration;
math::Matrix<3, 3> _rotation_matrix; math::Matrix<3, 3> _rotation_matrix;
int _accel_orb_class_instance; int _accel_orb_class_instance;
int _gyro_orb_class_instance; int _gyro_orb_class_instance;
int _mag_orb_class_instance; int _mag_orb_class_instance;
@@ -572,26 +571,28 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
math::Vector<3> vec_integrated_unused; math::Vector<3> vec_integrated_unused;
uint64_t integral_dt_unused; uint64_t integral_dt_unused;
math::Vector<3> accel_val(data.accel_m_s2_x,
math::Vector<3> accel_val((data.accel_m_s2_x - _accel_calibration.x_offset) * _accel_calibration.x_scale, data.accel_m_s2_y,
(data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale, data.accel_m_s2_z);
(data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale);
// apply sensor rotation on the accel measurement // apply sensor rotation on the accel measurement
accel_val = _rotation_matrix * accel_val; accel_val = _rotation_matrix * accel_val;
// Apply calibration after rotation
accel_val(0) = (accel_val(0) - _accel_calibration.x_offset) * _accel_calibration.x_scale;
accel_val(1) = (accel_val(1) - _accel_calibration.y_offset) * _accel_calibration.y_scale;
accel_val(2) = (accel_val(2) - _accel_calibration.z_offset) * _accel_calibration.z_scale;
_accel_int.put_with_interval(data.fifo_sample_interval_us, _accel_int.put_with_interval(data.fifo_sample_interval_us,
accel_val, accel_val,
vec_integrated_unused, vec_integrated_unused,
integral_dt_unused); integral_dt_unused);
math::Vector<3> gyro_val(data.gyro_rad_s_x,
math::Vector<3> gyro_val((data.gyro_rad_s_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale, data.gyro_rad_s_y,
(data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale, data.gyro_rad_s_z);
(data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale);
// 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,
@@ -671,12 +672,13 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
accel_report.z = accel_val_filt(2); accel_report.z = accel_val_filt(2);
if (_mag_enabled) { if (_mag_enabled) {
math::Vector<3> mag_val(data.mag_ga_x,
math::Vector<3> mag_val((data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale, data.mag_ga_y,
(data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale, data.mag_ga_z);
(data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale);
mag_val = _rotation_matrix * mag_val; mag_val = _rotation_matrix * mag_val;
mag_val(0) = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale;
mag_val(1) = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale;
mag_val(2) = (mag_val(2) - _mag_calibration.z_offset) * _mag_calibration.z_scale;
mag_report.x = mag_val(0); mag_report.x = mag_val(0);
mag_report.y = mag_val(1); mag_report.y = mag_val(1);