Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

NavEKF: use maxf when retrieving vibration levels 

This reduces the performance hit that was caused by multiple calls to get_vibration_levels
This commit is contained in:
Randy Mackay 2015-08-19 15:16:55 +09:00
parent 46c652e42f
commit ec2bc5e41d
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -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 // 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; 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 // read IMU2 delta velocity data
readDeltaVelocity(1, dVelIMU2, dtDelVel2); readDeltaVelocity(1, dVelIMU2, dtDelVel2);
// apply a peak hold 0.2 second time constant decaying envelope filter to the noise length on IMU2 // apply a peak hold 0.2 second time constant decaying envelope filter to the noise length on IMU2
alpha = 1.0f - 5.0f*dtDelVel2; 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 // calculate the filtered difference between acceleration vectors from IMU1 and 2
// apply a LPF filter with a 1.0 second time constant // apply a LPF filter with a 1.0 second time constant