forked from Archive/PX4-Autopilot
MPU6000 driver: Rotate before applying offsets.
This commit is contained in:
parent
3436f08976
commit
92a52ea195
|
@ -1711,17 +1711,21 @@ MPU6000::measure()
|
||||||
arb.y_raw = report.accel_y;
|
arb.y_raw = report.accel_y;
|
||||||
arb.z_raw = report.accel_z;
|
arb.z_raw = report.accel_z;
|
||||||
|
|
||||||
float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
|
float xraw_f = report.accel_x;
|
||||||
float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
|
float yraw_f = report.accel_y;
|
||||||
float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
|
float zraw_f = report.accel_z;
|
||||||
|
|
||||||
|
// apply user specified rotation
|
||||||
|
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
|
||||||
|
|
||||||
|
float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
|
||||||
|
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
|
||||||
|
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
|
||||||
|
|
||||||
arb.x = _accel_filter_x.apply(x_in_new);
|
arb.x = _accel_filter_x.apply(x_in_new);
|
||||||
arb.y = _accel_filter_y.apply(y_in_new);
|
arb.y = _accel_filter_y.apply(y_in_new);
|
||||||
arb.z = _accel_filter_z.apply(z_in_new);
|
arb.z = _accel_filter_z.apply(z_in_new);
|
||||||
|
|
||||||
// apply user specified rotation
|
|
||||||
rotate_3f(_rotation, arb.x, arb.y, arb.z);
|
|
||||||
|
|
||||||
arb.scaling = _accel_range_scale;
|
arb.scaling = _accel_range_scale;
|
||||||
arb.range_m_s2 = _accel_range_m_s2;
|
arb.range_m_s2 = _accel_range_m_s2;
|
||||||
|
|
||||||
|
@ -1732,17 +1736,21 @@ MPU6000::measure()
|
||||||
grb.y_raw = report.gyro_y;
|
grb.y_raw = report.gyro_y;
|
||||||
grb.z_raw = report.gyro_z;
|
grb.z_raw = report.gyro_z;
|
||||||
|
|
||||||
float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
|
xraw_f = report.gyro_x;
|
||||||
float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
|
yraw_f = report.gyro_y;
|
||||||
float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
|
zraw_f = report.gyro_z;
|
||||||
|
|
||||||
|
// apply user specified rotation
|
||||||
|
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
|
||||||
|
|
||||||
|
float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
|
||||||
|
float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
|
||||||
|
float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
|
||||||
|
|
||||||
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
|
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
|
||||||
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
|
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
|
||||||
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
|
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
|
||||||
|
|
||||||
// apply user specified rotation
|
|
||||||
rotate_3f(_rotation, grb.x, grb.y, grb.z);
|
|
||||||
|
|
||||||
grb.scaling = _gyro_range_scale;
|
grb.scaling = _gyro_range_scale;
|
||||||
grb.range_rad_s = _gyro_range_rad_s;
|
grb.range_rad_s = _gyro_range_rad_s;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue