AP_InertialSensor: publish delta_velocity_dt

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -125,9 +125,13 @@ public:
         if(_delta_velocity_valid[i]) delta_velocity = _delta_velocity[i];
         return _delta_velocity_valid[i];
     }
-
     bool get_delta_velocity(Vector3f &delta_velocity) const { return get_delta_velocity(_primary_accel, delta_velocity); }
 
+    float get_delta_velocity_dt(uint8_t i) const {
+        return _delta_velocity_dt[i];
+    }
+    float get_delta_velocity() const { return get_delta_velocity_dt(_primary_accel); }
+
     /// Fetch the current accelerometer values
     ///
     /// @returns	vector of current accelerations in m/s/s
@@ -250,6 +254,7 @@ private:
     // Most recent accelerometer reading
     Vector3f _accel[INS_MAX_INSTANCES];
     Vector3f _delta_velocity[INS_MAX_INSTANCES];
+    float _delta_velocity_dt[INS_MAX_INSTANCES];
     bool _delta_velocity_valid[INS_MAX_INSTANCES];
 
     // Most recent gyro reading

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -57,10 +57,11 @@ void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &
     }
 }
 
-void AP_InertialSensor_Backend::_publish_delta_velocity(uint8_t instance, const Vector3f &delta_velocity)
+void AP_InertialSensor_Backend::_publish_delta_velocity(uint8_t instance, const Vector3f &delta_velocity, float dt)
 {
     // publish delta velocity
     _imu._delta_velocity[instance] = delta_velocity;
+    _imu._delta_velocity_dt[instance] = dt;
     _imu._delta_velocity_valid[instance] = true;
 }
 

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -64,7 +64,7 @@ protected:
     void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel);
     void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro);
 
-    void _publish_delta_velocity(uint8_t instance, const Vector3f &delta_velocity);
+    void _publish_delta_velocity(uint8_t instance, const Vector3f &delta_velocity, float dt);
     void _publish_delta_angle(uint8_t instance, const Vector3f &delta_angle);
 
     // rotate gyro vector, offset and publish

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -28,6 +28,7 @@ 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;
     }
 }
 
@@ -210,7 +211,7 @@ 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, false);
-            _publish_delta_velocity(_accel_instance[k], _delta_velocity_accumulator[k]);
+            _publish_delta_velocity(_accel_instance[k], _delta_velocity_accumulator[k], _delta_velocity_dt[k]);
             _last_accel_update_timestamp[k] = _last_accel_timestamp[k];
         }
     }
@@ -229,6 +230,7 @@ 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()) {
@@ -263,6 +265,7 @@ void AP_InertialSensor_PX4::_new_accel_sample(uint8_t i, accel_report &accel_rep
 
     // integrate delta velocity accumulator
     _delta_velocity_accumulator[i] += delVel;
+    _delta_velocity_dt[i] += dt;
 
     // save last timestamp
     _last_accel_timestamp[i] = accel_report.timestamp;

libraries/AP_InertialSensor/AP_InertialSensor_PX4.h

@@ -78,6 +78,7 @@ private:
 
     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];