Subversion Repositories FlightCtrl

Rev

Rev 886 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 886 Rev 907
Line 1... Line 1...
1
#include <avr/io.h>
1
#include <avr/io.h>
2
#include <stdlib.h>
2
#include <stdlib.h>
3
#include <inttypes.h>
3
#include <inttypes.h>
4
#include "timer0.h"
4
#include "timer0.h"
5
#include "uart.h"
-
 
6
#include "fc.h"
5
#include "fc.h"
7
#include "rc.h"
6
#include "rc.h"
8
#include "eeprom.h"
7
#include "eeprom.h"
9
#include "mk3mag.h"
8
#include "mk3mag.h"
Line 39... Line 38...
39
        // in other words 100us/° with a +1ms offset.
38
        // in other words 100us/° with a +1ms offset.
40
        // The signal goes low for 65ms between pulses,
39
        // The signal goes low for 65ms between pulses,
41
        // so the cycle time is 65mS + the pulse width.
40
        // so the cycle time is 65mS + the pulse width.
Line 42... Line 41...
42
 
41
 
-
 
42
        // pwm is high
43
        // pwm is high
43
 
44
        if(PINC & (1<<PINC4))
44
        if(PINC & (1<<PINC4))
45
        {       // If PWM signal is high increment PWM high counter
45
        {       // If PWM signal is high increment PWM high counter
46
                // This counter is incremented by a periode of 102.4us,
46
                // This counter is incremented by a periode of 102.4us,
47
                // i.e. the resoluton of pwm coded heading is approx. 1 deg.
47
                // i.e. the resoluton of pwm coded heading is approx. 1 deg.
Line 54... Line 54...
54
                        PWMCount = 0; // reset PWM Counter
54
                        PWMCount = 0; // reset PWM Counter
55
                }
55
                }
Line 56... Line 56...
56
 
56
 
57
        }
57
        }
58
        else // pwm is low
58
        else // pwm is low
59
        {   // ignore pwm values values of 0 and higher than 37 ms
59
        {   // ignore pwm values values of 0 and higher than 37 ms;
60
                if((PWMCount) && (PWMCount < 362)) // 362 * 102.4us = 37.0688 ms
60
                if((PWMCount) && (PWMCount < 362)) // 362 * 102.4us = 37.0688 ms
61
                {
61
                {
62
                        if(PWMCount <10) CompassHeading = 0;
62
                        if(PWMCount <10) CompassHeading = 0;
63
                        else CompassHeading = ((uint32_t)(PWMCount - 10) * 1049L)/1024; // correct timebase and offset
63
                        else CompassHeading = ((uint32_t)(PWMCount - 10) * 1049L)/1024; // correct timebase and offset
64
                        CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
64
                        CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
65
                        PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid
65
                        PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid
66
                        // 12 * 362 counts * 102.4 us
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
                }
67
                }
74
                PWMCount = 0; // reset pwm counter
68
                PWMCount = 0; // reset pwm counter
75
        }
-
 
76
 
69
        }
77
        if(!PWMTimeout)
70
        if(!PWMTimeout)
78
        {
71
        {
79
                if(CheckDelay(BeepDelay))
72
                if(CheckDelay(BeepDelay))
80
                {
73
                {