Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
1910 - 1
#include <avr/boot.h>
2
#include <avr/io.h>
3
#include <avr/interrupt.h>
2132 - 4
#include <avr/wdt.h>
1910 - 5
#include <util/delay.h>
6
 
7
#include "timer0.h"
8
#include "timer2.h"
9
#include "uart0.h"
10
#include "output.h"
11
#include "attitude.h"
2099 - 12
#include "commands.h"
1910 - 13
#include "flight.h"
14
#include "rc.h"
15
#include "analog.h"
16
#include "configuration.h"
2099 - 17
#include "controlMixer.h"
1910 - 18
#include "eeprom.h"
2099 - 19
#include "printf_P.h"
1910 - 20
 
2132 - 21
uint8_t resetFlag = 0;
22
 
23
void reset(void) {
24
        resetFlag = 1;
25
        wdt_enable(WDTO_15MS);
26
        while (1)
27
                ;
28
}
29
 
1910 - 30
int16_t main(void) {
2132 - 31
        uint16_t timer = 0;
1910 - 32
 
2132 - 33
#ifdef DO_PROFILE
34
        static uint8_t profileTimer;
35
#endif
1910 - 36
 
2132 - 37
        // disable interrupts global
38
        cli();
1910 - 39
 
2137 - 40
        // wdt_enable(WDTO_120MS);
41
        wdt_disable();
1910 - 42
 
2132 - 43
        // analyze hardware environment
44
        setCPUType();
45
        setBoardRelease();
1910 - 46
 
2132 - 47
        // initalize modules
48
        output_init();
49
        timer0_init();
50
        timer2_init();
51
        usart0_init();
52
        //if (CPUType == ATMEGA644P);// usart1_Init();
53
        RC_Init();
54
        analog_init();
1910 - 55
 
2132 - 56
        // Parameter Set handling
57
        IMUConfig_readOrDefault();
58
        channelMap_readOrDefault();
59
        rcTrim_readOrDefault();
60
        paramSet_readOrDefault();
2099 - 61
 
2132 - 62
        // enable interrupts global
63
        sei();
1910 - 64
 
2132 - 65
        controlMixer_setNeutral();
2099 - 66
 
2132 - 67
        // Cal. attitude sensors and reset integrals.
68
        attitude_setNeutral();
2099 - 69
 
2132 - 70
        // Init flight parameters
71
        // flight_setNeutral();
1910 - 72
 
2132 - 73
        // This is not a busy-wait operation and should be OK.
74
        beep(2000);
1910 - 75
 
2132 - 76
        while (1) {
77
                if (runFlightControl) { // control interval
78
                        runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
2125 - 79
 
2132 - 80
                        if (!resetFlag) {
81
                                wdt_reset();
82
                        }
83
 
84
                        if (!analogDataReady) {
85
                                // Analog data should have been ready but is not!!
86
                                debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
87
                        } else {
88
                                debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
89
                        }
90
                        // This is probably the correct order:
91
                        // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
92
                        // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
93
                        // Flight control uses results from both.
2124 - 94
#ifdef DO_PROFILE
2132 - 95
                        startProfileTimer(ATTITUDE);
2124 - 96
#endif
2132 - 97
                        calculateFlightAttitude();
2124 - 98
#ifdef DO_PROFILE
2132 - 99
                        stopProfileTimer(ATTITUDE);
2124 - 100
#endif
101
 
102
#ifdef DO_PROFILE
2132 - 103
                        startProfileTimer(CONTROLMIXER);
2124 - 104
#endif
2132 - 105
                        controlMixer_periodicTask();
2124 - 106
#ifdef DO_PROFILE
2132 - 107
                        stopProfileTimer(CONTROLMIXER);
2124 - 108
#endif
109
 
110
#ifdef DO_PROFILE
2132 - 111
                        startProfileTimer(COMMANDS);
2124 - 112
#endif
2132 - 113
                        commands_handleCommands();
2124 - 114
#ifdef DO_PROFILE
2132 - 115
                        stopProfileTimer(COMMANDS);
2124 - 116
#endif
117
 
118
#ifdef DO_PROFILE
2132 - 119
                        startProfileTimer(FLIGHT);
2124 - 120
#endif
2132 - 121
                        flight_control();
2124 - 122
#ifdef DO_PROFILE
2132 - 123
                        stopProfileTimer(FLIGHT);
2124 - 124
#endif
2125 - 125
 
2132 - 126
                        // Allow Serial Data Transmit if motors must not updated or motors are not running
127
                        if (!runFlightControl || !isFlying) {
128
                                usart0_transmitTxData();
129
                        }
1910 - 130
 
2132 - 131
                        usart0_processRxData();
1910 - 132
 
2132 - 133
                        static uint8_t aboveWarningLimitVoltageSeen = 0;
2122 - 134
 
2132 - 135
                        if (checkDelay(timer)) {
136
                                if (UBat >= staticParams.batteryWarningVoltage) {
137
                                        aboveWarningLimitVoltageSeen = 1;
138
                                } else { // If we are above USB voltage, or if we have once been above warning voltage
139
                                        if (aboveWarningLimitVoltageSeen || UBat > UBAT_AT_5V) {
140
                                                beepBatteryAlarm();
141
                                        }
142
                                }
143
                                calculateFeaturedServoValues();
144
                                timer = setDelay(20); // every 20 ms
145
                        }
2125 - 146
 
2132 - 147
                        output_update();
2125 - 148
 
2132 - 149
                        if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.
150
                                debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
151
                        } else {
152
                                debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
153
                        }
154
                }
155
        }
156
        return (1);
1910 - 157
}