Subversion Repositories MK3Mag

Compare Revisions

Ignore whitespace Rev 19 → Rev 20

/branches/MK3Mag V0.14 Code Redesign Killagreg/main.c
106,14 → 106,12
MagnetZ = (1024L * (int32_t)(UncalMagnetZ - Calibration.Z.Offset)) / (Calibration.Z.Range);
}
 
 
void CalcHeading(void)
{
double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz;
int16_t heading = -1;
 
// calculate nick and roll angle i rad
nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800);
roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800);
 
Cx = MagnetX;
Cy = MagnetY;
126,22 → 124,22
Cz = MagnetZ;
}
 
Hx = Cx * (double)cos(nick_rad) +
Cy * (double)sin(nick_rad) * (double)sin(roll_rad) -
Cz * (double)sin(nick_rad) * (double)cos(roll_rad);
// calculate nick and roll angle in rad
nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800.0);
roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800.0);
// calculate attitude correction
Hx = Cx * (double)cos(nick_rad) - Cz * (double)sin(nick_rad);
Hy = Cy * (double)cos(roll_rad) + Cz * (double)sin(roll_rad);
 
Hy = Cy * (double)cos(roll_rad) +
Cz * (double)sin(roll_rad);
// calculate Heading
heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI);
// atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation
// but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
if (heading < 0) heading = -heading;
else heading = 360 - heading;
 
 
if(Hx == 0 && Hy < 0) heading = 90;
else if(Hx == 0 && Hy > 0) heading = 270;
else if(Hx < 0) heading = 180 - (atan(Hy / Hx) * 180.0) / M_PI;
else if(Hx > 0 && Hy < 0) heading = - (atan(Hy / Hx) * 180.0) / M_PI;
else if(Hx > 0 && Hy > 0) heading = 360 - (atan(Hy / Hx) * 180.0) / M_PI;
 
if(abs(heading) < 361) Heading = heading;
 
else (Heading = -1);
}