diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 3020004c88..3c2c967f77 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -518,6 +518,12 @@ void AP_AHRS::update_nmea_out() #endif } +// return current vibration vector for primary IMU +Vector3f AP_AHRS::get_vibration(void) const +{ + return AP::ins().get_vibration_levels(); +} + // singleton instance AP_AHRS *AP_AHRS::_singleton; diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 63a404d380..22da4dd995 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -550,6 +550,9 @@ public: // Write position and quaternion data from an external navigation system virtual void writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms) { } + // return current vibration vector for primary IMU + Vector3f get_vibration(void) const; + // allow threads to lock against AHRS update HAL_Semaphore &get_semaphore(void) { return _rsem;