rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

faster cast with lrintf

This commit is contained in:
treymarc 2014-04-12 09:48:32 +00:00
parent 7fca3d17a2
commit 39f3874661
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/imu.c
View File

@ -307,13 +307,13 @@ static void getEstimatedAttitude(void)
float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
if (cosZ <= 0.015f) { // // we are inverted, vertical or with a small angle < 0.86 deg
if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg
throttleAngleCorrection = 0;
} else {
int angle = acosf(cosZ) * throttleAngleScale;
int angle = lrintf(acosf(cosZ) * throttleAngleScale);
if (angle > 900)
angle = 900;
throttleAngleCorrection = cfg.throttle_correction_value * sinf(angle / 900.0f * M_PI / 2.0f) ;
throttleAngleCorrection = lrintf(cfg.throttle_correction_value * sinf(angle / 900.0f * M_PI / 2.0f)) ;
}
}