Rev 765 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 765 | Rev 782 | ||
|---|---|---|---|
| Line 1... | Line 1... | ||
| 1 | #include <avr/io.h> |
1 | #include <avr/io.h> |
| - | 2 | #include <stdlib.h> |
|
| 2 | #include "fc.h" |
3 | #include "fc.h" |
| 3 | #include "timer0.h" |
4 | #include "timer0.h" |
| - | 5 | #include "uart.h" |
|
| Line 4... | Line 6... | ||
| 4 | 6 | ||
| 5 | int32_t PWMHeading = -1; |
7 | int32_t PWMHeading = -1; |
| Line 6... | Line 8... | ||
| 6 | uint8_t PWMTimeout = 0; |
8 | uint8_t PWMTimeout = 0; |
| Line 35... | Line 37... | ||
| 35 | if(PINC & (1<<PINC4)) |
37 | if(PINC & (1<<PINC4)) |
| 36 | { // If PWM signal is high increment PWM high counter |
38 | { // If PWM signal is high increment PWM high counter |
| 37 | // This counter is incremented by a periode of 102.4us, |
39 | // This counter is incremented by a periode of 102.4us, |
| 38 | // i.e. the resoluton of pwm coded heading is approx. 1 deg. |
40 | // i.e. the resoluton of pwm coded heading is approx. 1 deg. |
| 39 | PWMCount++; |
41 | PWMCount++; |
| - | 42 | // pwm overflow? |
|
| - | 43 | if (PWMCount > 400) |
|
| - | 44 | { |
|
| - | 45 | if(PWMTimeout ) PWMTimeout--; // decrement timeout |
|
| - | 46 | PWMCount = 0; // reset PWM Counter |
|
| - | 47 | } |
|
| - | 48 | ||
| 40 | } |
49 | } |
| 41 | else // PWM is low |
50 | else // PWM is low |
| 42 | { |
51 | { |
| 43 | if((PWMCount) && (PWMCount < 400)) |
52 | if((PWMCount) && (PWMCount < 400)) |
| 44 | { |
53 | { |
| Line 50... | Line 59... | ||
| 50 | { // overflow at low edge detection |
59 | { // overflow at low edge detection |
| 51 | if(PWMTimeout ) PWMTimeout--; |
60 | if(PWMTimeout ) PWMTimeout--; |
| 52 | } |
61 | } |
| 53 | PWMCount = 0; // reset pwm counter |
62 | PWMCount = 0; // reset pwm counter |
| 54 | } |
63 | } |
| 55 | // overflow without a low edge (nothing connected to PC4) |
- | |
| 56 | if (PWMCount > 400) |
- | |
| 57 | { |
- | |
| 58 | if(PWMTimeout ) PWMTimeout--; |
- | |
| 59 | PWMCount = 0; |
- | |
| 60 | } |
- | |
| 61 | - | ||
| 62 | - | ||
| 63 | } |
64 | } |
| Line 64... | Line 65... | ||
| 64 | 65 | ||
| 65 | 66 | ||
| 66 | /*********************************************/ |
67 | /*********************************************/ |
| 67 | /* Calculate north direction (heading) */ |
68 | /* Calculate north direction (heading) */ |
| 68 | /*********************************************/ |
69 | /*********************************************/ |
| 69 | int16_t CMPS03_Heading(void) |
70 | int16_t CMPS03_Heading(void) |
| - | 71 | { |
|
| - | 72 | int16_t heading, w, v; |
|
| - | 73 | ||
| 70 | { |
74 | DebugOut.Analog[11] = PWMTimeout; |
| 71 | int16_t heading, w, v; |
75 | |
| 72 | if(PWMTimeout) |
76 | if(PWMTimeout) |
| 73 | { |
77 | { |
| 74 | w = abs(IntegralPitch / 512); |
78 | w = abs(IntegralPitch / 512); |
| Line 80... | Line 84... | ||
| 80 | // range from 0 to 359 |
84 | // range from 0 to 359 |
| 81 | heading = (int16_t)PWMHeading; |
85 | heading = (int16_t)PWMHeading; |
| 82 | if (heading < 0) heading += 360; |
86 | if (heading < 0) heading += 360; |
| 83 | heading = heading%360; |
87 | heading = heading%360; |
| 84 | } |
88 | } |
| 85 | else // no date available from compass |
89 | else // compass to much tilted |
| 86 | { |
90 | { |
| 87 | if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem |
- | |
| 88 | heading = -1; |
91 | heading = -1; |
| 89 | } |
92 | } |
| 90 | } |
93 | } |
| 91 | else // no data from compass |
94 | else // no data from compass |
| 92 | { |
95 | { |
| - | 96 | if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem |
|
| 93 | heading = -1; |
97 | heading = -1; |
| 94 | } |
98 | } |
| 95 | return heading; |
99 | return heading; |
| 96 | } |
100 | } |