Rev 256 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 256 | Rev 257 | ||
|---|---|---|---|
| Line 270... | Line 270... | ||
| 270 | { |
270 | { |
| 271 | crc += pBuff[i]; |
271 | crc += pBuff[i]; |
| 272 | } |
272 | } |
| 273 | crc = ~crc; |
273 | crc = ~crc; |
| 274 | if(Calibration.crc != crc) return(0); // crc mismatch |
274 | if(Calibration.crc != crc) return(0); // crc mismatch |
| 275 | if(Calibration.Version == CALIBRATION_VERSION) |
275 | if(Calibration.Version == CALIBRATION_VERSION) return(1); |
| 276 | { |
- | |
| 277 | //#ifdef DEBUG |
- | |
| 278 | u8 msg[50]; |
- | |
| 279 | UART1_PutString("\r\n"); |
- | |
| 280 | sprintf(msg, "XRange = %d, XOffset = %d \r\n", Calibration.MagX.Range, Calibration.MagX.Offset); |
- | |
| 281 | UART1_PutString(msg); |
- | |
| 282 | sprintf(msg, "YRange = %d, YOffset = %d \r\n", Calibration.MagY.Range, Calibration.MagY.Offset); |
- | |
| 283 | UART1_PutString(msg); |
- | |
| 284 | sprintf(msg, "ZRange = %d, ZOffset = %d \r\n", Calibration.MagZ.Range, Calibration.MagZ.Offset); |
- | |
| 285 | UART1_PutString(msg); |
- | |
| 286 | //#endif |
- | |
| 287 | return(1); |
- | |
| 288 | } |
- | |
| 289 | } |
276 | } |
| 290 | return(0); |
277 | return(0); |
| 291 | } |
278 | } |
| Line 415... | Line 402... | ||
| 415 | cosnick = (s32)c_cos_8192(tmp); |
402 | cosnick = (s32)c_cos_8192(tmp); |
| 416 | tmp = FromFlightCtrl.AngleRoll/10; // in deg |
403 | tmp = FromFlightCtrl.AngleRoll/10; // in deg |
| 417 | sinroll = (s32)c_sin_8192(tmp); |
404 | sinroll = (s32)c_sin_8192(tmp); |
| 418 | cosroll = (s32)c_cos_8192(tmp); |
405 | cosroll = (s32)c_cos_8192(tmp); |
| 419 | // tbd. compensation signs and oriantation has to be fixed |
406 | // tbd. compensation signs and oriantation has to be fixed |
| 420 | Hx = (s16)((MagVector.X * cosnick - MagVector.Z * sinnick)/8192L); |
407 | Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L); |
| 421 | Hy = (s16)((MagVector.Y * cosroll + MagVector.Z * sinroll)/8192L); |
408 | Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L); |
| 422 | - | ||
| 423 | DebugOut.Analog[23] = (s16)Hx; |
- | |
| 424 | DebugOut.Analog[24] = (s16)Hy; |
- | |
| 425 | - | ||
| 426 | // calculate heading |
409 | // calculate heading |
| 427 | tmp = (s16)(c_tan2_546(Hy, Hx)/546L); |
410 | tmp = (s16)(c_tan2_546(Hy, Hx)/546L); |
| 428 | if (tmp <= 0) tmp = -tmp; |
411 | if (tmp > 0) tmp = 360 - tmp; |
| 429 | else tmp = 360 - tmp; |
412 | else tmp = -tmp; |
| 430 | Compass_Heading = tmp; |
413 | Compass_Heading = tmp; |
| 431 | } |
414 | } |
| 432 | } |
415 | } |
| 433 | } |
416 | } |
| 434 | // rx data handler for acceleration raw data |
417 | // rx data handler for acceleration raw data |