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