Subversion Repositories FlightCtrl

Rev

Rev 2158 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1612 dongfang 1
#include <avr/boot.h>
2
#include <avr/io.h>
3
#include <avr/interrupt.h>
4
#include <util/delay.h>
2189 - 5
#include <stdio.h>
1612 dongfang 6
 
7
#include "timer0.h"
8
#include "timer2.h"
9
#include "uart0.h"
2189 - 10
//#include "uart1.h"
1612 dongfang 11
#include "output.h"
12
#include "attitude.h"
2189 - 13
//#include "commands.h"
14
//#include "flight.h"
15
//#include "profiler.h"
1612 dongfang 16
#include "rc.h"
17
#include "analog.h"
18
#include "configuration.h"
19
#include "twimaster.h"
1969 - 20
#include "controlMixer.h"
2189 - 21
#include "eeprom.h"
22
#include "beeper.h"
2052 - 23
#ifdef USE_MK3MAG
1612 dongfang 24
#include "mk3mag.h"
25
#endif
26
 
1821 - 27
int16_t main(void) {
2189 - 28
        uint16_t timer;
1612 dongfang 29
 
2189 - 30
        // disable interrupts global
31
        cli();
1612 dongfang 32
 
2189 - 33
        // analyze hardware environment
34
        setCPUType();
35
        setBoardRelease();
1612 dongfang 36
 
2189 - 37
        fdevopen(uart_putchar, NULL);
1612 dongfang 38
 
2189 - 39
        // disable watchdog
40
        MCUSR &= ~(1 << WDRF);
41
        WDTCSR |= (1 << WDCE) | (1 << WDE);
42
        WDTCSR = 0;
1969 - 43
 
2189 - 44
        // initalize modules
45
        output_init();
46
        timer0_init();
47
        // timer2_init();
48
        usart0_init();
49
        //if (CPUType == ATMEGA644P);// usart1_Init();
50
        RC_Init();
51
        I2C_init();
52
        analog_init();
2052 - 53
#ifdef USE_MK3MAG
2189 - 54
        MK3MAG_init();
2052 - 55
#endif
2039 - 56
#ifdef USE_DIRECT_GPS
2189 - 57
        usart1_init();
1612 dongfang 58
#endif
59
 
2189 - 60
        // Parameter Set handling
61
        IMUConfig_readOrDefault();
62
        channelMap_readOrDefault();
63
        outputMixer_readOrDefault();
64
        paramSet_readOrDefault();
1969 - 65
 
2189 - 66
        // enable interrupts global
67
        sei();
1612 dongfang 68
 
2189 - 69
        printf("\n\r===================================");
70
        printf("\n\rFlightControl");
71
        printf("\n\rHardware: Custom");
72
        printf("\n\r     CPU: Atmega644");
73
        if (CPUType == ATMEGA644P)
74
                printf("p");
75
        printf("\n\rSoftware: V%d.%d%c", VERSION_MAJOR, VERSION_MINOR,
76
                        VERSION_PATCH + 'a');
77
        printf("\n\r===================================");
1612 dongfang 78
 
2189 - 79
        // Wait for a short time (otherwise the RC channel check won't work below)
80
        // timer = SetDelay(500);
81
        // while(!CheckDelay(timer));
1612 dongfang 82
 
2189 - 83
        // Instead, while away the time by flashing the 2 outputs:
84
        // First J16, then J17. Makes it easier to see which is which.
85
        timer = setDelay(200);
86
        output_setLED(0, 1);
87
        GRN_OFF;
88
        RED_ON;
89
        while (!checkDelay(timer))
90
                ;
1612 dongfang 91
 
2189 - 92
        timer = setDelay(200);
93
        output_setLED(0, 0);
94
        output_setLED(1, 1);
95
        RED_OFF;
96
        GRN_ON;
97
        while (!checkDelay(timer))
98
                ;
1849 - 99
 
2189 - 100
        timer = setDelay(200);
101
        while (!checkDelay(timer))
102
                ;
103
        output_setLED(1, 0);
104
        GRN_OFF;
1612 dongfang 105
 
2189 - 106
        twi_diagnostics();
1634 - 107
 
2189 - 108
        printf("\n\r===================================");
1612 dongfang 109
 
110
#ifdef USE_NAVICTRL
2189 - 111
        printf("\n\rSupport for NaviCtrl");
1612 dongfang 112
#endif
113
 
2039 - 114
#ifdef USE_DIRECT_GPS
2189 - 115
        printf("\n\rDirect (no NaviCtrl) navigation");
1612 dongfang 116
#endif
117
 
2189 - 118
        controlMixer_setNeutral();
1612 dongfang 119
 
2189 - 120
        I2CTimeout = 5000;
1612 dongfang 121
 
2189 - 122
        // Cal. attitude sensors and reset integrals.
123
        attitude_setNeutral();
1612 dongfang 124
 
2189 - 125
        // Init flight parameters
126
        // flight_setNeutral();
1612 dongfang 127
 
2189 - 128
        beep(2000);
129
        printf("\n\n\r");
1612 dongfang 130
 
2189 - 131
        timer2_init();
132
 
133
        /*
134
         * Main loop updates attitude and does some nonessential tasks
135
         * like beeping alarms and computing servo values.
136
         */
137
        while (1) {
138
                attitude_update();
1612 dongfang 139
 
2189 - 140
        // This is fair to leave here - servo values only need update after a change in attitude anyway.
141
        // calculateServoValues();
1612 dongfang 142
 
2189 - 143
                if (UBat <= UBAT_AT_5V || UBat >= staticParams.batteryVoltageWarning) {
144
                        // Do nothing. The voltage on the input side of the regulator is <5V;
145
                        // we must be running off USB power. Keep it quiet.
146
          MKFlags &= ~MKFLAG_LOWBAT;
147
                } else {
148
                        beepBatteryAlarm();
149
                        MKFlags |= MKFLAG_LOWBAT;
150
                }
151
        }
152
        return 1;
1612 dongfang 153
}