Subversion Repositories FlightCtrl

Rev

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
                PWMCount = 0; // reset pwm counter
59
        }
60
}
61
 
62
 
63
/*********************************************/
64
/*  Calculate north direction (heading)      */
65
/*********************************************/
66
int16_t CMPS03_Heading(void)
67
{
765 killagreg 68
        int16_t heading, w, v;
782 killagreg 69
 
70
        DebugOut.Analog[11] = PWMTimeout;
71
 
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
783 killagreg 77
                // if declination is small enough to have valid compass heading
765 killagreg 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
                }
782 killagreg 85
                else // compass to much tilted
765 killagreg 86
                {
87
                        heading = -1;
88
                }
755 killagreg 89
        }
90
        else // no data from compass
91
        {
782 killagreg 92
                if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem
755 killagreg 93
                heading = -1;
94
        }
95
        return heading;
96
}