Subversion Repositories FlightCtrl

Rev

Go to most recent revision | 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
                {
785 killagreg 54
                        if(PWMCount <9) PWMHeading = 0;
790 killagreg 55
                        else PWMHeading = ((uint32_t)(PWMCount - 9) * 1049L)/1024; // correct timebase and offset
755 killagreg 56
                        PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid
57
                        // 12 * 400 counts * 102.4 us = 419 ms
58
                }
59
                PWMCount = 0; // reset pwm counter
60
        }
61
}
62
 
63
 
64
/*********************************************/
65
/*  Calculate north direction (heading)      */
66
/*********************************************/
67
int16_t CMPS03_Heading(void)
68
{
765 killagreg 69
        int16_t heading, w, v;
782 killagreg 70
 
755 killagreg 71
        if(PWMTimeout)
765 killagreg 72
        {
73
                w = abs(IntegralPitch / 512);
74
                v = abs(IntegralRoll  / 512);
75
            if(v > w) w = v; // get maximum declination
783 killagreg 76
                // if declination is small enough to have valid compass heading
765 killagreg 77
                if(w < 35)
78
                {
79
                        // range from 0 to 359
80
                        heading = (int16_t)PWMHeading;
81
                        if (heading < 0) heading += 360;
82
                        heading = heading%360;
83
                }
782 killagreg 84
                else // compass to much tilted
765 killagreg 85
                {
86
                        heading = -1;
87
                }
755 killagreg 88
        }
89
        else // no data from compass
90
        {
782 killagreg 91
                if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem
755 killagreg 92
                heading = -1;
93
        }
94
        return heading;
95
}