From 75fdac648fb641ef2bc1ca63fc4b2d5fbd452a4b Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Tue, 8 Sep 2015 11:42:28 -0300
Subject: [PATCH] AP_InertialSensor: unify delta velocity calculation

This commit basically moves delta velocity calculation that was previously done
in AP_InertialSensor_PX4 to a common place. Instances must publish their accel
raw sample rate to enable delta velocity calculation.
---
 .../AP_InertialSensor/AP_InertialSensor.cpp   |  9 +++++++
 .../AP_InertialSensor/AP_InertialSensor.h     |  4 +++
 .../AP_InertialSensor_Backend.cpp             | 26 ++++++++++++++++---
 3 files changed, 35 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index b004e3e0e8..0c24274f23 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -338,6 +338,9 @@ AP_InertialSensor::AP_InertialSensor() :
 
         _accel_max_abs_offsets[i] = 3.5f;
         _accel_sample_rates[i] = 0;
+
+        _delta_velocity_acc[i].zero();
+        _delta_velocity_acc_dt[i] = 0;
     }
     for (uint8_t i=0; i<INS_VIBRATION_CHECK_INSTANCES; i++) {
         _accel_vibe_floor_filter[i].set_cutoff_frequency(AP_INERTIAL_SENSOR_ACCEL_VIBE_FLOOR_FILT_HZ);
@@ -1263,6 +1266,12 @@ void AP_InertialSensor::update(void)
             _backends[i]->update();
         }
 
+        // clear accumulators
+        for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
+            _delta_velocity_acc[i].zero();
+            _delta_velocity_acc_dt[i] = 0;
+        }
+
         // adjust health status if a sensor has a non-zero error count
         // but another sensor doesn't. 
         bool have_zero_accel_error_count = false;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 7ce46fa027..78a706d635 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -276,6 +276,10 @@ private:
     Vector3f _delta_velocity[INS_MAX_INSTANCES];
     float _delta_velocity_dt[INS_MAX_INSTANCES];
     bool _delta_velocity_valid[INS_MAX_INSTANCES];
+    // delta velocity accumulator
+    Vector3f _delta_velocity_acc[INS_MAX_INSTANCES];
+    // time accumulator for delta velocity accumulator
+    float _delta_velocity_acc_dt[INS_MAX_INSTANCES];
 
     // Most recent gyro reading
     Vector3f _gyro[INS_MAX_INSTANCES];
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
index 07bbf68f6b..699bedc09c 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -68,17 +68,35 @@ void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f
 {
     _imu._accel[instance] = accel;
     _imu._accel_healthy[instance] = true;
+
+    if (_imu._accel_sample_rates[instance] <= 0) {
+        return;
+    }
+
+    // publish delta velocity
+    _imu._delta_velocity[instance] = _imu._delta_velocity_acc[instance];
+    _imu._delta_velocity_dt[instance] = _imu._delta_velocity_acc_dt[instance];
+    _imu._delta_velocity_valid[instance] = true;
 }
 
 void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
                                                              const Vector3f &accel)
 {
-#if INS_VIBRATION_CHECK
-    if (_imu._accel_sample_rates[instance] > 0) {
-        float dt = 1.0f / _imu._accel_sample_rates[instance];
-        _imu.calc_vibration_and_clipping(instance, accel, dt);
+    float dt;
+
+    if (_imu._accel_sample_rates[instance] <= 0) {
+        return;
     }
+
+    dt = 1.0f / _imu._accel_sample_rates[instance];
+
+#if INS_VIBRATION_CHECK
+    _imu.calc_vibration_and_clipping(instance, accel, dt);
 #endif
+
+    // delta velocity
+    _imu._delta_velocity_acc[instance] += accel * dt;
+    _imu._delta_velocity_acc_dt[instance] += dt;
 }
 
 void AP_InertialSensor_Backend::_set_accel_max_abs_offset(uint8_t instance,