From 616c40a827c73fbaf46b929309f67cdf0e39a83c Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Wed, 21 Jan 2015 19:44:01 -0800 Subject: [PATCH] Added clarifying comment and todo. --- src/main/flight/imu.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index cff9b315c..f0a4535a0 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -298,12 +298,17 @@ static void getEstimatedAttitude(void) 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) { 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; } int angle = lrintf(acosf(cosZ) * throttleAngleScale);