Rev 764 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 764 | Rev 765 | ||
|---|---|---|---|
| Line 1... | Line 1... | ||
| 1 | #include <avr/io.h> |
1 | #include <avr/io.h> |
| 2 | 2 | #include "fc.h" |
|
| - | 3 | #include "timer0.h" |
|
| Line 3... | Line 4... | ||
| 3 | 4 | ||
| 4 | int32_t PWMHeading = -1; |
5 | int32_t PWMHeading = -1; |
| Line 5... | Line 6... | ||
| 5 | uint8_t PWMTimeout = 0; |
6 | uint8_t PWMTimeout = 0; |
| Line 65... | Line 66... | ||
| 65 | /*********************************************/ |
66 | /*********************************************/ |
| 66 | /* Calculate north direction (heading) */ |
67 | /* Calculate north direction (heading) */ |
| 67 | /*********************************************/ |
68 | /*********************************************/ |
| 68 | int16_t CMPS03_Heading(void) |
69 | int16_t CMPS03_Heading(void) |
| 69 | { |
70 | { |
| 70 | int16_t heading; |
71 | int16_t heading, w, v; |
| 71 | if(PWMTimeout) |
72 | if(PWMTimeout) |
| - | 73 | { |
|
| - | 74 | w = abs(IntegralPitch / 512); |
|
| - | 75 | v = abs(IntegralRoll / 512); |
|
| - | 76 | if(v > w) w = v; // get maximum declination |
|
| - | 77 | // if declination is small enough do have valid compass heading |
|
| - | 78 | if(w < 35) |
|
| - | 79 | { |
|
| 72 | { // range from 0 to 359 |
80 | // range from 0 to 359 |
| 73 | heading = (int16_t)PWMHeading; |
81 | heading = (int16_t)PWMHeading; |
| 74 | if (heading < 0) heading += 360; |
82 | if (heading < 0) heading += 360; |
| 75 | heading = heading%360; |
83 | heading = heading%360; |
| - | 84 | } |
|
| - | 85 | else // no date available from compass |
|
| - | 86 | { |
|
| - | 87 | if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem |
|
| - | 88 | heading = -1; |
|
| - | 89 | } |
|
| 76 | } |
90 | } |
| 77 | else // no data from compass |
91 | else // no data from compass |
| 78 | { |
92 | { |
| 79 | heading = -1; |
93 | heading = -1; |
| 80 | } |
94 | } |