Subversion Repositories FlightCtrl

Rev

Rev 907 | 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 "uart.h"
6
#include "fc.h"
7
#include "rc.h"
8
#include "eeprom.h"
9
#include "mk3mag.h"
10
 
11
uint8_t PWMTimeout = 12;
12
ToMk3Mag_t ToMk3Mag;
13
 
14
 
15
/*********************************************/
16
/*  Initialize Interface to MK3MAG Compass   */
17
/*********************************************/
18
void MK3MAG_Init(void)
19
{
20
        // Port PC4 connected to PWM output from compass module
21
        DDRC &= ~(1<<DDC4); // set as input
22
        PORTC |= (1<<PORTC4); // pull up  to increase PWM counter also if nothing is connected
23
 
24
        PWMTimeout = 0;
25
 
26
        ToMk3Mag.CalState = 0;
27
        ToMk3Mag.Orientation = 1;
28
}
29
 
30
 
31
/*********************************************/
32
/*  Get PWM from MK3MAG                      */
33
/*********************************************/
34
void MK3MAG_Update(void) // called every 102.4 us by timer 0 ISR
35
{
36
        static uint16_t PWMCount = 0;
37
        static uint16_t BeepDelay = 0;
38
        // The pulse width varies from 1ms (0°) to 36.99ms (359.9°)
39
        // in other words 100us/° with a +1ms offset.
40
        // The signal goes low for 65ms between pulses,
41
        // so the cycle time is 65mS + the pulse width.
42
 
43
        // pwm is high
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
59
        {   // ignore pwm values values of 0 and higher than 37 ms
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
                else
69
                {  // bad pwm value (out of range or permanent low)
70
                        if(PWMTimeout) PWMTimeout--; // decrement timeout
71
                        CompassHeading = -1; // unknown heading
72
                        CompassOffCourse = 0;
73
                }
74
                PWMCount = 0; // reset pwm counter
75
        }
76
 
77
        if(!PWMTimeout)
78
        {
79
                if(CheckDelay(BeepDelay))
80
                {
81
                        if(!BeepTime) BeepTime = 100; // make noise with 10Hz to signal the compass problem
82
                        BeepDelay = SetDelay(100);
83
                }
84
        }
85
}
86
 
87
 
88