Subversion Repositories FlightCtrl

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

#include "timer0.h"
#include "timer2.h"
#include "uart0.h"
//#include "uart1.h"
#include "output.h"
#include "attitude.h"
//#include "commands.h"
//#include "flight.h"
//#include "profiler.h"
#include "rc.h"
#include "analog.h"
#include "configuration.h"
#include "twimaster.h"
#include "controlMixer.h"
#include "eeprom.h"
#include "beeper.h"
#ifdef USE_MK3MAG
#include "mk3mag.h"
#endif

int16_t main(void) {
        uint16_t timer;

        // disable interrupts global
        cli();

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

        fdevopen(uart_putchar, NULL);

        // disable watchdog
        MCUSR &= ~(1 << WDRF);
        WDTCSR |= (1 << WDCE) | (1 << WDE);
        WDTCSR = 0;

        // initalize modules
        output_init();
        timer0_init();
        // timer2_init();
        usart0_init();
        //if (CPUType == ATMEGA644P);// usart1_Init();
        RC_Init();
        I2C_init();
        analog_init();
#ifdef USE_MK3MAG
        MK3MAG_init();
#endif
#ifdef USE_DIRECT_GPS
        usart1_init();
#endif

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

        // enable interrupts global
        sei();

        printf("\n\r===================================");
        printf("\n\rFlightControl");
        printf("\n\rHardware: Custom");
        printf("\n\r     CPU: Atmega644");
        if (CPUType == ATMEGA644P)
                printf("p");
        printf("\n\rSoftware: V%d.%d%c", VERSION_MAJOR, VERSION_MINOR,
                        VERSION_PATCH + 'a');
        printf("\n\r===================================");

        // Wait for a short time (otherwise the RC channel check won't work below)
        // timer = SetDelay(500);
        // while(!CheckDelay(timer));

        // Instead, while away the time by flashing the 2 outputs:
        // First J16, then J17. Makes it easier to see which is which.
        timer = setDelay(200);
        output_setLED(0, 1);
        GRN_OFF;
        RED_ON;
        while (!checkDelay(timer))
                ;

        timer = setDelay(200);
        output_setLED(0, 0);
        output_setLED(1, 1);
        RED_OFF;
        GRN_ON;
        while (!checkDelay(timer))
                ;

        timer = setDelay(200);
        while (!checkDelay(timer))
                ;
        output_setLED(1, 0);
        GRN_OFF;

        twi_diagnostics();

        printf("\n\r===================================");

#ifdef USE_NAVICTRL
        printf("\n\rSupport for NaviCtrl");
#endif

#ifdef USE_DIRECT_GPS
        printf("\n\rDirect (no NaviCtrl) navigation");
#endif

        controlMixer_setNeutral();

        I2CTimeout = 5000;

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

        // Init flight parameters
        // flight_setNeutral();

        beep(2000);
        printf("\n\n\r");

        timer2_init();
       
        /*
         * Main loop updates attitude and does some nonessential tasks
         * like beeping alarms and computing servo values.
         */
        while (1) {
                attitude_update();

        // This is fair to leave here - servo values only need update after a change in attitude anyway.
        // calculateServoValues();

                if (UBat <= UBAT_AT_5V || UBat >= staticParams.batteryVoltageWarning) {
                        // Do nothing. The voltage on the input side of the regulator is <5V;
                        // we must be running off USB power. Keep it quiet.
          MKFlags &= ~MKFLAG_LOWBAT;
                } else {
                        beepBatteryAlarm();
                        MKFlags |= MKFLAG_LOWBAT;
                }
        }
        return 1;
}