Subversion Repositories FlightCtrl

Rev

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