Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
886 killagreg 1
#include <avr/io.h>
2
#include <stdlib.h>
3
#include <inttypes.h>
4
#include "timer0.h"
5
#include "fc.h"
6
#include "rc.h"
7
#include "eeprom.h"
8
#include "mk3mag.h"
9
 
10
uint8_t PWMTimeout = 12;
11
ToMk3Mag_t ToMk3Mag;
12
 
13
 
14
/*********************************************/
15
/*  Initialize Interface to MK3MAG Compass   */
16
/*********************************************/
17
void MK3MAG_Init(void)
18
{
19
        // Port PC4 connected to PWM output from compass module
20
        DDRC &= ~(1<<DDC4); // set as input
21
        PORTC |= (1<<PORTC4); // pull up  to increase PWM counter also if nothing is connected
22
 
23
        PWMTimeout = 0;
24
 
25
        ToMk3Mag.CalState = 0;
26
        ToMk3Mag.Orientation = 1;
27
}
28
 
29
 
30
/*********************************************/
31
/*  Get PWM from MK3MAG                      */
32
/*********************************************/
33
void MK3MAG_Update(void) // called every 102.4 us by timer 0 ISR
34
{
35
        static uint16_t PWMCount = 0;
36
        static uint16_t BeepDelay = 0;
37
        // The pulse width varies from 1ms (0°) to 36.99ms (359.9°)
38
        // in other words 100us/° with a +1ms offset.
39
        // The signal goes low for 65ms between pulses,
40
        // so the cycle time is 65mS + the pulse width.
41
 
42
        // pwm is high
907 killagreg 43
 
886 killagreg 44
        if(PINC & (1<<PINC4))
45
        {       // If PWM signal is high increment PWM high counter
46
                // This counter is incremented by a periode of 102.4us,
47
                // i.e. the resoluton of pwm coded heading is approx. 1 deg.
48
                PWMCount++;
49
                // pwm overflow?
50
                if (PWMCount > 400)
51
                {
52
                        if(PWMTimeout) PWMTimeout--; // decrement timeout
53
                        CompassHeading = -1; // unknown heading
54
                        PWMCount = 0; // reset PWM Counter
55
                }
56
 
57
        }
58
        else // pwm is low
907 killagreg 59
        {   // ignore pwm values values of 0 and higher than 37 ms;
886 killagreg 60
                if((PWMCount) && (PWMCount < 362)) // 362 * 102.4us = 37.0688 ms
61
                {
62
                        if(PWMCount <10) CompassHeading = 0;
63
                        else CompassHeading = ((uint32_t)(PWMCount - 10) * 1049L)/1024; // correct timebase and offset
64
                        CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
65
                        PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid
66
                        // 12 * 362 counts * 102.4 us
67
                }
68
                PWMCount = 0; // reset pwm counter
69
        }
70
        if(!PWMTimeout)
71
        {
72
                if(CheckDelay(BeepDelay))
73
                {
74
                        if(!BeepTime) BeepTime = 100; // make noise with 10Hz to signal the compass problem
75
                        BeepDelay = SetDelay(100);
76
                }
77
        }
78
}
79
 
80
 
81