Rev 764 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 764 | Rev 765 | ||
---|---|---|---|
1 | #include <avr/io.h> |
1 | #include <avr/io.h> |
2 | 2 | #include "fc.h" |
|
- | 3 | #include "timer0.h" |
|
3 | 4 | ||
4 | int32_t PWMHeading = -1; |
5 | int32_t PWMHeading = -1; |
5 | uint8_t PWMTimeout = 0; |
6 | uint8_t PWMTimeout = 0; |
6 | 7 | ||
7 | /*********************************************/ |
8 | /*********************************************/ |
8 | /* Initialize Interface to CMPS02 Compass */ |
9 | /* Initialize Interface to CMPS02 Compass */ |
9 | /*********************************************/ |
10 | /*********************************************/ |
10 | void CMPS03_Init(void) |
11 | void CMPS03_Init(void) |
11 | { |
12 | { |
12 | // Port PC4 connected to PWM output from compass module |
13 | // Port PC4 connected to PWM output from compass module |
13 | DDRC &= ~(1<<DDC4); // set as input |
14 | DDRC &= ~(1<<DDC4); // set as input |
14 | PORTC |= (1<<PORTC4); // pull up to increase PWM counter also if nothing is connected |
15 | PORTC |= (1<<PORTC4); // pull up to increase PWM counter also if nothing is connected |
15 | 16 | ||
16 | PWMTimeout = 0; |
17 | PWMTimeout = 0; |
17 | } |
18 | } |
18 | 19 | ||
19 | 20 | ||
20 | /*********************************************/ |
21 | /*********************************************/ |
21 | /* Get Data from CMPS03 */ |
22 | /* Get Data from CMPS03 */ |
22 | /*********************************************/ |
23 | /*********************************************/ |
23 | void CMPS03_Update(void) // called every 102.4 us by timer 0 ISR |
24 | void CMPS03_Update(void) // called every 102.4 us by timer 0 ISR |
24 | { |
25 | { |
25 | static uint16_t PWMCount = 0; |
26 | static uint16_t PWMCount = 0; |
26 | // The pulse width varies from 1ms (0°) to 36.99ms (359.9°) |
27 | // The pulse width varies from 1ms (0°) to 36.99ms (359.9°) |
27 | // in other words 100us/° with a +1ms offset. |
28 | // in other words 100us/° with a +1ms offset. |
28 | // The signal goes low for 65ms between pulses, |
29 | // The signal goes low for 65ms between pulses, |
29 | // so the cycle time is 65mS + the pulse width. |
30 | // so the cycle time is 65mS + the pulse width. |
30 | // The pulse is generated by a 16 bit timer in the processor |
31 | // The pulse is generated by a 16 bit timer in the processor |
31 | // giving a 1uS resolution, however I would not recommend |
32 | // giving a 1uS resolution, however I would not recommend |
32 | // measuring this to anything better than 0.1° (10uS). |
33 | // measuring this to anything better than 0.1° (10uS). |
33 | 34 | ||
34 | if(PINC & (1<<PINC4)) |
35 | if(PINC & (1<<PINC4)) |
35 | { // If PWM signal is high increment PWM high counter |
36 | { // If PWM signal is high increment PWM high counter |
36 | // This counter is incremented by a periode of 102.4us, |
37 | // This counter is incremented by a periode of 102.4us, |
37 | // i.e. the resoluton of pwm coded heading is approx. 1 deg. |
38 | // i.e. the resoluton of pwm coded heading is approx. 1 deg. |
38 | PWMCount++; |
39 | PWMCount++; |
39 | } |
40 | } |
40 | else // PWM is low |
41 | else // PWM is low |
41 | { |
42 | { |
42 | if((PWMCount) && (PWMCount < 400)) |
43 | if((PWMCount) && (PWMCount < 400)) |
43 | { |
44 | { |
44 | PWMHeading = (((uint32_t)PWMCount * 1024L) / 1000L) - 10; // correct timebase and offset |
45 | PWMHeading = (((uint32_t)PWMCount * 1024L) / 1000L) - 10; // correct timebase and offset |
45 | PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid |
46 | PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid |
46 | // 12 * 400 counts * 102.4 us = 419 ms |
47 | // 12 * 400 counts * 102.4 us = 419 ms |
47 | } |
48 | } |
48 | else // PWM counter is over the pwm periode of 37 ms |
49 | else // PWM counter is over the pwm periode of 37 ms |
49 | { // overflow at low edge detection |
50 | { // overflow at low edge detection |
50 | if(PWMTimeout ) PWMTimeout--; |
51 | if(PWMTimeout ) PWMTimeout--; |
51 | } |
52 | } |
52 | PWMCount = 0; // reset pwm counter |
53 | PWMCount = 0; // reset pwm counter |
53 | } |
54 | } |
54 | // overflow without a low edge (nothing connected to PC4) |
55 | // overflow without a low edge (nothing connected to PC4) |
55 | if (PWMCount > 400) |
56 | if (PWMCount > 400) |
56 | { |
57 | { |
57 | if(PWMTimeout ) PWMTimeout--; |
58 | if(PWMTimeout ) PWMTimeout--; |
58 | PWMCount = 0; |
59 | PWMCount = 0; |
59 | } |
60 | } |
60 | 61 | ||
61 | 62 | ||
62 | } |
63 | } |
63 | 64 | ||
64 | 65 | ||
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 | } |
81 | return heading; |
95 | return heading; |
82 | } |
96 | } |
83 | 97 |