Subversion Repositories FlightCtrl

Rev

Rev 762 | Go to most recent revision | Details | Last modification | View Log | RSS feed

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