Rev 1179 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1179 | Rev 1199 | ||
|---|---|---|---|
| Line 25... | Line 25... | ||
| 25 | #include "timer0.h" |
25 | #include "timer0.h" |
| 26 | #include "rc.h" |
26 | #include "rc.h" |
| 27 | #include "eeprom.h" |
27 | #include "eeprom.h" |
| 28 | #include "printf_P.h" |
28 | #include "printf_P.h" |
| Line -... | Line 29... | ||
| - | 29 | ||
| - | 30 | ||
| - | 31 | // for compatibility reasons gcc3.x <-> gcc4.x |
|
| - | 32 | #ifndef SPCR |
|
| - | 33 | #define SPCR SPCR0 |
|
| - | 34 | #endif |
|
| - | 35 | #ifndef SPIE |
|
| - | 36 | #define SPIE SPIE0 |
|
| - | 37 | #endif |
|
| - | 38 | #ifndef SPE |
|
| - | 39 | #define SPE SPE0 |
|
| - | 40 | #endif |
|
| - | 41 | #ifndef DORD |
|
| - | 42 | #define DORD DORD0 |
|
| - | 43 | #endif |
|
| - | 44 | #ifndef MSTR |
|
| - | 45 | #define MSTR MSTR0 |
|
| - | 46 | #endif |
|
| - | 47 | #ifndef CPOL |
|
| - | 48 | #define CPOL CPOL0 |
|
| - | 49 | #endif |
|
| - | 50 | #ifndef CPHA |
|
| - | 51 | #define CPHA CPHA0 |
|
| - | 52 | #endif |
|
| - | 53 | #ifndef SPR1 |
|
| - | 54 | #define SPR1 SPR01 |
|
| - | 55 | #endif |
|
| - | 56 | #ifndef SPR0 |
|
| - | 57 | #define SPR0 SPR00 |
|
| - | 58 | #endif |
|
| - | 59 | ||
| - | 60 | #ifndef SPDR |
|
| - | 61 | #define SPDR SPDR0 |
|
| - | 62 | #endif |
|
| - | 63 | ||
| - | 64 | #ifndef SPSR |
|
| - | 65 | #define SPSR SPSR0 |
|
| - | 66 | #endif |
|
| - | 67 | #ifndef SPIF |
|
| - | 68 | #define SPIF SPIF0 |
|
| - | 69 | #endif |
|
| - | 70 | #ifndef WCOL |
|
| - | 71 | #define WCOL WCOL0 |
|
| - | 72 | #endif |
|
| - | 73 | #ifndef SPI2X |
|
| - | 74 | #define SPI2X SPI2X0 |
|
| - | 75 | #endif |
|
| - | 76 | // ------------------------- |
|
| - | 77 | ||
| 29 | 78 | ||
| Line 30... | Line 79... | ||
| 30 | #define MAX_AXIS_VALUE 500 |
79 | #define MAX_AXIS_VALUE 500 |
| 31 | 80 | ||
| Line 84... | Line 133... | ||
| 84 | #define MM3_SS_PIN PC4 |
133 | #define MM3_SS_PIN PC4 |
| 85 | #define MM3_RESET_PORT PORTC |
134 | #define MM3_RESET_PORT PORTC |
| 86 | #define MM3_RESET_DDR DDRC |
135 | #define MM3_RESET_DDR DDRC |
| 87 | #define MM3_RESET_PIN PC5 |
136 | #define MM3_RESET_PIN PC5 |
| 88 | #endif |
137 | #endif |
| 89 | 138 | ||
| 90 | #define MM3_SS_ON MM3_SS_PORT &= ~(1<<MM3_SS_PIN); |
139 | #define MM3_SS_ON MM3_SS_PORT &= ~(1<<MM3_SS_PIN); |
| 91 | #define MM3_SS_OFF MM3_SS_PORT |= (1<<MM3_SS_PIN); |
140 | #define MM3_SS_OFF MM3_SS_PORT |= (1<<MM3_SS_PIN); |
| 92 | #define MM3_RESET_ON MM3_RESET_PORT |= (1<<MM3_RESET_PIN); |
141 | #define MM3_RESET_ON MM3_RESET_PORT |= (1<<MM3_RESET_PIN); |
| 93 | #define MM3_RESET_OFF MM3_RESET_PORT &= ~(1<<MM3_RESET_PIN); |
142 | #define MM3_RESET_OFF MM3_RESET_PORT &= ~(1<<MM3_RESET_PIN); |
| Line 414... | Line 463... | ||
| 414 | void MM3_Heading(void) |
463 | void MM3_Heading(void) |
| 415 | { |
464 | { |
| 416 | int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw; |
465 | int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw; |
| 417 | int32_t Hx, Hy, Hz, Hx_corr, Hy_corr; |
466 | int32_t Hx, Hy, Hz, Hx_corr, Hy_corr; |
| 418 | int16_t angle; |
467 | int16_t angle; |
| 419 | uint16_t div_factor; |
- | |
| 420 | int16_t heading; |
468 | int16_t heading; |
| Line 421... | Line 469... | ||
| 421 | 469 | ||
| 422 | if (MM3_Timeout) |
470 | if (MM3_Timeout) |
| 423 | { |
471 | { |
| Line 447... | Line 495... | ||
| 447 | Hy = (Hx_corr * sin_yaw + Hy_corr * cos_yaw) / 8192; |
495 | Hy = (Hx_corr * sin_yaw + Hy_corr * cos_yaw) / 8192; |
| Line 448... | Line 496... | ||
| 448 | 496 | ||
| Line 449... | Line -... | ||
| 449 | - | ||
| 450 | // tilt compensation |
- | |
| 451 | - | ||
| 452 | // calibration factor for transforming Gyro Integrals to angular degrees |
497 | |
| 453 | div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
498 | // tilt compensation |
| 454 | 499 | ||
| 455 | // calculate sinus cosinus of nick and tilt angle |
500 | // calculate sinus cosinus of nick and tilt angle |
| Line 456... | Line 501... | ||
| 456 | angle = (IntegralNick/div_factor); |
501 | angle = (int16_t)(IntegralGyroNick/GYRO_DEG_FACTOR); |
| 457 | sin_nick = (int32_t)(c_sin_8192(angle)); |
502 | sin_nick = (int32_t)(c_sin_8192(angle)); |
| 458 | cos_nick = (int32_t)(c_cos_8192(angle)); |
503 | cos_nick = (int32_t)(c_cos_8192(angle)); |
| Line 459... | Line 504... | ||
| 459 | 504 | ||
| 460 | angle = (IntegralRoll/div_factor); |
505 | angle = (int16_t)(IntegralGyroRoll/GYRO_DEG_FACTOR); |