From fc38691e0b153ed91fea95bc62d33430737e3a70 Mon Sep 17 00:00:00 2001 From: Gustavo Jose de Sousa Date: Fri, 28 Aug 2015 10:36:20 -0300 Subject: [PATCH] AP_InertialSensor: MPU9250: apply correction on each new sample These changes are for enabling unified accelerometer vibration and clipping calculation. For that, we need the values "rotated and corrected" before they are filtered and the calculation must be called as soon as a new sample arrives as it takes the sample rate into account. Thus, move code that applies "corrections" to be executed as soon as accel data arrive and call _publish_accel() passing rotate_and_correct parameter as false. Also, do the same for gyro so we can keep it consistent. --- .../AP_InertialSensor_MPU9250.cpp | 36 ++++++++++--------- 1 file changed, 20 insertions(+), 16 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index b870131576..9a70c2fa4a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -330,14 +330,8 @@ bool AP_InertialSensor_MPU9250::update( void ) _have_sample_available = false; - accel *= MPU9250_ACCEL_SCALE_1G; - gyro *= GYRO_SCALE; - - accel.rotate(_default_rotation); - gyro.rotate(_default_rotation); - - _publish_gyro(_gyro_instance, gyro); - _publish_accel(_accel_instance, accel); + _publish_gyro(_gyro_instance, gyro, false); + _publish_accel(_accel_instance, accel, false); if (_last_accel_filter_hz != _accel_filter_cutoff()) { _set_accel_filter(_accel_filter_cutoff()); @@ -385,21 +379,31 @@ void AP_InertialSensor_MPU9250::_read_data_transaction() uint8_t v[14]; } rx, tx = { cmd : MPUREG_INT_STATUS | 0x80, }; + Vector3f accel, gyro; + _spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx)); #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) - Vector3f _accel_filtered = _accel_filter.apply(Vector3f(int16_val(rx.v, 1), - int16_val(rx.v, 0), - -int16_val(rx.v, 2))); + accel = Vector3f(int16_val(rx.v, 1), + int16_val(rx.v, 0), + -int16_val(rx.v, 2)); + accel *= MPU9250_ACCEL_SCALE_1G; + accel.rotate(_default_rotation); + _rotate_and_correct_accel(_accel_instance, accel); + + gyro = Vector3f(int16_val(rx.v, 5), + int16_val(rx.v, 4), + -int16_val(rx.v, 6)); + gyro *= GYRO_SCALE; + gyro.rotate(_default_rotation); + _rotate_and_correct_gyro(_gyro_instance, gyro); + - Vector3f _gyro_filtered = _gyro_filter.apply(Vector3f(int16_val(rx.v, 5), - int16_val(rx.v, 4), - -int16_val(rx.v, 6))); // update the shared buffer uint8_t idx = _shared_data_idx ^ 1; - _shared_data[idx]._accel_filtered = _accel_filtered; - _shared_data[idx]._gyro_filtered = _gyro_filtered; + _shared_data[idx]._accel_filtered = _accel_filter.apply(accel); + _shared_data[idx]._gyro_filtered = _gyro_filter.apply(gyro); _shared_data_idx = idx; _have_sample_available = true;