Browse Source

AP_NavEKF: use MAX() instead of maxf()

master
Lucas De Marchi 9 years ago
parent
commit
798b743660
  1. 4
      libraries/AP_NavEKF/AP_NavEKF_core.cpp

4
libraries/AP_NavEKF/AP_NavEKF_core.cpp

@ -3841,11 +3841,11 @@ void NavEKF_core::readIMUData() @@ -3841,11 +3841,11 @@ void NavEKF_core::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 = maxf(ins.get_vibration_levels(0).length(), alpha*imuNoiseFiltState1);
imuNoiseFiltState1 = MAX(ins.get_vibration_levels(0).length(), alpha*imuNoiseFiltState1);
// 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 = maxf(ins.get_vibration_levels(1).length(), alpha*imuNoiseFiltState2);
imuNoiseFiltState2 = MAX(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

Loading…
Cancel
Save