Subversion Repositories FlightCtrl

#include <avr/boot.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <util/delay.h>

#include "timer0.h"
#include "timer2.h"
#include "uart0.h"
#include "output.h"
#include "attitude.h"
#include "commands.h"
#include "flight.h"
#include "rc.h"
#include "analog.h"
#include "configuration.h"
#include "controlMixer.h"
#include "eeprom.h"
#include "printf_P.h"

uint8_t resetFlag = 0;

void reset(void) {
        resetFlag = 1;
        wdt_enable(WDTO_15MS);
        while (1)
                ;
}

int16_t main(void) {
        uint16_t timer = 0;

#ifdef DO_PROFILE
        static uint8_t profileTimer;
#endif

        // disable interrupts global
        cli();

        // wdt_enable(WDTO_120MS);
        wdt_disable();

        // analyze hardware environment
        setCPUType();
        setBoardRelease();

        // initalize modules
        output_init();
        timer0_init();
        timer2_init();
        usart0_init();
        //if (CPUType == ATMEGA644P);// usart1_Init();
        RC_Init();
        analog_init();

        // Parameter Set handling
        IMUConfig_readOrDefault();
        channelMap_readOrDefault();
        rcTrim_readOrDefault();
        paramSet_readOrDefault();

        // enable interrupts global
        sei();

        controlMixer_setNeutral();

        // Cal. attitude sensors and reset integrals.
        attitude_setNeutral();

        // Init flight parameters
        // flight_setNeutral();

        // This is not a busy-wait operation and should be OK.
        beep(2000);

        while (1) {
                if (runFlightControl) { // control interval
                        runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0

                        if (!resetFlag) {
                                wdt_reset();
                        }

                        if (!analogDataReady) {
                                // Analog data should have been ready but is not!!
                                debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
                        } else {
                                debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
                        }
                        // This is probably the correct order:
                        // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
                        // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
                        // Flight control uses results from both.
#ifdef DO_PROFILE
                        startProfileTimer(ATTITUDE);
#endif
                        calculateFlightAttitude();
#ifdef DO_PROFILE
                        stopProfileTimer(ATTITUDE);
#endif

#ifdef DO_PROFILE
                        startProfileTimer(CONTROLMIXER);
#endif
                        controlMixer_periodicTask();
#ifdef DO_PROFILE
                        stopProfileTimer(CONTROLMIXER);
#endif

#ifdef DO_PROFILE
                        startProfileTimer(COMMANDS);
#endif
                        commands_handleCommands();
#ifdef DO_PROFILE
                        stopProfileTimer(COMMANDS);
#endif

#ifdef DO_PROFILE
                        startProfileTimer(FLIGHT);
#endif
                        flight_control();
#ifdef DO_PROFILE
                        stopProfileTimer(FLIGHT);
#endif

                        // Allow Serial Data Transmit if motors must not updated or motors are not running
                        if (!runFlightControl || !isFlying) {
                                usart0_transmitTxData();
                        }

                        usart0_processRxData();

                        static uint8_t aboveWarningLimitVoltageSeen = 0;

                        if (checkDelay(timer)) {
                                if (UBat >= staticParams.batteryWarningVoltage) {
                                        aboveWarningLimitVoltageSeen = 1;
                                } else { // If we are above USB voltage, or if we have once been above warning voltage
                                        if (aboveWarningLimitVoltageSeen || UBat > UBAT_AT_5V) {
                                                beepBatteryAlarm();
                                        }
                                }
                                //calculateFeaturedServoValues();
                                timer = setDelay(20); // every 20 ms
                        }

                        output_update();

                        if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.
                                debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
                        } else {
                                debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
                        }
                }
        }
        return (1);
}