Rev 1801 | Rev 1821 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1801 | Rev 1805 | ||
---|---|---|---|
Line 85... | Line 85... | ||
85 | static uint16_t debugCounter = 0; |
85 | static uint16_t debugCounter = 0; |
86 | // The pulse width varies from 1ms (0°) to 36.99ms (359.9°) |
86 | // The pulse width varies from 1ms (0°) to 36.99ms (359.9°) |
87 | // in other words 100us/° with a +1ms offset. |
87 | // in other words 100us/° with a +1ms offset. |
88 | // The signal goes low for 65ms between pulses, |
88 | // The signal goes low for 65ms between pulses, |
89 | // so the cycle time is 65mS + the pulse width. |
89 | // so the cycle time is 65mS + the pulse width. |
90 | - | ||
91 | // pwm is high |
90 | // pwm is high |
Line -... | Line 91... | ||
- | 91 | ||
- | 92 | if (debugCounter++ == 5000) { |
|
- | 93 | DebugOut.Digital[0] ^= DEBUG_MK3MAG; |
|
- | 94 | debugCounter = 0; |
|
- | 95 | } |
|
92 | 96 | ||
93 | if (PINC & (1 << PINC4)) { |
97 | if (PINC & (1 << PINC4)) { |
94 | // If PWM signal is high increment PWM high counter |
98 | // If PWM signal is high increment PWM high counter |
95 | // This counter is incremented by a periode of 102.4us, |
99 | // This counter is incremented by a periode of 102.4us, |
96 | // i.e. the resoluton of pwm coded heading is approx. 1 deg. |
100 | // i.e. the resoluton of pwm coded heading is approx. 1 deg. |
Line 101... | Line 105... | ||
101 | PWMTimeout--; // decrement timeout |
105 | PWMTimeout--; // decrement timeout |
102 | compassHeading = -1; // unknown heading |
106 | compassHeading = -1; // unknown heading |
103 | PWMCount = 0; // reset PWM Counter |
107 | PWMCount = 0; // reset PWM Counter |
104 | } |
108 | } |
105 | } else { // pwm is low |
109 | } else { // pwm is low |
106 | // ignore pwm values values of 0 and higher than 37 ms; |
110 | // ignore pwm values values of 0 and higher than 37 ms; |
107 | if ((PWMCount) && (PWMCount < 362)) { // 362 * 102.4us = 37.0688 ms |
111 | if ((PWMCount) && (PWMCount < 362)) { // 362 * 102.4us = 37.0688 ms |
108 | if (PWMCount < 10) |
112 | if (PWMCount < 10) |
109 | compassHeading = 0; |
113 | compassHeading = 0; |
110 | else |
114 | else { |
111 | compassHeading = ((uint32_t) (PWMCount - 10) * 1049L) / 1024; // correct timebase and offset |
115 | compassHeading = ((uint32_t) (PWMCount - 10) * 1049L) / 1024; // correct timebase and offset |
- | 116 | DebugOut.Digital[1] ^= DEBUG_MK3MAG; // correct signal recd. |
|
- | 117 | } |
|
- | 118 | /* |
|
- | 119 | compassHeading - compassCourse on a -180..179 range. |
|
- | 120 | compassHeading 20 compassCourse 30 --> ((540 - 10)%360) - 180 = -10 |
|
- | 121 | compassHeading 30 compassCourse 20 --> ((540 + 10)%360) - 180 = 10 |
|
- | 122 | compassHeading 350 compassCourse 10 --> ((540 + 340)%360) - 180 = -20 |
|
- | 123 | compassHeading 10 compassCourse 350 --> ((540 - 340)%360) - 180 = 20 |
|
- | 124 | */ |
|
112 | compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
125 | //compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
113 | PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid |
126 | PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid |
114 | // 12 * 362 counts * 102.4 us |
127 | // 12 * 362 counts * 102.4 us |
115 | } |
128 | } |
116 | PWMCount = 0; // reset pwm counter |
129 | PWMCount = 0; // reset pwm counter |
117 | DebugOut.Digital[0] ^= DEBUG_MK3MAG; |
- | |
118 | DebugOut.Digital[1] &= ~DEBUG_MK3MAG; |
- | |
- | 130 | } |
|
119 | } if (!PWMTimeout) { |
131 | if (!PWMTimeout) { |
120 | if (CheckDelay(BeepDelay)) { |
132 | if (CheckDelay(BeepDelay)) { |
121 | if (!BeepTime) |
133 | if (!BeepTime) |
122 | BeepTime = 100; // make noise with 10Hz to signal the compass problem |
134 | BeepTime = 100; // make noise with 10Hz to signal the compass problem |
123 | BeepDelay = SetDelay(100); |
135 | BeepDelay = SetDelay(100); |
124 | DebugOut.Digital[1] |= DEBUG_MK3MAG; |
- | |
125 | } |
136 | } |
126 | } |
137 | } |
127 | } |
138 | } |
Line -... | Line 139... | ||
- | 139 |