Rev 782 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 782 | Rev 783 | ||
|---|---|---|---|
| Line 53... | Line 53... | ||
| 53 | { |
53 | { |
| 54 | PWMHeading = (((uint32_t)PWMCount * 1024L) / 1000L) - 10; // correct timebase and offset |
54 | PWMHeading = (((uint32_t)PWMCount * 1024L) / 1000L) - 10; // correct timebase and offset |
| 55 | PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid |
55 | PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid |
| 56 | // 12 * 400 counts * 102.4 us = 419 ms |
56 | // 12 * 400 counts * 102.4 us = 419 ms |
| 57 | } |
57 | } |
| 58 | else // PWM counter is over the pwm periode of 37 ms |
- | |
| 59 | { // overflow at low edge detection |
- | |
| 60 | if(PWMTimeout ) PWMTimeout--; |
- | |
| 61 | } |
- | |
| 62 | PWMCount = 0; // reset pwm counter |
58 | PWMCount = 0; // reset pwm counter |
| 63 | } |
59 | } |
| 64 | } |
60 | } |
| Line 76... | Line 72... | ||
| 76 | if(PWMTimeout) |
72 | if(PWMTimeout) |
| 77 | { |
73 | { |
| 78 | w = abs(IntegralPitch / 512); |
74 | w = abs(IntegralPitch / 512); |
| 79 | v = abs(IntegralRoll / 512); |
75 | v = abs(IntegralRoll / 512); |
| 80 | if(v > w) w = v; // get maximum declination |
76 | if(v > w) w = v; // get maximum declination |
| 81 | // if declination is small enough do have valid compass heading |
77 | // if declination is small enough to have valid compass heading |
| 82 | if(w < 35) |
78 | if(w < 35) |
| 83 | { |
79 | { |
| 84 | // range from 0 to 359 |
80 | // range from 0 to 359 |
| 85 | heading = (int16_t)PWMHeading; |
81 | heading = (int16_t)PWMHeading; |
| 86 | if (heading < 0) heading += 360; |
82 | if (heading < 0) heading += 360; |