Rev 750 | Rev 761 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 750 | Rev 754 | ||
---|---|---|---|
Line 62... | Line 62... | ||
62 | #include "fc.h" |
62 | #include "fc.h" |
63 | #include "gps.h" |
63 | #include "gps.h" |
64 | #include "uart.h" |
64 | #include "uart.h" |
65 | #include "rc.h" |
65 | #include "rc.h" |
66 | #include "twimaster.h" |
66 | #include "twimaster.h" |
- | 67 | #ifdef USE_MM3 |
|
67 | #include "mm3.h" |
68 | #include "mm3.h" |
- | 69 | #endif |
|
- | 70 | #ifdef USE_CMPS03 |
|
- | 71 | #include "cmps03.h" |
|
- | 72 | #endif |
|
Line 68... | Line 73... | ||
68 | 73 | ||
69 | volatile uint16_t I2CTimeout = 100; |
74 | volatile uint16_t I2CTimeout = 100; |
70 | // gyro readings |
75 | // gyro readings |
71 | volatile int16_t Reading_GyroPitch, Reading_GyroRoll, Reading_GyroYaw; |
76 | volatile int16_t Reading_GyroPitch, Reading_GyroRoll, Reading_GyroYaw; |
Line 92... | Line 97... | ||
92 | // attitude acceleration integrals |
97 | // attitude acceleration integrals |
93 | volatile int32_t IntegralAccPitch = 0, IntegralAccRoll = 0; |
98 | volatile int32_t IntegralAccPitch = 0, IntegralAccRoll = 0; |
94 | volatile int32_t Reading_Integral_Top = 0; |
99 | volatile int32_t Reading_Integral_Top = 0; |
Line 95... | Line 100... | ||
95 | 100 | ||
96 | // compass course |
101 | // compass course |
97 | volatile int16_t CompassHeading = 0; |
102 | volatile int16_t CompassHeading = -1; // negative angle indicates invalid data. |
98 | volatile int16_t CompassCourse = 0; |
103 | volatile int16_t CompassCourse = -1; |
Line 99... | Line 104... | ||
99 | volatile int16_t CompassOffCourse = 0; |
104 | volatile int16_t CompassOffCourse = 0; |
100 | 105 | ||
101 | // flags |
106 | // flags |
Line 1013... | Line 1018... | ||
1013 | 1018 | ||
1014 | if (!updCompass--) |
1019 | if (!updCompass--) |
1015 | { |
1020 | { |
1016 | updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
1021 | updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
- | 1022 | // get current compass heading (angule between MK head and magnetic north) |
|
1017 | // get current compass heading (angule between MK head and magnetic north) |
1023 | #ifdef USE_MM3 |
- | 1024 | CompassHeading = MM3_Heading(); |
|
- | 1025 | #endif |
|
- | 1026 | #ifdef USE_CMPS03 |
|
- | 1027 | CompassHeading = CMPS03_Heading(); |
|
- | 1028 | #endif |
|
1018 | CompassHeading = MM3_Heading(); |
1029 | |
1019 | if (CompassHeading < 0) // no compass data available |
1030 | if (CompassHeading < 0) // no compass data available |
1020 | { |
1031 | { |
1021 | CompassOffCourse = 0; |
1032 | CompassOffCourse = 0; |
1022 | if(!BeepTime) BeepTime = 100; // make noise at 10 Hz to signal the compass problem |
1033 | if(!BeepTime) BeepTime = 100; // make noise at 10 Hz to signal the compass problem |