From f769a39449ef4366f342e52719520d1ecd3e71b7 Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Tue, 8 Sep 2015 11:37:36 -0300
Subject: [PATCH] AP_InertialSensor: PX4: don't calculate delta velocity

Delta velocity calculation will be unified.
---
 .../AP_InertialSensor_PX4.cpp                  | 18 +++---------------
 .../AP_InertialSensor/AP_InertialSensor_PX4.h  |  2 --
 2 files changed, 3 insertions(+), 17 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
index 873090e33b..4f54e2d0e3 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
@@ -29,8 +29,6 @@ AP_InertialSensor_PX4::AP_InertialSensor_PX4(AP_InertialSensor &imu) :
 {
     for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
         _delta_angle_accumulator[i].zero();
-        _delta_velocity_accumulator[i].zero();
-        _delta_velocity_dt[i] = 0.0f;
     }
 }
 
@@ -218,7 +216,6 @@ bool AP_InertialSensor_PX4::update(void)
         // so we only want to do this if we have new data from it
         if (_last_accel_timestamp[k] != _last_accel_update_timestamp[k]) {
             _publish_accel(_accel_instance[k], accel);
-            _publish_delta_velocity(_accel_instance[k], _delta_velocity_accumulator[k], _delta_velocity_dt[k]);
             _last_accel_update_timestamp[k] = _last_accel_timestamp[k];
         }
     }
@@ -236,8 +233,6 @@ bool AP_InertialSensor_PX4::update(void)
 
     for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
         _delta_angle_accumulator[i].zero();
-        _delta_velocity_accumulator[i].zero();
-        _delta_velocity_dt[i] = 0.0f;
     }
 
     if (_last_accel_filter_hz != _accel_filter_cutoff()) {
@@ -265,16 +260,6 @@ void AP_InertialSensor_PX4::_new_accel_sample(uint8_t i, accel_report &accel_rep
     // apply filter for control path
     _accel_in[i] = _accel_filter[i].apply(accel);
 
-    // get time since last sample
-    float dt = _accel_sample_time[i];
-
-    // compute delta velocity
-    Vector3f delVel = Vector3f(accel.x, accel.y, accel.z) * dt;
-
-    // integrate delta velocity accumulator
-    _delta_velocity_accumulator[i] += delVel;
-    _delta_velocity_dt[i] += dt;
-
     // save last timestamp
     _last_accel_timestamp[i] = accel_report.timestamp;
 
@@ -285,6 +270,9 @@ void AP_InertialSensor_PX4::_new_accel_sample(uint8_t i, accel_report &accel_rep
     _publish_temperature(frontend_instance, accel_report.temperature);
 
 #ifdef AP_INERTIALSENSOR_PX4_DEBUG
+    // get time since last sample
+    float dt = _accel_sample_time[i];
+
     _accel_dt_max[i] = max(_accel_dt_max[i],dt);
 
     _accel_meas_count[i] ++;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
index 879711c10b..d5bd1b0835 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
@@ -79,8 +79,6 @@ private:
     LowPassFilter2pVector3f _gyro_filter[INS_MAX_INSTANCES];
 
     Vector3f _delta_angle_accumulator[INS_MAX_INSTANCES];
-    Vector3f _delta_velocity_accumulator[INS_MAX_INSTANCES];
-    float _delta_velocity_dt[INS_MAX_INSTANCES];
     Vector3f _last_delAng[INS_MAX_INSTANCES];
     Vector3f _last_gyro[INS_MAX_INSTANCES];