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);

        // 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);

}