Rev 785 | Go to most recent revision | Only display areas with differences | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 785 | Rev 790 | ||
---|---|---|---|
1 | #include <avr/io.h> |
1 | #include <avr/io.h> |
2 | #include <stdlib.h> |
2 | #include <stdlib.h> |
3 | #include "fc.h" |
3 | #include "fc.h" |
4 | #include "timer0.h" |
4 | #include "timer0.h" |
5 | #include "uart.h" |
5 | #include "uart.h" |
6 | 6 | ||
7 | int32_t PWMHeading = -1; |
7 | int32_t PWMHeading = -1; |
8 | uint8_t PWMTimeout = 0; |
8 | uint8_t PWMTimeout = 0; |
9 | 9 | ||
10 | /*********************************************/ |
10 | /*********************************************/ |
11 | /* Initialize Interface to CMPS02 Compass */ |
11 | /* Initialize Interface to CMPS02 Compass */ |
12 | /*********************************************/ |
12 | /*********************************************/ |
13 | void CMPS03_Init(void) |
13 | void CMPS03_Init(void) |
14 | { |
14 | { |
15 | // Port PC4 connected to PWM output from compass module |
15 | // Port PC4 connected to PWM output from compass module |
16 | DDRC &= ~(1<<DDC4); // set as input |
16 | DDRC &= ~(1<<DDC4); // set as input |
17 | PORTC |= (1<<PORTC4); // pull up to increase PWM counter also if nothing is connected |
17 | PORTC |= (1<<PORTC4); // pull up to increase PWM counter also if nothing is connected |
18 | 18 | ||
19 | PWMTimeout = 0; |
19 | PWMTimeout = 0; |
20 | } |
20 | } |
21 | 21 | ||
22 | 22 | ||
23 | /*********************************************/ |
23 | /*********************************************/ |
24 | /* Get Data from CMPS03 */ |
24 | /* Get Data from CMPS03 */ |
25 | /*********************************************/ |
25 | /*********************************************/ |
26 | void CMPS03_Update(void) // called every 102.4 us by timer 0 ISR |
26 | void CMPS03_Update(void) // called every 102.4 us by timer 0 ISR |
27 | { |
27 | { |
28 | static uint16_t PWMCount = 0; |
28 | static uint16_t PWMCount = 0; |
29 | // The pulse width varies from 1ms (0°) to 36.99ms (359.9°) |
29 | // The pulse width varies from 1ms (0°) to 36.99ms (359.9°) |
30 | // in other words 100us/° with a +1ms offset. |
30 | // in other words 100us/° with a +1ms offset. |
31 | // The signal goes low for 65ms between pulses, |
31 | // The signal goes low for 65ms between pulses, |
32 | // so the cycle time is 65mS + the pulse width. |
32 | // so the cycle time is 65mS + the pulse width. |
33 | // The pulse is generated by a 16 bit timer in the processor |
33 | // The pulse is generated by a 16 bit timer in the processor |
34 | // giving a 1uS resolution, however I would not recommend |
34 | // giving a 1uS resolution, however I would not recommend |
35 | // measuring this to anything better than 0.1° (10uS). |
35 | // measuring this to anything better than 0.1° (10uS). |
36 | 36 | ||
37 | if(PINC & (1<<PINC4)) |
37 | if(PINC & (1<<PINC4)) |
38 | { // If PWM signal is high increment PWM high counter |
38 | { // If PWM signal is high increment PWM high counter |
39 | // This counter is incremented by a periode of 102.4us, |
39 | // This counter is incremented by a periode of 102.4us, |
40 | // 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. |
41 | PWMCount++; |
41 | PWMCount++; |
42 | // pwm overflow? |
42 | // pwm overflow? |
43 | if (PWMCount > 400) |
43 | if (PWMCount > 400) |
44 | { |
44 | { |
45 | if(PWMTimeout ) PWMTimeout--; // decrement timeout |
45 | if(PWMTimeout ) PWMTimeout--; // decrement timeout |
46 | PWMCount = 0; // reset PWM Counter |
46 | PWMCount = 0; // reset PWM Counter |
47 | } |
47 | } |
48 | 48 | ||
49 | } |
49 | } |
50 | else // PWM is low |
50 | else // PWM is low |
51 | { |
51 | { |
52 | if((PWMCount) && (PWMCount < 400)) |
52 | if((PWMCount) && (PWMCount < 400)) |
53 | { |
53 | { |
54 | if(PWMCount <9) PWMHeading = 0; |
54 | if(PWMCount <9) PWMHeading = 0; |
55 | else PWMHeading = ((uint32_t)(PWMCount - 9) * 1049L) >> 10; // correct timebase and offset |
55 | else PWMHeading = ((uint32_t)(PWMCount - 9) * 1049L)/1024; // correct timebase and offset |
56 | PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid |
56 | PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid |
57 | // 12 * 400 counts * 102.4 us = 419 ms |
57 | // 12 * 400 counts * 102.4 us = 419 ms |
58 | } |
58 | } |
59 | PWMCount = 0; // reset pwm counter |
59 | PWMCount = 0; // reset pwm counter |
60 | } |
60 | } |
61 | } |
61 | } |
62 | 62 | ||
63 | 63 | ||
64 | /*********************************************/ |
64 | /*********************************************/ |
65 | /* Calculate north direction (heading) */ |
65 | /* Calculate north direction (heading) */ |
66 | /*********************************************/ |
66 | /*********************************************/ |
67 | int16_t CMPS03_Heading(void) |
67 | int16_t CMPS03_Heading(void) |
68 | { |
68 | { |
69 | int16_t heading, w, v; |
69 | int16_t heading, w, v; |
70 | - | ||
71 | DebugOut.Analog[11] = PWMTimeout; |
- | |
72 | 70 | ||
73 | if(PWMTimeout) |
71 | if(PWMTimeout) |
74 | { |
72 | { |
75 | w = abs(IntegralPitch / 512); |
73 | w = abs(IntegralPitch / 512); |
76 | v = abs(IntegralRoll / 512); |
74 | v = abs(IntegralRoll / 512); |
77 | if(v > w) w = v; // get maximum declination |
75 | if(v > w) w = v; // get maximum declination |
78 | // if declination is small enough to have valid compass heading |
76 | // if declination is small enough to have valid compass heading |
79 | if(w < 35) |
77 | if(w < 35) |
80 | { |
78 | { |
81 | // range from 0 to 359 |
79 | // range from 0 to 359 |
82 | heading = (int16_t)PWMHeading; |
80 | heading = (int16_t)PWMHeading; |
83 | if (heading < 0) heading += 360; |
81 | if (heading < 0) heading += 360; |
84 | heading = heading%360; |
82 | heading = heading%360; |
85 | } |
83 | } |
86 | else // compass to much tilted |
84 | else // compass to much tilted |
87 | { |
85 | { |
88 | heading = -1; |
86 | heading = -1; |
89 | } |
87 | } |
90 | } |
88 | } |
91 | else // no data from compass |
89 | else // no data from compass |
92 | { |
90 | { |
93 | if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem |
91 | if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem |
94 | heading = -1; |
92 | heading = -1; |
95 | } |
93 | } |
96 | return heading; |
94 | return heading; |
97 | } |
95 | } |
98 | 96 |