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

Added clarifying comment and todo.

This commit is contained in:
Pierre Hugo 2015-01-21 19:44:01 -08:00
parent 29c5630957
commit 616c40a827
1 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/main/flight/imu.c
View File

@ -298,12 +298,17 @@ static void getEstimatedAttitude(void)
acc_calc(deltaT); // rotate acc vector into earth frame acc_calc(deltaT); // rotate acc vector into earth frame
} }
// correction of throttle in lateral wind, // Correction of throttle in lateral wind.
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
{ {
float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z); 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 /*
* Use 0 as the throttle angle correction if we are inverted, vertical or with a
* small angle < 0.86 deg
* TODO: Define this small angle in config.
*/
if (cosZ <= 0.015f) {
return 0; return 0;
} }
int angle = lrintf(acosf(cosZ) * throttleAngleScale); int angle = lrintf(acosf(cosZ) * throttleAngleScale);