From 2ea8f1de3f9c071f808ddd79d58d94090f19d18b Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Fri, 28 Aug 2015 10:35:35 -0300
Subject: [PATCH] AP_InertialSensor: L3G4200D: 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_L3G4200D.cpp            | 21 +++++++++++--------
 1 file changed, 12 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
index c001a8c3d6..967980da1f 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
@@ -271,13 +271,8 @@ bool AP_InertialSensor_L3G4200D::update(void)
     _have_sample = false;
     pthread_spin_unlock(&_data_lock);
 
-    // Adjust for chip scaling to get m/s/s
-    accel *= ADXL345_ACCELEROMETER_SCALE_M_S;
-    _publish_accel(_accel_instance, accel);
-
-    // Adjust for chip scaling to get radians/sec
-    gyro *= L3G4200D_GYRO_SCALE_R_S;
-    _publish_gyro(_gyro_instance, gyro);
+    _publish_accel(_accel_instance, accel, false);
+    _publish_gyro(_gyro_instance, gyro, false);
 
     if (_last_filter_hz != _accel_filter_cutoff()) {
         _set_filter_frequency(_accel_filter_cutoff());
@@ -322,7 +317,11 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
         if (hal.i2c->readRegisters(L3G4200D_I2C_ADDRESS, L3G4200D_REG_XL | L3G4200D_REG_AUTO_INCREMENT, 
                                    sizeof(buffer), (uint8_t *)&buffer[0][0]) == 0) {
             for (uint8_t i=0; i<num_samples_available; i++) {
-                _data[_data_idx].gyro_filtered = _gyro_filter.apply(Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]));
+                Vector3f gyro = Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]);
+                // Adjust for chip scaling to get radians/sec
+                gyro *= L3G4200D_GYRO_SCALE_R_S;
+                _rotate_and_correct_gyro(_gyro_instance, gyro);
+                _data[_data_idx].gyro_filtered = _gyro_filter.apply(gyro);
                 _have_gyro_sample = true;
             }
         }
@@ -342,7 +341,11 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
                                            sizeof(buffer[0]), num_samples_available,
                                            (uint8_t *)&buffer[0][0]) == 0) {
             for (uint8_t i=0; i<num_samples_available; i++) {
-                _data[_data_idx].accel_filtered = _accel_filter.apply(Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]));
+                Vector3f accel = Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]);
+                // Adjust for chip scaling to get m/s/s
+                accel *= ADXL345_ACCELEROMETER_SCALE_M_S;
+                _rotate_and_correct_accel(_accel_instance, accel);
+                _data[_data_idx].accel_filtered = _accel_filter.apply(accel);
                 _have_accel_sample = true;
             }
         }