From d6b08f22c1c966c78402f018f6284d2d6560f515 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Wed, 21 Jan 2015 19:28:05 -0800 Subject: [PATCH] Added clarifying comments to calculateHeading body. --- src/main/flight/imu.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index d3f7a1866b..e363a2f97e 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -264,8 +264,12 @@ static int16_t calculateHeading(t_fp_vector *vec) float sinePitch = sinf(anglerad[AI_PITCH]); float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll; float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll; + //TODO: Replace this comment with an explanation of why Yh and Xh can never simultanoeusly be zero, + // or handle the case in which they are and (atan2f(0, 0) is undefined. float hd = (atan2f(Yh, Xh) * 1800.0f / M_PIf + magneticDeclination) / 10.0f; head = lrintf(hd); + + // Arctan returns a value in the range -180 to 180 degrees. We 'normalize' negative angles to be positive. if (head < 0) head += 360;