100,9 → 100,9 |
|
void CalcFields(void) |
{ |
UncalMagnetX = (1 * UncalMagnetX + (RawMagnet1a - RawMagnet1b)) / 2; |
UncalMagnetY = (1 * UncalMagnetY + (RawMagnet3a - RawMagnet3b)) / 2; |
UncalMagnetZ = (1 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 2; |
UncalMagnetX = (RawMagnet1a - RawMagnet1b) / 2; |
UncalMagnetY = (RawMagnet3a - RawMagnet3b) / 2; |
UncalMagnetZ = (RawMagnet2a - RawMagnet2b) / 2; |
|
if(Calibration.X.Range != 0) MagnetX = (1024L * (int32_t)(UncalMagnetX - Calibration.X.Offset)) / (Calibration.X.Range); |
else MagnetX = 0; |
137,11 → 137,11 |
} |
|
// 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); |
nick_rad = ((double)I2C_WriteAttitude.Nick) * M_PI / (double)(1800.0); |
roll_rad = ((double)I2C_WriteAttitude.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); |
Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad); |
Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad); |
|
// calculate Heading |
heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI); |
266,8 → 266,8 |
DebugOut.Analog[3] = UncalMagnetX; |
DebugOut.Analog[4] = UncalMagnetY; |
DebugOut.Analog[5] = UncalMagnetZ; |
DebugOut.Analog[6] = ExternData.Attitude[NICK]; |
DebugOut.Analog[7] = ExternData.Attitude[ROLL]; |
DebugOut.Analog[6] = I2C_WriteAttitude.Nick; |
DebugOut.Analog[7] = I2C_WriteAttitude.Roll; |
DebugOut.Analog[8] = Calibration.X.Offset; |
DebugOut.Analog[9] = Calibration.X.Range; |
DebugOut.Analog[10] = Calibration.Y.Offset; |