Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
755 killagreg 1
#include <avr/io.h>
765 killagreg 2
#include "fc.h"
3
#include "timer0.h"
755 killagreg 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
{
764 killagreg 13
        // Port PC4 connected to PWM output from compass module
755 killagreg 14
        DDRC &= ~(1<<DDC4); // set as input
764 killagreg 15
        PORTC |= (1<<PORTC4); // pull up  to increase PWM counter also if nothing is connected
755 killagreg 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
{
765 killagreg 71
        int16_t heading, w, v;
755 killagreg 72
        if(PWMTimeout)
765 killagreg 73
        {
74
                w = abs(IntegralPitch / 512);
75
                v = abs(IntegralRoll  / 512);
76
            if(v > w) w = v; // get maximum declination
77
                // if declination is small enough do have valid compass heading
78
                if(w < 35)
79
                {
80
                        // range from 0 to 359
81
                        heading = (int16_t)PWMHeading;
82
                        if (heading < 0) heading += 360;
83
                        heading = heading%360;
84
                }
85
                else // no date available from compass
86
                {
87
                        if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem
88
                        heading = -1;
89
                }
755 killagreg 90
        }
91
        else // no data from compass
92
        {
93
                heading = -1;
94
        }
95
        return heading;
96
}