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;
/branches/MK3Mag V0.14 Code Redesign Killagreg/twislave.c
186,8 → 186,6
I2C_RxBuffer = (uint8_t *)&I2C_WriteAttitude;
I2C_RxBufferSize = sizeof(I2C_WriteAttitude);
I2C_Heading.Heading = Heading;
ExternData.Attitude[NICK] = I2C_WriteAttitude.Nick;
ExternData.Attitude[ROLL] = I2C_WriteAttitude.Roll;
break;
default: // unknown command id
I2C_RxBuffer = 0;
/branches/MK3Mag V0.14 Code Redesign Killagreg/uart.c
418,6 → 418,8
{
case 'w':// Attitude
Decode64((uint8_t *) &ExternData, sizeof(ExternData), 3, ReceivedBytes);
I2C_WriteAttitude.Nick = ExternData.Attitude[NICK];
I2C_WriteAttitude.Nick = ExternData.Attitude[ROLL];
RequestFlags |= COMPASS_HEADING;
break;