Rev 2051 | Only display areas with differences | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2051 | Rev 2052 | ||
---|---|---|---|
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 "output.h" |
|
5 | #include "eeprom.h" |
6 | #include "eeprom.h" |
6 | #include "mk3mag.h" |
7 | #include "mk3mag.h" |
7 | 8 | ||
8 | uint8_t PWMTimeout = 12; |
9 | uint8_t PWMTimeout = 12; |
9 | // This will updated in interrupt handler. Should not be processed by main, other than just atomic copy. |
10 | // This will updated in interrupt handler. Should not be processed by main, other than just atomic copy. |
10 | volatile uint16_t volatileMagneticHeading; |
11 | volatile uint16_t volatileMagneticHeading; |
11 | ToMk3Mag_t toMk3Mag; |
12 | ToMk3Mag_t toMk3Mag; |
12 | 13 | ||
13 | /*********************************************/ |
14 | /*********************************************/ |
14 | /* Initialize Interface to MK3MAG Compass */ |
15 | /* Initialize Interface to MK3MAG Compass */ |
15 | /*********************************************/ |
16 | /*********************************************/ |
16 | void MK3MAG_init(void) { |
17 | void MK3MAG_init(void) { |
17 | // Port PC4 connected to PWM output from compass module |
18 | // Port PC4 connected to PWM output from compass module |
18 | DDRC &= ~(1 << DDC4); // set as input |
19 | DDRC &= ~(1 << DDC4); // set as input |
19 | PORTC |= (1 << PORTC4); // pull up to increase PWM counter also if nothing is connected |
20 | PORTC |= (1 << PORTC4); // pull up to increase PWM counter also if nothing is connected |
20 | PWMTimeout = 0; |
21 | PWMTimeout = 0; |
21 | toMk3Mag.calState = 0; |
22 | toMk3Mag.calState = 0; |
22 | toMk3Mag.orientation = 1; |
23 | toMk3Mag.orientation = 1; |
23 | } |
24 | } |
24 | 25 | ||
25 | /*********************************************/ |
26 | /*********************************************/ |
26 | /* Get PWM from MK3MAG */ |
27 | /* Get PWM from MK3MAG */ |
27 | /*********************************************/ |
28 | /*********************************************/ |
28 | void MK3MAG_periodicTask(void) {// called every 102.4 us by timer 0 ISR |
29 | void MK3MAG_periodicTask(void) {// called every 102.4 us by timer 0 ISR |
29 | static uint16_t PWMCount = 0; |
30 | static uint16_t PWMCount = 0; |
30 | static uint16_t beepDelay = 0; |
31 | static uint16_t beepDelay = 0; |
31 | // static uint16_t debugCounter = 0; |
32 | // static uint16_t debugCounter = 0; |
32 | // The pulse width varies from 1ms (0°) to 36.99ms (359.9°) |
33 | // The pulse width varies from 1ms (0°) to 36.99ms (359.9°) |
33 | // in other words 100us/° with a +1ms offset. |
34 | // in other words 100us/° with a +1ms offset. |
34 | // The signal goes low for 65ms between pulses, |
35 | // The signal goes low for 65ms between pulses, |
35 | // so the cycle time is 65mS + the pulse width. |
36 | // so the cycle time is 65mS + the pulse width. |
36 | // pwm is high |
37 | // pwm is high |
37 | 38 | ||
38 | // if (debugCounter++ == 5000) { |
39 | // if (debugCounter++ == 5000) { |
39 | // DebugOut.Digital[0] ^= DEBUG_MK3MAG; |
40 | // DebugOut.Digital[0] ^= DEBUG_MK3MAG; |
40 | // debugCounter = 0; |
41 | // debugCounter = 0; |
41 | // } |
42 | // } |
42 | 43 | ||
43 | if (PINC & (1 << PINC4)) { |
44 | if (PINC & (1 << PINC4)) { |
44 | // If PWM signal is high increment PWM high counter |
45 | // If PWM signal is high increment PWM high counter |
45 | // This counter is incremented by a periode of 102.4us, |
46 | // This counter is incremented by a periode of 102.4us, |
46 | // 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. |
47 | PWMCount++; |
48 | PWMCount++; |
48 | // pwm overflow? |
49 | // pwm overflow? |
49 | if (PWMCount > 400) { |
50 | if (PWMCount > 400) { |
50 | if (PWMTimeout) |
51 | if (PWMTimeout) |
51 | PWMTimeout--; // decrement timeout |
52 | PWMTimeout--; // decrement timeout |
52 | volatileMagneticHeading = -1; // unknown heading |
53 | volatileMagneticHeading = -1; // unknown heading |
53 | PWMCount = 0; // reset PWM Counter |
54 | PWMCount = 0; // reset PWM Counter |
54 | } |
55 | } |
55 | } else { // pwm is low |
56 | } else { // pwm is low |
56 | // ignore pwm values values of 0 and higher than 37 ms; |
57 | // ignore pwm values values of 0 and higher than 37 ms; |
57 | if ((PWMCount) && (PWMCount < 362)) { // 362 * 102.4us = 37.0688 ms |
58 | if ((PWMCount) && (PWMCount < 362)) { // 362 * 102.4us = 37.0688 ms |
58 | if (PWMCount < 10) |
59 | if (PWMCount < 10) |
59 | volatileMagneticHeading = 0; |
60 | volatileMagneticHeading = 0; |
60 | else { |
61 | else { |
61 | volatileMagneticHeading = ((uint32_t) (PWMCount - 10) * 1049L) / 1024; // correct timebase and offset |
62 | volatileMagneticHeading = ((uint32_t) (PWMCount - 10) * 1049L) / 1024; // correct timebase and offset |
62 | //DebugOut.Digital[1] ^= DEBUG_MK3MAG; // correct signal recd. |
63 | //DebugOut.Digital[1] ^= DEBUG_MK3MAG; // correct signal recd. |
63 | } |
64 | } |
64 | /* |
65 | /* |
65 | compassHeading - compassCourse on a -180..179 range. |
66 | compassHeading - compassCourse on a -180..179 range. |
66 | compassHeading 20 compassCourse 30 --> ((540 - 10)%360) - 180 = -10 |
67 | compassHeading 20 compassCourse 30 --> ((540 - 10)%360) - 180 = -10 |
67 | compassHeading 30 compassCourse 20 --> ((540 + 10)%360) - 180 = 10 |
68 | compassHeading 30 compassCourse 20 --> ((540 + 10)%360) - 180 = 10 |
68 | compassHeading 350 compassCourse 10 --> ((540 + 340)%360) - 180 = -20 |
69 | compassHeading 350 compassCourse 10 --> ((540 + 340)%360) - 180 = -20 |
69 | compassHeading 10 compassCourse 350 --> ((540 - 340)%360) - 180 = 20 |
70 | compassHeading 10 compassCourse 350 --> ((540 - 340)%360) - 180 = 20 |
70 | */ |
71 | */ |
71 | //compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
72 | //compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
72 | PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid |
73 | PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid |
73 | // 12 * 362 counts * 102.4 us |
74 | // 12 * 362 counts * 102.4 us |
74 | } |
75 | } |
75 | PWMCount = 0; // reset pwm counter |
76 | PWMCount = 0; // reset pwm counter |
76 | } |
77 | } |
77 | 78 | ||
78 | if (!PWMTimeout) { |
79 | if (!PWMTimeout) { |
79 | if (checkDelay(beepDelay)) { |
80 | if (checkDelay(beepDelay)) { |
80 | if (!beepTime) |
81 | if (!beepTime) |
81 | beep(50); // make noise with 10Hz to signal the compass problem |
82 | beep(50); // make noise with 10Hz to signal the compass problem |
82 | beepDelay = setDelay(100); |
83 | beepDelay = setDelay(100); |
83 | } |
84 | } |
84 | } |
85 | } |
85 | } |
86 | } |
86 | 87 | ||
87 | 88 |