diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 0c5a07fb71..e8b3e4d384 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 42df23aa1d..807544b6ba 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/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; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index d5d72e5c0c..60aa4d17ae 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index eb2070b701..a2792f0618 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/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