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.

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;