From b844b220c391ec5f8e2ef7491fa196a9237eda75 Mon Sep 17 00:00:00 2001 From: Gustavo Jose de Sousa Date: Fri, 28 Aug 2015 10:36:14 -0300 Subject: [PATCH] AP_InertialSensor: MPU9150: 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_MPU9150.cpp | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp index 7a31ff73ab..90095a8b04 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp @@ -1059,6 +1059,8 @@ void AP_InertialSensor_MPU9150::_accumulate(void) // read the samples for (uint16_t i=0; i< fifo_count; i++) { + Vector3f accel, gyro; + // read the data // TODO check whether it's possible to read all the packages in a single call hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_r_w, packet_size, data); @@ -1086,9 +1088,16 @@ void AP_InertialSensor_MPU9150::_accumulate(void) // TODO Revisit why AP_InertialSensor_L3G4200D uses a minus sign in the y and z component. Maybe this // is because the sensor is placed in the bottom side of the board? - _accel_filtered = _accel_filter.apply(Vector3f(accel_x, accel_y, accel_z)); - _gyro_filtered = _gyro_filter.apply(Vector3f(gyro_x, gyro_y, gyro_z)); + accel = Vector3f(accel_x, accel_y, accel_z); + accel *= MPU9150_ACCEL_SCALE_2G; + _rotate_and_correct_accel(_accel_instance, accel); + _accel_filtered = _accel_filter.apply(accel); + + gyro = Vector3f(gyro_x, gyro_y, gyro_z); + gyro *= MPU9150_GYRO_SCALE_2000; + _rotate_and_correct_gyro(_gyro_instance, gyro); + _gyro_filtered = _gyro_filter.apply(gyro); _have_sample_available = true; } @@ -1107,11 +1116,8 @@ bool AP_InertialSensor_MPU9150::update(void) _have_sample_available = false; hal.scheduler->resume_timer_procs(); - accel *= MPU9150_ACCEL_SCALE_2G; - _publish_accel(_accel_instance, accel); - - gyro *= MPU9150_GYRO_SCALE_2000; - _publish_gyro(_gyro_instance, gyro); + _publish_accel(_accel_instance, accel, false); + _publish_gyro(_gyro_instance, gyro, false); if (_last_accel_filter_hz != _accel_filter_cutoff()) { _set_accel_filter_frequency(_accel_filter_cutoff());