| 272,20 → 272,7 |
|---|
| } |
| crc = ~crc; |
| if(Calibration.crc != crc) return(0); // crc mismatch |
| if(Calibration.Version == CALIBRATION_VERSION) |
| { |
| //#ifdef DEBUG |
| u8 msg[50]; |
| UART1_PutString("\r\n"); |
| sprintf(msg, "XRange = %d, XOffset = %d \r\n", Calibration.MagX.Range, Calibration.MagX.Offset); |
| UART1_PutString(msg); |
| sprintf(msg, "YRange = %d, YOffset = %d \r\n", Calibration.MagY.Range, Calibration.MagY.Offset); |
| UART1_PutString(msg); |
| sprintf(msg, "ZRange = %d, ZOffset = %d \r\n", Calibration.MagZ.Range, Calibration.MagZ.Offset); |
| UART1_PutString(msg); |
| //#endif |
| return(1); |
| } |
| if(Calibration.Version == CALIBRATION_VERSION) return(1); |
| } |
| return(0); |
| } |
| 417,16 → 404,12 |
|---|
| sinroll = (s32)c_sin_8192(tmp); |
| cosroll = (s32)c_cos_8192(tmp); |
| // tbd. compensation signs and oriantation has to be fixed |
| Hx = (s16)((MagVector.X * cosnick - MagVector.Z * sinnick)/8192L); |
| Hy = (s16)((MagVector.Y * cosroll + MagVector.Z * sinroll)/8192L); |
| |
| DebugOut.Analog[23] = (s16)Hx; |
| DebugOut.Analog[24] = (s16)Hy; |
| |
| Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L); |
| Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L); |
| // calculate heading |
| tmp = (s16)(c_tan2_546(Hy, Hx)/546L); |
| if (tmp <= 0) tmp = -tmp; |
| else tmp = 360 - tmp; |
| if (tmp > 0) tmp = 360 - tmp; |
| else tmp = -tmp; |
| Compass_Heading = tmp; |
| } |
| } |