From ec2bc5e41d134c2e265e9b95350e900273756b3e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 19 Aug 2015 15:16:55 +0900 Subject: [PATCH] NavEKF: use maxf when retrieving vibration levels This reduces the performance hit that was caused by multiple calls to get_vibration_levels --- libraries/AP_NavEKF/AP_NavEKF.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index a27780cde5..2080c448cd 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -4133,14 +4133,14 @@ void NavEKF::readIMUData() // apply a peak hold 0.2 second time constant decaying envelope filter to the noise length on IMU1 float alpha = 1.0f - 5.0f*dtDelVel1; - imuNoiseFiltState1 = max(ins.get_vibration_levels(0).length(), alpha*imuNoiseFiltState1); + imuNoiseFiltState1 = maxf(ins.get_vibration_levels(0).length(), alpha*imuNoiseFiltState1); // read IMU2 delta velocity data readDeltaVelocity(1, dVelIMU2, dtDelVel2); // apply a peak hold 0.2 second time constant decaying envelope filter to the noise length on IMU2 alpha = 1.0f - 5.0f*dtDelVel2; - imuNoiseFiltState2 = max(ins.get_vibration_levels(1).length(), alpha*imuNoiseFiltState2); + imuNoiseFiltState2 = maxf(ins.get_vibration_levels(1).length(), alpha*imuNoiseFiltState2); // calculate the filtered difference between acceleration vectors from IMU1 and 2 // apply a LPF filter with a 1.0 second time constant