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