float calcheading(HIL.Vector3 mag)
{
HIL.Matrix3 dcm_matrix = new HIL.Matrix3();
dcm_matrix.from_euler(0, 0, 0);
// Tilt compensated magnetic field Y component:
double headY = mag.y * dcm_matrix.c.z - mag.z * dcm_matrix.c.y;
// Tilt compensated magnetic field X component:
double headX = mag.x + dcm_matrix.c.x * (headY - mag.x * dcm_matrix.c.x);
// magnetic heading
// 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
double heading = constrain_float((float)Math.Atan2(-headY, headX), -3.15f, 3.15f);
return((float)((heading * MathHelper.rad2deg) + 360) % 360f);
}
float calcheading(HIL.Vector3 mag)
{
HIL.Matrix3 dcm_matrix = new HIL.Matrix3();
dcm_matrix.from_euler(0, 0, 0);
// Tilt compensated magnetic field Y component:
double headY = mag.y * dcm_matrix.c.z - mag.z * dcm_matrix.c.y;
// Tilt compensated magnetic field X component:
double headX = mag.x + dcm_matrix.c.x * (headY - mag.x * dcm_matrix.c.x);
// magnetic heading
// 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
double heading = constrain_float((float)Math.Atan2(-headY, headX), -3.15f, 3.15f);
return (float)((heading * rad2deg) + 360) % 360f;
}