Subversion Repositories MK3Mag

Compare Revisions

Ignore whitespace Rev 26 → Rev 27

/branches/MK3Mag V0.14 Code Redesign Killagreg/main.c
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;