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.

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;

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];

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,