diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-03-15 17:26:11 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-03-15 17:26:11 +0100 |
commit | 92a52ea1953e0e17e7a55512b484adf7048e619c (patch) | |
tree | 87728d9b48279b5d0d033f1bf3409f460b24f1c1 /src/drivers | |
parent | 3436f089768c31f0d925faf36b41626b78ddb6f5 (diff) | |
download | px4-firmware-92a52ea1953e0e17e7a55512b484adf7048e619c.tar.gz px4-firmware-92a52ea1953e0e17e7a55512b484adf7048e619c.tar.bz2 px4-firmware-92a52ea1953e0e17e7a55512b484adf7048e619c.zip |
MPU6000 driver: Rotate before applying offsets.
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 32 |
1 files changed, 20 insertions, 12 deletions
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index b48ea8577..eaf25a9f3 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1711,17 +1711,21 @@ MPU6000::measure() arb.y_raw = report.accel_y; 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 y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float xraw_f = report.accel_x; + float yraw_f = report.accel_y; + 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.y = _accel_filter_y.apply(y_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.range_m_s2 = _accel_range_m_s2; @@ -1732,17 +1736,21 @@ MPU6000::measure() grb.y_raw = report.gyro_y; 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; - float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + xraw_f = report.gyro_x; + yraw_f = report.gyro_y; + 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.y = _gyro_filter_y.apply(y_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.range_rad_s = _gyro_range_rad_s; |