Subversion Repositories FlightCtrl

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Copyright (c) 04.2007 Holger Buss

// + Nur f�r den privaten Gebrauch

// + www.MikroKopter.com

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),

// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zul�ssig ist.

// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt

// + bzgl. der Nutzungsbedingungen aufzunehmen.

// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen,

// + Verkauf von Luftbildaufnahmen, usw.

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht,

// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts

// + auf anderen Webseiten oder Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"

// + eindeutig als Ursprung verlinkt und genannt werden

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion

// + Benutzung auf eigene Gefahr

// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur

// + mit unserer Zustimmung zul�ssig

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Redistributions of source code (with or without modifications) must retain the above copyright notice,

// + this list of conditions and the following disclaimer.

// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived

// +     from this software without specific prior written permission.

// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet

// +     for non-commercial use (directly or indirectly)

// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted

// +     with our written permission

// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be

// +     clearly linked as origin

// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed

// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

// +  POSSIBILITY OF SUCH DAMAGE.

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#include <avr/boot.h>

#include <avr/io.h>

#include <avr/interrupt.h>

#include <util/delay.h>

#include "timer0.h"

#include "timer2.h"

#include "uart0.h"

#include "uart1.h"

#include "output.h"

#include "menu.h"

#include "attitude.h"

#include "flight.h"

#include "controlMixer.h"

#include "rc.h"

#include "analog.h"

#include "configuration.h"

#include "printf_P.h"

#include "twimaster.h"

#ifdef USE_NAVICTRL

#include "spi.h"

#endif

#ifdef USE_MK3MAG

#include "mk3mag.h"

#endif

#include "eeprom.h"

int16_t main(void) {

        uint16_t timer;

        // disable interrupts global

        cli();

        // analyze hardware environment

        CPUType = getCPUType();

        BoardRelease = getBoardRelease();

        // disable watchdog

        MCUSR &= ~(1 << WDRF);

        WDTCSR |= (1 << WDCE) | (1 << WDE);

        WDTCSR = 0;

        // PPM_in[CH_THROTTLE] = 0;

        // Why??? They are already initialized to 0.

        // stickPitch = stickRoll = stickYaw = 0;

        RED_OFF;

        // initalize modules

        output_init();

        timer0_init();

        timer2_init();

        usart0_Init();

        if (CPUType == ATMEGA644P)

                usart1_Init();

        RC_Init();

        analog_init();

        I2C_init();

#ifdef USE_NAVICTRL

        SPI_MasterInit();

#endif

#ifdef USE_MK3MAG

        MK3MAG_Init();

#endif

        // enable interrupts global

        sei();

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

        printf("\n\rFlightControl");

        printf("\n\rHardware: Custom");

        printf("\r\n     CPU: Atmega644");

        if (CPUType == ATMEGA644P)

                printf("p");

        printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');

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

        // Parameter Set handling

        ParamSet_Init();

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

        OUTPUT_SET(0,1);

        GRN_OFF;

        RED_ON;

        while (!CheckDelay(timer))

                ;

        OUTPUT_SET(0,0);

        timer = SetDelay(250);

        while (!CheckDelay(timer))

                ;

        timer = SetDelay(250);

        OUTPUT_SET(1,1);

        RED_OFF;

        GRN_ON;

        while (!CheckDelay(timer))

                ;

        timer = SetDelay(250);

        while (!CheckDelay(timer))

                ;

        OUTPUT_SET(1,0);

        twi_diagnostics();

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

        /*

         if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL)

         {

         printf("\n\rCalibrating air pressure sensor..");

         timer = SetDelay(1000);

         SearchAirPressureOffset();

         while (!CheckDelay(timer));

         printf("OK\n\r");

         }

         */

#ifdef USE_NAVICTRL

        printf("\n\rSupport for NaviCtrl");

#ifdef USE_RC_DSL

        printf("\r\nSupport for DSL RC at 2nd UART");

#endif

#ifdef USE_RC_SPECTRUM

        printf("\r\nSupport for SPECTRUM RC at 2nd UART");

#endif

#endif

#ifdef USE_MK3MAG

        printf("\n\rSupport for MK3MAG Compass");

#endif

#if (defined (USE_MK3MAG))

        if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART");

        else printf("\n\rSupport for GPS at 1st UART");

#endif

        controlMixer_setNeutral();

        // Cal. attitude sensors and reset integrals.

        attitude_setNeutral();

        Servo_On();

        // Init flight parameters

        flight_setNeutral();

        // RED_OFF;

        beep(2000);

        printf("\n\rControl: ");

        if (staticParams.GlobalConfig & CFG_HEADING_HOLD)

                printf("HeadingHold");

                else printf("Neutral (ACC-Mode)");

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

        LCD_Clear();

        I2CTimeout = 5000;

        while (1) {

                if (runFlightControl && analogDataReady) { // control interval

                        runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0

                        J4HIGH;

                        flight_control();

                        J4LOW;

                        /*

                         * If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit

                         * the throttle vector just computed. Otherwise, if motor test is engaged, transmit

                         * the test throttle vector. If no testing, stop all motors.

                         */

                        // Obsoleted.

                        // transmitMotorThrottleData();

                        RED_OFF;

                        /*

                         Does not belong here. Instead, external control should be ignored in

                         controlMixer if there was no new data from there for some time.

                         if(externalControlActive) externalControlActive--;

                         else {

                         externalControl.config = 0;

                         externalStickPitch = 0;

                         externalStickRoll = 0;

                         externalStickYaw = 0;

                         }

                         */

                        /*

                         Does not belong here.

                         if(RC_Quality)  RC_Quality--;

                         */

                        /* Does not belong here. Well since we are not supporting navi right now anyway, leave out.

                         #ifdef USE_NAVICTRL

                         if(NCDataOkay) {

                         if(--NCDataOkay == 0) // no data from NC

                         {  // set gps control sticks neutral

                         GPSStickPitch = 0;

                         GPSStickRoll = 0;

                         NCSerialDataOkay = 0;

                         }

                         }

                         #endif

                         */

                        if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing ot timeout

                                RED_ON;

                                if (!I2CTimeout) {

                                        I2C_Reset();

                                        I2CTimeout = 5;

                                }

                        } else {

                                RED_OFF;

                        }

                        // Allow Serial Data Transmit if motors must not updated or motors are not running

                        if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) {

                                usart0_TransmitTxData();

                        }

                        usart0_ProcessRxData();

                        if (CheckDelay(timer)) {

                                if (UBat <= UBAT_AT_5V) {

                                        // Do nothing. The voltage on the input side of the regulator is <5V;

                                        // we must be running off USB power. Keep it quiet.

                                } else if (UBat < staticParams.LowVoltageWarning) {

                                        beepBatteryAlarm();

                                }

#ifdef USE_NAVICTRL

                                SPI_StartTransmitPacket();

                                SendSPI = 4;

#endif

                                timer = SetDelay(20); // every 20 ms

                        }

                        output_update();

                }

#ifdef USE_NAVICTRL

                if(!SendSPI) {

                        // SendSPI is decremented in timer0.c with a rate of 9.765 kHz.

                        // within the SPI_TransmitByte() routine the value is set to 4.

                        // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,

                        // and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.

                        SPI_TransmitByte();

                }

#endif

        }

        return (1);

}