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); |
} |
|
|