Added clarifying comments to calculateHeading body.
This commit is contained in:
parent
d1a1cc3f13
commit
d6b08f22c1
|
@ -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;
|
||||
|
||||
|
|
Loading…
Reference in New Issue