Rev 886 | Rev 935 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 886 | Rev 907 | ||
---|---|---|---|
1 | #include <avr/io.h> |
1 | #include <avr/io.h> |
2 | #include <stdlib.h> |
2 | #include <stdlib.h> |
3 | #include <inttypes.h> |
3 | #include <inttypes.h> |
4 | #include "timer0.h" |
4 | #include "timer0.h" |
5 | #include "uart.h" |
- | |
6 | #include "fc.h" |
5 | #include "fc.h" |
7 | #include "rc.h" |
6 | #include "rc.h" |
8 | #include "eeprom.h" |
7 | #include "eeprom.h" |
9 | #include "mk3mag.h" |
8 | #include "mk3mag.h" |
10 | 9 | ||
11 | uint8_t PWMTimeout = 12; |
10 | uint8_t PWMTimeout = 12; |
12 | ToMk3Mag_t ToMk3Mag; |
11 | ToMk3Mag_t ToMk3Mag; |
13 | 12 | ||
14 | 13 | ||
15 | /*********************************************/ |
14 | /*********************************************/ |
16 | /* Initialize Interface to MK3MAG Compass */ |
15 | /* Initialize Interface to MK3MAG Compass */ |
17 | /*********************************************/ |
16 | /*********************************************/ |
18 | void MK3MAG_Init(void) |
17 | void MK3MAG_Init(void) |
19 | { |
18 | { |
20 | // Port PC4 connected to PWM output from compass module |
19 | // Port PC4 connected to PWM output from compass module |
21 | DDRC &= ~(1<<DDC4); // set as input |
20 | DDRC &= ~(1<<DDC4); // set as input |
22 | PORTC |= (1<<PORTC4); // pull up to increase PWM counter also if nothing is connected |
21 | PORTC |= (1<<PORTC4); // pull up to increase PWM counter also if nothing is connected |
23 | 22 | ||
24 | PWMTimeout = 0; |
23 | PWMTimeout = 0; |
25 | 24 | ||
26 | ToMk3Mag.CalState = 0; |
25 | ToMk3Mag.CalState = 0; |
27 | ToMk3Mag.Orientation = 1; |
26 | ToMk3Mag.Orientation = 1; |
28 | } |
27 | } |
29 | 28 | ||
30 | 29 | ||
31 | /*********************************************/ |
30 | /*********************************************/ |
32 | /* Get PWM from MK3MAG */ |
31 | /* Get PWM from MK3MAG */ |
33 | /*********************************************/ |
32 | /*********************************************/ |
34 | void MK3MAG_Update(void) // called every 102.4 us by timer 0 ISR |
33 | void MK3MAG_Update(void) // called every 102.4 us by timer 0 ISR |
35 | { |
34 | { |
36 | static uint16_t PWMCount = 0; |
35 | static uint16_t PWMCount = 0; |
37 | static uint16_t BeepDelay = 0; |
36 | static uint16_t BeepDelay = 0; |
38 | // The pulse width varies from 1ms (0°) to 36.99ms (359.9°) |
37 | // The pulse width varies from 1ms (0°) to 36.99ms (359.9°) |
39 | // in other words 100us/° with a +1ms offset. |
38 | // in other words 100us/° with a +1ms offset. |
40 | // The signal goes low for 65ms between pulses, |
39 | // The signal goes low for 65ms between pulses, |
41 | // so the cycle time is 65mS + the pulse width. |
40 | // so the cycle time is 65mS + the pulse width. |
42 | 41 | ||
43 | // pwm is high |
42 | // pwm is high |
- | 43 | ||
44 | if(PINC & (1<<PINC4)) |
44 | if(PINC & (1<<PINC4)) |
45 | { // If PWM signal is high increment PWM high counter |
45 | { // If PWM signal is high increment PWM high counter |
46 | // This counter is incremented by a periode of 102.4us, |
46 | // This counter is incremented by a periode of 102.4us, |
47 | // i.e. the resoluton of pwm coded heading is approx. 1 deg. |
47 | // i.e. the resoluton of pwm coded heading is approx. 1 deg. |
48 | PWMCount++; |
48 | PWMCount++; |
49 | // pwm overflow? |
49 | // pwm overflow? |
50 | if (PWMCount > 400) |
50 | if (PWMCount > 400) |
51 | { |
51 | { |
52 | if(PWMTimeout) PWMTimeout--; // decrement timeout |
52 | if(PWMTimeout) PWMTimeout--; // decrement timeout |
53 | CompassHeading = -1; // unknown heading |
53 | CompassHeading = -1; // unknown heading |
54 | PWMCount = 0; // reset PWM Counter |
54 | PWMCount = 0; // reset PWM Counter |
55 | } |
55 | } |
56 | 56 | ||
57 | } |
57 | } |
58 | else // pwm is low |
58 | else // pwm is low |
59 | { // ignore pwm values values of 0 and higher than 37 ms |
59 | { // ignore pwm values values of 0 and higher than 37 ms; |
60 | if((PWMCount) && (PWMCount < 362)) // 362 * 102.4us = 37.0688 ms |
60 | if((PWMCount) && (PWMCount < 362)) // 362 * 102.4us = 37.0688 ms |
61 | { |
61 | { |
62 | if(PWMCount <10) CompassHeading = 0; |
62 | if(PWMCount <10) CompassHeading = 0; |
63 | else CompassHeading = ((uint32_t)(PWMCount - 10) * 1049L)/1024; // correct timebase and offset |
63 | else CompassHeading = ((uint32_t)(PWMCount - 10) * 1049L)/1024; // correct timebase and offset |
64 | CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
64 | CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
65 | PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid |
65 | PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid |
66 | // 12 * 362 counts * 102.4 us |
66 | // 12 * 362 counts * 102.4 us |
67 | } |
67 | } |
68 | else |
- | |
69 | { // bad pwm value (out of range or permanent low) |
- | |
70 | if(PWMTimeout) PWMTimeout--; // decrement timeout |
- | |
71 | CompassHeading = -1; // unknown heading |
- | |
72 | CompassOffCourse = 0; |
- | |
73 | } |
- | |
74 | PWMCount = 0; // reset pwm counter |
68 | PWMCount = 0; // reset pwm counter |
75 | } |
69 | } |
76 | - | ||
77 | if(!PWMTimeout) |
70 | if(!PWMTimeout) |
78 | { |
71 | { |
79 | if(CheckDelay(BeepDelay)) |
72 | if(CheckDelay(BeepDelay)) |
80 | { |
73 | { |
81 | if(!BeepTime) BeepTime = 100; // make noise with 10Hz to signal the compass problem |
74 | if(!BeepTime) BeepTime = 100; // make noise with 10Hz to signal the compass problem |
82 | BeepDelay = SetDelay(100); |
75 | BeepDelay = SetDelay(100); |
83 | } |
76 | } |
84 | } |
77 | } |
85 | } |
78 | } |
86 | 79 | ||
87 | 80 | ||
88 | 81 | ||
89 | 82 |