WebSVN - NaviCtrl - Diff - Rev 320 and 324


/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 2008 Ingo Busker, Holger Buss
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
// + FOR NON COMMERCIAL USE ONLY
// + 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 (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 sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt 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 oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Teile, die nicht eindeutig mit unserer Lizenzvereinbarung gekennzeichnet sind, unterliegen u.U. eigenen Vereinbarungen:
// + z.B. Die Funktion printf_P() unterliegt ihrer eigenen Lizenz
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 permitted
// +     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 the sources to other systems or using the software on other systems (except 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.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//#define MCLK96MHZ
const unsigned long _Main_Crystal = 25000;
#include <stdio.h>
#include "91x_lib.h"
#include "led.h"
#include "uart0.h"
#include "uart1.h"
#include "uart2.h"
#include "gps.h"
#include "i2c.h"
#include "compass.h"
#include "timer1.h"
#include "timer2.h"
#include "analog.h"
#include "spi_slave.h"
#include "fat16.h"
#include "usb.h"
#include "sdc.h"
#include "logging.h"
#include "params.h"
#include "settings.h"
#include "config.h"
#include "main.h"
#include "debug.h"
#include "eeprom.h"
 
#ifdef FOLLOW_ME
u8 TransmitAlsoToFC = 0;
#endif
u32 TimerCheckError;
u8 ErrorCode = 0;
u16 BeepTime;
u8  NCFlags = 0;
s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg
 
u8 ClearFCStatusFlags = 0;
u8 StopNavigation = 0;
Param_t Parameter;
volatile FC_t FC;
 
s8 ErrorMSG[25];
 
//----------------------------------------------------------------------------------------------------
void SCU_Config(void)
{
        /* configure PLL and set it as master clock source */
        SCU_MCLKSourceConfig(SCU_MCLK_OSC);             // set master clock source to external oscillator clock (25MHz) before diabling the PLL
        SCU_PLLCmd(DISABLE);                                    // now disable the PLL
        #ifdef MCLK96MHZ
        SCU_BRCLKDivisorConfig(SCU_BRCLK_Div2); // set BRCLK to MCLK/2 = 48MHz
        SCU_PCLKDivisorConfig(SCU_PCLK_Div4);   // set PCLK     (APB bus clock) divisor to 4 (half Reference Clock)
        SCU_RCLKDivisorConfig(SCU_RCLK_Div2);   // set RCLK     (Reference Clock) divisor to 1 (full PPL clock)
        SCU_HCLKDivisorConfig(SCU_HCLK_Div2);   // set HCLK     (AHB bus clock) divisor to 1 (full Reference Clock)
        SCU_PLLFactorsConfig(192,25,2);                 // PLL = 96 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=2
        #else
        SCU_BRCLKDivisorConfig(SCU_BRCLK_Div1); // set BRCLK to MCLK = 48MHz
        SCU_PCLKDivisorConfig(SCU_PCLK_Div2);   // set PCLK     (APB bus clock) divisor to 2 (half Reference Clock)
        SCU_RCLKDivisorConfig(SCU_RCLK_Div1);   // set RCLK     (Reference Clock) divisor to 1 (full PPL clock)
        SCU_HCLKDivisorConfig(SCU_HCLK_Div1);   // set HCLK     (AHB bus clock) divisor to 1 (full Reference Clock)
        SCU_PLLFactorsConfig(192,25,3);                 // PLL = 48 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=3
        #endif
        SCU_PLLCmd(ENABLE);                                     // Enable PLL (is disabled by SCU_PLLFactorsConfig)
        SCU_MCLKSourceConfig(SCU_MCLK_PLL);     // set master clock source to PLL
}
 
//----------------------------------------------------------------------------------------------------
void GetNaviCtrlVersion(void)
{
        u8 msg[25];
 
        sprintf(msg,"\n\r NaviCtrl HW: V%d.%d SW: V%d.%d%c",  Version_HW/10, Version_HW%10, VERSION_MAJOR,  VERSION_MINOR, 'a'+ VERSION_PATCH);
        UART1_PutString(msg);
}
 
//----------------------------------------------------------------------------------------------------
 
void CheckErrors(void)
{
        UART_VersionInfo.HardwareError[0] = 0;
 
        if(CheckDelay(I2C1_Timeout) || (Compass_Heading < 0)) DebugOut.Status[1] |= 0x08;
        else DebugOut.Status[1] &= ~0x08; // MK3Mag green status
 
        if((FC.Error[1] & FC_ERROR1_I2C) || (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.Status[1] |= 0x02;
        else DebugOut.Status[1] &= ~0x02; // BL-Ctrl green status
 
        if(CheckDelay(SPI0_Timeout))
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"no FC communication ");
                ErrorCode = 3;
                StopNavigation = 1;
                DebugOut.Status[0] &= ~0x01; // status of FC Present
                DebugOut.Status[0] &= ~0x02; // status of BL Present
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX;
        }
        else if(CheckDelay(I2C1_Timeout))
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"no compass communication ");
                //Reset I2CBus
                I2C1_Deinit();
                I2C1_Init();
                ErrorCode = 4;
                StopNavigation = 1;
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX;
                DebugOut.Status[1] |= 0x08;
        }
        else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
        {
                LED_RED_ON;
#ifndef FOLLOW_ME
                sprintf(ErrorMSG,"FC not compatible ");
#else
                sprintf(ErrorMSG,"! FollowMe only ! ");
#endif
                ErrorCode = 1;
                StopNavigation = 1;
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_FC_INCOMPATIBLE;
        }
 
        else if(FC.Error[0] & FC_ERROR0_GYRO_NICK)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Nick Gyro");
                ErrorCode = 10;
        }
        else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Roll Gyro");
                ErrorCode = 11;
        }
        else if(FC.Error[0] & FC_ERROR0_GYRO_YAW)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Yaw Gyro");
                ErrorCode = 12;
        }
        else if(FC.Error[0] & FC_ERROR0_ACC_NICK)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Nick ACC");
                ErrorCode = 13;
        }
        else if(FC.Error[0] & FC_ERROR0_ACC_ROLL)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Roll ACC");
                ErrorCode = 14;
        }
        else if(FC.Error[0] & FC_ERROR0_ACC_TOP)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Z-ACC");
                ErrorCode = 15;
        }
        else if(FC.Error[0] & FC_ERROR0_PRESSURE)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: Pressure sensor");
                ErrorCode = 16;
        }
        else if(FC.Error[1] &  FC_ERROR1_I2C)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC I2C");
                ErrorCode = 17;
        }
        else if(FC.Error[1] &  FC_ERROR1_BL_MISSING)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: Bl Missing");
                ErrorCode = 18;
        }
        else if(FC.Error[1] &  FC_ERROR1_MIXER)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"Mixer Error");
                ErrorCode = 19;
        }
        else if(CheckDelay(UBX_Timeout))
        {
                LED_RED_ON;
        if(!(Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) sprintf(ErrorMSG,"GPS disconnected ");
                else
                 {
                  sprintf(ErrorMSG,"no GPS communication ");
                  UART_VersionInfo.HardwareError[0] |= NC_ERROR0_GPS_RX;
              ErrorCode = 5;
                 }
                StopNavigation = 1;
//              UBX_Timeout = SetDelay(500);
        }
        else if(Compass_Heading < 0)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"bad compass value ");
                ErrorCode = 6;
                StopNavigation = 1;
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
        }
        else if((FC.Error[1] &  FC_ERROR1_SPI_RX))
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"FC spi rx error ");
                ErrorCode = 8;
                StopNavigation = 1;
        }
        else if(FC.Error[0] &  FC_ERROR0_CAREFREE)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"FC: Carefree Error");
                ErrorCode = 20;
        }
        else if(FC.Error[1] &  FC_ERROR1_PPM)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"RC Signal lost ");
                ErrorCode = 7;
        }
        else // no error occured
        {
                sprintf(ErrorMSG,"No Error               ");
                ErrorCode = 0;
                StopNavigation = 0;
                LED_RED_OFF;
        }
        if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 0x04;
        else DebugOut.Status[1] &= ~0x04;
}
 
// the handler will be cyclic called by the timer 1 ISR
// used is for critical timing parts that normaly would handled
// within the main loop that could block longer at logging activities
void EXTIT3_IRQHandler(void)
{
        IENABLE;
 
        VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq
        VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit
        Compass_Update();               // update compass communication
        Analog_Update();                // get new ADC values
        VIC_ITCmd(EXTIT3_ITLine, ENABLE); // enable irq
 
        IDISABLE;
}
 
//----------------------------------------------------------------------------------------------------
int main(void)
{
        /*
        static u32 ftimer =0;
        static u8 fstate = 0;
        static File_t* f = NULL;
        */
       
        /* Configure the system clocks */
        SCU_Config();
        /* init VIC (Vectored Interrupt Controller)     */
        SCU_AHBPeriphClockConfig(__VIC,ENABLE); // enable AHB bus clock for VIC
        SCU_AHBPeriphReset(__VIC, DISABLE);             // disable reset state for VIC
        VIC_DeInit();                                                   // deinitializes the VIC module registers to their default reset values.
        VIC_InitDefaultVectors();
 
        // initialize timer 1 for System Clock and delay rountines
        TIMER1_Init();
        // initialize the LEDs (needs Timer 1)
        Led_Init();
        // initialize the debug UART1
        UART1_Init();
        UART1_PutString("\r\n---------------------------------------------");
        // initialize usb
        USB_ConfigInit();
        // initialize timer 2 for servo outputs
        //TIMER2_Init();
        // initialize UART2 to FLIGHTCTRL
        UART2_Init();
        // initialize UART0 (to MKGPS or MK3MAG)
        UART0_Init();
        // initialize adc
        Analog_Init();
        // initialize SPI0 to FC
        SPI0_Init();
        // initialize i2c bus (needs Timer 1)
        I2C1_Init();
        // initialize fat16 partition on sd card (needs Timer 1)
        Fat16_Init();
        // initialize NC params
        NCParams_Init();
        // initialize the settings
        Settings_Init();
        // initialize logging (needs settings)
        Logging_Init();
 
        TimerCheckError = SetDelay(3000);
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
        UART1_PutString("\n\r Version information:");
 
        GetNaviCtrlVersion();
        DebugOut.Status[0] = 0x04 | 0x08; // NC and MK3Mag
        DebugOut.Status[1] = 0x00;
 
        Compass_Init();
 
#ifdef FOLLOW_ME
        TransmitAlsoToFC = 1;
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
        UART1_PutString("\n\r FOLLOW-ME Transmitter only!");
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++\r\n\r\n");
        TransmitAlsoToFC = 0;
#else
        SPI0_GetFlightCtrlVersion();
        if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
        {
                UART1_PutString("\n\r Flight-Ctrl not compatible");
                LED_RED_ON;
        }
#endif
 
        GPS_Init();
 
        // ---------- Prepare the isr driven
        // set to absolute lowest priority
    VIC_Config(EXTIT3_ITLine, VIC_IRQ, PRIORITY_SW);
        // enable interrupts
    VIC_ITCmd(EXTIT3_ITLine, ENABLE);
 
        Debug_OK("START");
 
        for (;;) // the endless main loop
        {
                UART0_ProcessRxData();  // process request
                UART1_ProcessRxData();  // process request
                USB_ProcessRxData();    // process request
                UART0_TransmitTxData(); // send answer
                UART1_TransmitTxData(); // send answer
                UART2_TransmitTxData(); // send answer
                USB_TransmitTxData();   // send answer
                SPI0_UpdateBuffer();    // handle new SPI Data
                // ---------------- Error Check Timing ----------------------------
                if(CheckDelay(TimerCheckError))
                {
                        TimerCheckError = SetDelay(1000);
                        if(CheckDelay(SPI0_Timeout)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected
                        CheckErrors();
                        if(FC.StatusFlags & FC_STATUS_FLY) NaviData.FlyingTime++; // we want to count the battery-time
//                      else NaviData.FlyingTime = 0; // not the time per flight
                        if(SerialLinkOkay) SerialLinkOkay--;
                        if(SerialLinkOkay < 250 - 5) NCFlags |= NC_FLAG_NOSERIALLINK; // 5 seconds timeout for serial communication
                        else NCFlags &= ~NC_FLAG_NOSERIALLINK;
                        if(StopNavigation && (Parameter.NaviGpsModeControl >=  50) && (Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) BeepTime = 1000;
                }
                // ---------------- Logging  ---------------------------------------
                Logging_Update();  // could be block some time for at max. 2 seconds, therefore move time critical part of the mainloop into the ISR of timer 1
                /*
                // test
                if(CheckDelay(ftimer))
                {
 
                        s8* filename = "test.txt";
 
                        ftimer = SetDelay(100);
                        if(FC.Poti[3]>100 && fstate == 0)
                        {
                                fstate = 1;
                        }
                        else if(FC.Poti[3]<100 && fstate == 2)
                        {
                                fstate = 3;
                        }
 
                        switch(fstate)
                        {
                                case 1:
                                        sprintf(text,"\r\nStart writing file: %s", filename);
                                        UART1_PutString(text);
                                        f = fopen_(filename, 'a');
                                        if(f== NULL) Fat16_Init();
                                        fstate = 2;
                                        break;
 
                                case 2:
                                        fputs_("\r\ntest edins sdv dsivbds iv dsivb disbv idsv bisd bv d suiv dsibsivbdis fbvisdöb visdbvisdb vidbfibds ibv", f);
                                        break;
                                       
                                case 3:
                                        sprintf(text,"\r\nClosing file: %s", filename);
                                        UART1_PutString(text);
                                        fclose_(f);
                                        fstate = 0;
                                        break;
 
                                default:
                                        break;
                        }
                }
                */
        }
}
 
 
 

Rev 320	Rev 324
1	/#######################################################################################/	1	/#######################################################################################/
2	/* !!! THIS IS NOT FREE SOFTWARE !!! */	2	/* !!! THIS IS NOT FREE SOFTWARE !!! */
3	/#######################################################################################/	3	/#######################################################################################/
4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5	// + Copyright (c) 2008 Ingo Busker, Holger Buss	5	// + Copyright (c) 2008 Ingo Busker, Holger Buss
6	// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY	6	// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
7	// + FOR NON COMMERCIAL USE ONLY	7	// + FOR NON COMMERCIAL USE ONLY
8	// + www.MikroKopter.com	8	// + www.MikroKopter.com
9	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	9	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
10	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),	10	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
11	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.	11	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
12	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt	12	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
13	// + bzgl. der Nutzungsbedingungen aufzunehmen.	13	// + bzgl. der Nutzungsbedingungen aufzunehmen.
14	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,	14	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
15	// + Verkauf von Luftbildaufnahmen, usw.	15	// + Verkauf von Luftbildaufnahmen, usw.
16	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	16	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,	17	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
18	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen	18	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts	20	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
21	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"	21	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
22	// + eindeutig als Ursprung verlinkt werden	22	// + eindeutig als Ursprung verlinkt werden
23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion	24	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
25	// + Benutzung auf eigene Gefahr	25	// + Benutzung auf eigene Gefahr
26	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden	26	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
27	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	27	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28	// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur	28	// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
29	// + mit unserer Zustimmung zulässig	29	// + mit unserer Zustimmung zulässig
30	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	30	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31	// + Teile, die nicht eindeutig mit unserer Lizenzvereinbarung gekennzeichnet sind, unterliegen u.U. eigenen Vereinbarungen:	31	// + Teile, die nicht eindeutig mit unserer Lizenzvereinbarung gekennzeichnet sind, unterliegen u.U. eigenen Vereinbarungen:
32	// + z.B. Die Funktion printf_P() unterliegt ihrer eigenen Lizenz	32	// + z.B. Die Funktion printf_P() unterliegt ihrer eigenen Lizenz
33	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	33	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
34	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,	34	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
35	// + this list of conditions and the following disclaimer.	35	// + this list of conditions and the following disclaimer.
36	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived	36	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
37	// + from this software without specific prior written permission.	37	// + from this software without specific prior written permission.
38	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted	38	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
39	// + for non-commercial use (directly or indirectly)	39	// + for non-commercial use (directly or indirectly)
40	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted	40	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
41	// + with our written permission	41	// + with our written permission
42	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be	42	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
43	// + clearly linked as origin	43	// + clearly linked as origin
44	// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed	44	// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
45	//	45	//
46	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"	46	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
47	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE	47	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
48	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE	48	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
49	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE	49	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
50	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR	50	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
51	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF	51	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
52	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS	52	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
53	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN	53	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
54	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)	54	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
55	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE	55	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
56	// + POSSIBILITY OF SUCH DAMAGE.	56	// + POSSIBILITY OF SUCH DAMAGE.
57	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	57	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
58	//#define MCLK96MHZ	58	//#define MCLK96MHZ
59	const unsigned long _Main_Crystal = 25000;	59	const unsigned long _Main_Crystal = 25000;
60	#include <stdio.h>	60	#include <stdio.h>
61	#include "91x_lib.h"	61	#include "91x_lib.h"
62	#include "led.h"	62	#include "led.h"
63	#include "uart0.h"	63	#include "uart0.h"
64	#include "uart1.h"	64	#include "uart1.h"
65	#include "uart2.h"	65	#include "uart2.h"
66	#include "gps.h"	66	#include "gps.h"
67	#include "i2c.h"	67	#include "i2c.h"
68	#include "compass.h"	68	#include "compass.h"
69	#include "timer1.h"	69	#include "timer1.h"
70	#include "timer2.h"	70	#include "timer2.h"
71	#include "analog.h"	71	#include "analog.h"
72	#include "spi_slave.h"	72	#include "spi_slave.h"
73	#include "fat16.h"	73	#include "fat16.h"
74	#include "usb.h"	74	#include "usb.h"
75	#include "sdc.h"	75	#include "sdc.h"
76	#include "logging.h"	76	#include "logging.h"
77	#include "params.h"	77	#include "params.h"
78	#include "settings.h"	78	#include "settings.h"
79	#include "config.h"	79	#include "config.h"
80	#include "main.h"	80	#include "main.h"
81	#include "debug.h"	81	#include "debug.h"
82	#include "eeprom.h"	82	#include "eeprom.h"
83		83
84	#ifdef FOLLOW_ME	84	#ifdef FOLLOW_ME
85	u8 TransmitAlsoToFC = 0;	85	u8 TransmitAlsoToFC = 0;
86	#endif	86	#endif
87	u32 TimerCheckError;	87	u32 TimerCheckError;
88	u8 ErrorCode = 0;	88	u8 ErrorCode = 0;
89	u16 BeepTime;	89	u16 BeepTime;
90	u8 NCFlags = 0;	90	u8 NCFlags = 0;
91	s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg	91	s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg
92		92
93	u8 ClearFCStatusFlags = 0;	93	u8 ClearFCStatusFlags = 0;
94	u8 StopNavigation = 0;	94	u8 StopNavigation = 0;
95	Param_t Parameter;	95	Param_t Parameter;
96	volatile FC_t FC;	96	volatile FC_t FC;
97		97
98	s8 ErrorMSG[25];	98	s8 ErrorMSG[25];
99		99
100	//----------------------------------------------------------------------------------------------------	100	//----------------------------------------------------------------------------------------------------
101	void SCU_Config(void)	101	void SCU_Config(void)
102	{	102	{
103	/* configure PLL and set it as master clock source */	103	/* configure PLL and set it as master clock source */
104	SCU_MCLKSourceConfig(SCU_MCLK_OSC); // set master clock source to external oscillator clock (25MHz) before diabling the PLL	104	SCU_MCLKSourceConfig(SCU_MCLK_OSC); // set master clock source to external oscillator clock (25MHz) before diabling the PLL
105	SCU_PLLCmd(DISABLE); // now disable the PLL	105	SCU_PLLCmd(DISABLE); // now disable the PLL
106	#ifdef MCLK96MHZ	106	#ifdef MCLK96MHZ
107	SCU_BRCLKDivisorConfig(SCU_BRCLK_Div2); // set BRCLK to MCLK/2 = 48MHz	107	SCU_BRCLKDivisorConfig(SCU_BRCLK_Div2); // set BRCLK to MCLK/2 = 48MHz
108	SCU_PCLKDivisorConfig(SCU_PCLK_Div4); // set PCLK (APB bus clock) divisor to 4 (half Reference Clock)	108	SCU_PCLKDivisorConfig(SCU_PCLK_Div4); // set PCLK (APB bus clock) divisor to 4 (half Reference Clock)
109	SCU_RCLKDivisorConfig(SCU_RCLK_Div2); // set RCLK (Reference Clock) divisor to 1 (full PPL clock)	109	SCU_RCLKDivisorConfig(SCU_RCLK_Div2); // set RCLK (Reference Clock) divisor to 1 (full PPL clock)
110	SCU_HCLKDivisorConfig(SCU_HCLK_Div2); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock)	110	SCU_HCLKDivisorConfig(SCU_HCLK_Div2); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock)
111	SCU_PLLFactorsConfig(192,25,2); // PLL = 96 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=2	111	SCU_PLLFactorsConfig(192,25,2); // PLL = 96 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=2
112	#else	112	#else
113	SCU_BRCLKDivisorConfig(SCU_BRCLK_Div1); // set BRCLK to MCLK = 48MHz	113	SCU_BRCLKDivisorConfig(SCU_BRCLK_Div1); // set BRCLK to MCLK = 48MHz
114	SCU_PCLKDivisorConfig(SCU_PCLK_Div2); // set PCLK (APB bus clock) divisor to 2 (half Reference Clock)	114	SCU_PCLKDivisorConfig(SCU_PCLK_Div2); // set PCLK (APB bus clock) divisor to 2 (half Reference Clock)
115	SCU_RCLKDivisorConfig(SCU_RCLK_Div1); // set RCLK (Reference Clock) divisor to 1 (full PPL clock)	115	SCU_RCLKDivisorConfig(SCU_RCLK_Div1); // set RCLK (Reference Clock) divisor to 1 (full PPL clock)
116	SCU_HCLKDivisorConfig(SCU_HCLK_Div1); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock)	116	SCU_HCLKDivisorConfig(SCU_HCLK_Div1); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock)
117	SCU_PLLFactorsConfig(192,25,3); // PLL = 48 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=3	117	SCU_PLLFactorsConfig(192,25,3); // PLL = 48 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=3
118	#endif	118	#endif
119	SCU_PLLCmd(ENABLE); // Enable PLL (is disabled by SCU_PLLFactorsConfig)	119	SCU_PLLCmd(ENABLE); // Enable PLL (is disabled by SCU_PLLFactorsConfig)
120	SCU_MCLKSourceConfig(SCU_MCLK_PLL); // set master clock source to PLL	120	SCU_MCLKSourceConfig(SCU_MCLK_PLL); // set master clock source to PLL
121	}	121	}
122		122
123	//----------------------------------------------------------------------------------------------------	123	//----------------------------------------------------------------------------------------------------
124	void GetNaviCtrlVersion(void)	124	void GetNaviCtrlVersion(void)
125	{	125	{
126	u8 msg[25];	126	u8 msg[25];
127		127
128	sprintf(msg,"\n\r NaviCtrl HW: V%d.%d SW: V%d.%d%c", Version_HW/10, Version_HW%10, VERSION_MAJOR, VERSION_MINOR, 'a'+ VERSION_PATCH);	128	sprintf(msg,"\n\r NaviCtrl HW: V%d.%d SW: V%d.%d%c", Version_HW/10, Version_HW%10, VERSION_MAJOR, VERSION_MINOR, 'a'+ VERSION_PATCH);
129	UART1_PutString(msg);	129	UART1_PutString(msg);
130	}	130	}
131		131
132	//----------------------------------------------------------------------------------------------------	132	//----------------------------------------------------------------------------------------------------
133		133
134	void CheckErrors(void)	134	void CheckErrors(void)
135	{	135	{
136	UART_VersionInfo.HardwareError[0] = 0;	136	UART_VersionInfo.HardwareError[0] = 0;
137		137
138	if(CheckDelay(I2C1_Timeout) \|\| (Compass_Heading < 0)) DebugOut.Status[1] \|= 0x08;	138	if(CheckDelay(I2C1_Timeout) \|\| (Compass_Heading < 0)) DebugOut.Status[1] \|= 0x08;
139	else DebugOut.Status[1] &= ~0x08; // MK3Mag green status	139	else DebugOut.Status[1] &= ~0x08; // MK3Mag green status
140		140
141	if((FC.Error[1] & FC_ERROR1_I2C) \|\| (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.Status[1] \|= 0x02;	141	if((FC.Error[1] & FC_ERROR1_I2C) \|\| (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.Status[1] \|= 0x02;
142	else DebugOut.Status[1] &= ~0x02; // BL-Ctrl green status	142	else DebugOut.Status[1] &= ~0x02; // BL-Ctrl green status
143		143
144	if(CheckDelay(SPI0_Timeout))	144	if(CheckDelay(SPI0_Timeout))
145	{	145	{
146	LED_RED_ON;	146	LED_RED_ON;
147	sprintf(ErrorMSG,"no FC communication ");	147	sprintf(ErrorMSG,"no FC communication ");
148	ErrorCode = 3;	148	ErrorCode = 3;
149	StopNavigation = 1;	149	StopNavigation = 1;
150	DebugOut.Status[0] &= ~0x01; // status of FC Present	150	DebugOut.Status[0] &= ~0x01; // status of FC Present
151	DebugOut.Status[0] &= ~0x02; // status of BL Present	151	DebugOut.Status[0] &= ~0x02; // status of BL Present
152	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_SPI_RX;	152	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_SPI_RX;
153	}	153	}
154	else if(CheckDelay(I2C1_Timeout))	154	else if(CheckDelay(I2C1_Timeout))
155	{	155	{
156	LED_RED_ON;	156	LED_RED_ON;
157	sprintf(ErrorMSG,"no compass communication ");	157	sprintf(ErrorMSG,"no compass communication ");
158	//Reset I2CBus	158	//Reset I2CBus
159	I2C1_Deinit();	159	I2C1_Deinit();
160	I2C1_Init();	160	I2C1_Init();
161	ErrorCode = 4;	161	ErrorCode = 4;
162	StopNavigation = 1;	162	StopNavigation = 1;
163	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_RX;	163	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_RX;
164	DebugOut.Status[1] \|= 0x08;	164	DebugOut.Status[1] \|= 0x08;
165	}	165	}
166	else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)	166	else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
167	{	167	{
168	LED_RED_ON;	168	LED_RED_ON;
169	#ifndef FOLLOW_ME	169	#ifndef FOLLOW_ME
170	sprintf(ErrorMSG,"FC not compatible ");	170	sprintf(ErrorMSG,"FC not compatible ");
171	#else	171	#else
172	sprintf(ErrorMSG,"! FollowMe only ! ");	172	sprintf(ErrorMSG,"! FollowMe only ! ");
173	#endif	173	#endif
174	ErrorCode = 1;	174	ErrorCode = 1;
175	StopNavigation = 1;	175	StopNavigation = 1;
176	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_FC_INCOMPATIBLE;	176	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_FC_INCOMPATIBLE;
177	}	177	}
178		178
179	else if(FC.Error[0] & FC_ERROR0_GYRO_NICK)	179	else if(FC.Error[0] & FC_ERROR0_GYRO_NICK)
180	{	180	{
181	LED_RED_ON;	181	LED_RED_ON;
182	sprintf(ErrorMSG,"ERR: FC Nick Gyro");	182	sprintf(ErrorMSG,"ERR: FC Nick Gyro");
183	ErrorCode = 10;	183	ErrorCode = 10;
184	}	184	}
185	else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL)	185	else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL)
186	{	186	{
187	LED_RED_ON;	187	LED_RED_ON;
188	sprintf(ErrorMSG,"ERR: FC Roll Gyro");	188	sprintf(ErrorMSG,"ERR: FC Roll Gyro");
189	ErrorCode = 11;	189	ErrorCode = 11;
190	}	190	}
191	else if(FC.Error[0] & FC_ERROR0_GYRO_YAW)	191	else if(FC.Error[0] & FC_ERROR0_GYRO_YAW)
192	{	192	{
193	LED_RED_ON;	193	LED_RED_ON;
194	sprintf(ErrorMSG,"ERR: FC Yaw Gyro");	194	sprintf(ErrorMSG,"ERR: FC Yaw Gyro");
195	ErrorCode = 12;	195	ErrorCode = 12;
196	}	196	}
197	else if(FC.Error[0] & FC_ERROR0_ACC_NICK)	197	else if(FC.Error[0] & FC_ERROR0_ACC_NICK)
198	{	198	{
199	LED_RED_ON;	199	LED_RED_ON;
200	sprintf(ErrorMSG,"ERR: FC Nick ACC");	200	sprintf(ErrorMSG,"ERR: FC Nick ACC");
201	ErrorCode = 13;	201	ErrorCode = 13;
202	}	202	}
203	else if(FC.Error[0] & FC_ERROR0_ACC_ROLL)	203	else if(FC.Error[0] & FC_ERROR0_ACC_ROLL)
204	{	204	{
205	LED_RED_ON;	205	LED_RED_ON;
206	sprintf(ErrorMSG,"ERR: FC Roll ACC");	206	sprintf(ErrorMSG,"ERR: FC Roll ACC");
207	ErrorCode = 14;	207	ErrorCode = 14;
208	}	208	}
209	else if(FC.Error[0] & FC_ERROR0_ACC_TOP)	209	else if(FC.Error[0] & FC_ERROR0_ACC_TOP)
210	{	210	{
211	LED_RED_ON;	211	LED_RED_ON;
212	sprintf(ErrorMSG,"ERR: FC Z-ACC");	212	sprintf(ErrorMSG,"ERR: FC Z-ACC");
213	ErrorCode = 15;	213	ErrorCode = 15;
214	}	214	}
215	else if(FC.Error[0] & FC_ERROR0_PRESSURE)	215	else if(FC.Error[0] & FC_ERROR0_PRESSURE)
216	{	216	{
217	LED_RED_ON;	217	LED_RED_ON;
218	sprintf(ErrorMSG,"ERR: Pressure sensor");	218	sprintf(ErrorMSG,"ERR: Pressure sensor");
219	ErrorCode = 16;	219	ErrorCode = 16;
220	}	220	}
221	else if(FC.Error[1] & FC_ERROR1_I2C)	221	else if(FC.Error[1] & FC_ERROR1_I2C)
222	{	222	{
223	LED_RED_ON;	223	LED_RED_ON;
224	sprintf(ErrorMSG,"ERR: FC I2C");	224	sprintf(ErrorMSG,"ERR: FC I2C");
225	ErrorCode = 17;	225	ErrorCode = 17;
226	}	226	}
227	else if(FC.Error[1] & FC_ERROR1_BL_MISSING)	227	else if(FC.Error[1] & FC_ERROR1_BL_MISSING)
228	{	228	{
229	LED_RED_ON;	229	LED_RED_ON;
230	sprintf(ErrorMSG,"ERR: Bl Missing");	230	sprintf(ErrorMSG,"ERR: Bl Missing");
231	ErrorCode = 18;	231	ErrorCode = 18;
232	}	232	}
233	else if(FC.Error[1] & FC_ERROR1_MIXER)	233	else if(FC.Error[1] & FC_ERROR1_MIXER)
234	{	234	{
235	LED_RED_ON;	235	LED_RED_ON;
236	sprintf(ErrorMSG,"Mixer Error");	236	sprintf(ErrorMSG,"Mixer Error");
237	ErrorCode = 19;	237	ErrorCode = 19;
238	}	238	}
239	else if(CheckDelay(UBX_Timeout))	239	else if(CheckDelay(UBX_Timeout))
240	{	240	{
241	LED_RED_ON;	241	LED_RED_ON;
242	if(!(Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) sprintf(ErrorMSG,"GPS disconnected ");	242	if(!(Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) sprintf(ErrorMSG,"GPS disconnected ");
243	else	243	else
244	{	244	{
245	sprintf(ErrorMSG,"no GPS communication ");	245	sprintf(ErrorMSG,"no GPS communication ");
246	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_GPS_RX;	246	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_GPS_RX;
247	ErrorCode = 5;	247	ErrorCode = 5;
248	}	248	}
249	StopNavigation = 1;	249	StopNavigation = 1;
250	// UBX_Timeout = SetDelay(500);	250	// UBX_Timeout = SetDelay(500);
251	}	251	}
252	else if(Compass_Heading < 0)	252	else if(Compass_Heading < 0)
253	{	253	{
254	LED_RED_ON;	254	LED_RED_ON;
255	sprintf(ErrorMSG,"bad compass value ");	255	sprintf(ErrorMSG,"bad compass value ");
256	ErrorCode = 6;	256	ErrorCode = 6;
257	StopNavigation = 1;	257	StopNavigation = 1;
258	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_VALUE;	258	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_VALUE;
259	}	259	}
260	else if((FC.Error[1] & FC_ERROR1_SPI_RX))	260	else if((FC.Error[1] & FC_ERROR1_SPI_RX))
261	{	261	{
262	LED_RED_ON;	262	LED_RED_ON;
263	sprintf(ErrorMSG,"FC spi rx error ");	263	sprintf(ErrorMSG,"FC spi rx error ");
264	ErrorCode = 8;	264	ErrorCode = 8;
265	StopNavigation = 1;	265	StopNavigation = 1;
266	}	266	}
267	else if(FC.Error[0] & FC_ERROR0_CAREFREE)	267	else if(FC.Error[0] & FC_ERROR0_CAREFREE)
268	{	268	{
269	LED_RED_ON;	269	LED_RED_ON;
270	sprintf(ErrorMSG,"FC: Carefree Error");	270	sprintf(ErrorMSG,"FC: Carefree Error");
271	ErrorCode = 20;	271	ErrorCode = 20;
272	}	272	}
273	else if(FC.Error[1] & FC_ERROR1_PPM)	273	else if(FC.Error[1] & FC_ERROR1_PPM)
274	{	274	{
275	LED_RED_ON;	275	LED_RED_ON;
276	sprintf(ErrorMSG,"RC Signal lost ");	276	sprintf(ErrorMSG,"RC Signal lost ");
277	ErrorCode = 7;	277	ErrorCode = 7;
278	}	278	}
279	else // no error occured	279	else // no error occured
280	{	280	{
281	sprintf(ErrorMSG,"No Error ");	281	sprintf(ErrorMSG,"No Error ");
282	ErrorCode = 0;	282	ErrorCode = 0;
283	StopNavigation = 0;	283	StopNavigation = 0;
284	LED_RED_OFF;	284	LED_RED_OFF;
285	}	285	}
286	if(UART_VersionInfo.HardwareError[0] \|\| UART_VersionInfo.HardwareError[1]) DebugOut.Status[1] \|= 0x04;	286	if(UART_VersionInfo.HardwareError[0] \|\| UART_VersionInfo.HardwareError[1]) DebugOut.Status[1] \|= 0x04;
287	else DebugOut.Status[1] &= ~0x04;	287	else DebugOut.Status[1] &= ~0x04;
288	}	288	}
289		289
290	// the handler will be cyclic called by the timer 1 ISR	290	// the handler will be cyclic called by the timer 1 ISR
291	// used is for critical timing parts that normaly would handled	291	// used is for critical timing parts that normaly would handled
292	// within the main loop that could block longer at logging activities	292	// within the main loop that could block longer at logging activities
293	void EXTIT3_IRQHandler(void)	293	void EXTIT3_IRQHandler(void)
294	{	294	{
295	IENABLE;	295	IENABLE;
296		296
297	VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq	297	VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq
298	VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit	298	VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit
299	Compass_Update(); // update compass communication	299	Compass_Update(); // update compass communication
300	Analog_Update(); // get new ADC values	300	Analog_Update(); // get new ADC values
301	VIC_ITCmd(EXTIT3_ITLine, ENABLE); // enable irq	301	VIC_ITCmd(EXTIT3_ITLine, ENABLE); // enable irq
302		302
303	IDISABLE;	303	IDISABLE;
304	}	304	}
305		305
306	//----------------------------------------------------------------------------------------------------	306	//----------------------------------------------------------------------------------------------------
307	int main(void)	307	int main(void)
308	{	308	{
-		309	/*
-		310	static u32 ftimer =0;
-		311	static u8 fstate = 0;
-		312	static File_t* f = NULL;
-		313	*/
-		314
309	/* Configure the system clocks */	315	/* Configure the system clocks */
310	SCU_Config();	316	SCU_Config();
311	/* init VIC (Vectored Interrupt Controller) */	317	/* init VIC (Vectored Interrupt Controller) */
312	SCU_AHBPeriphClockConfig(__VIC,ENABLE); // enable AHB bus clock for VIC	318	SCU_AHBPeriphClockConfig(__VIC,ENABLE); // enable AHB bus clock for VIC
313	SCU_AHBPeriphReset(__VIC, DISABLE); // disable reset state for VIC	319	SCU_AHBPeriphReset(__VIC, DISABLE); // disable reset state for VIC
314	VIC_DeInit(); // deinitializes the VIC module registers to their default reset values.	320	VIC_DeInit(); // deinitializes the VIC module registers to their default reset values.
315	VIC_InitDefaultVectors();	321	VIC_InitDefaultVectors();
316		322
317	// initialize timer 1 for System Clock and delay rountines	323	// initialize timer 1 for System Clock and delay rountines
318	TIMER1_Init();	324	TIMER1_Init();
319	// initialize the LEDs (needs Timer 1)	325	// initialize the LEDs (needs Timer 1)
320	Led_Init();	326	Led_Init();
321	// initialize the debug UART1	327	// initialize the debug UART1
322	UART1_Init();	328	UART1_Init();
323	UART1_PutString("\r\n---------------------------------------------");	329	UART1_PutString("\r\n---------------------------------------------");
324	// initialize usb	330	// initialize usb
325	USB_ConfigInit();	331	USB_ConfigInit();
326	// initialize timer 2 for servo outputs	332	// initialize timer 2 for servo outputs
327	//TIMER2_Init();	333	//TIMER2_Init();
328	// initialize UART2 to FLIGHTCTRL	334	// initialize UART2 to FLIGHTCTRL
329	UART2_Init();	335	UART2_Init();
330	// initialize UART0 (to MKGPS or MK3MAG)	336	// initialize UART0 (to MKGPS or MK3MAG)
331	UART0_Init();	337	UART0_Init();
332	// initialize adc	338	// initialize adc
333	Analog_Init();	339	Analog_Init();
334	// initialize SPI0 to FC	340	// initialize SPI0 to FC
335	SPI0_Init();	341	SPI0_Init();
336	// initialize i2c bus (needs Timer 1)	342	// initialize i2c bus (needs Timer 1)
337	I2C1_Init();	343	I2C1_Init();
338	// initialize fat16 partition on sd card (needs Timer 1)	344	// initialize fat16 partition on sd card (needs Timer 1)
339	Fat16_Init();	345	Fat16_Init();
340	// initialize NC params	346	// initialize NC params
341	NCParams_Init();	347	NCParams_Init();
342	// initialize the settings	348	// initialize the settings
343	Settings_Init();	349	Settings_Init();
344	// initialize logging (needs settings)	350	// initialize logging (needs settings)
345	Logging_Init();	351	Logging_Init();
346		352
347	TimerCheckError = SetDelay(3000);	353	TimerCheckError = SetDelay(3000);
348	UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");	354	UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
349	UART1_PutString("\n\r Version information:");	355	UART1_PutString("\n\r Version information:");
350		356
351	GetNaviCtrlVersion();	357	GetNaviCtrlVersion();
352	DebugOut.Status[0] = 0x04 \| 0x08; // NC and MK3Mag	358	DebugOut.Status[0] = 0x04 \| 0x08; // NC and MK3Mag
353	DebugOut.Status[1] = 0x00;	359	DebugOut.Status[1] = 0x00;
354		360
355	Compass_Init();	361	Compass_Init();
356		362
357	#ifdef FOLLOW_ME	363	#ifdef FOLLOW_ME
358	TransmitAlsoToFC = 1;	364	TransmitAlsoToFC = 1;
359	UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");	365	UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
360	UART1_PutString("\n\r FOLLOW-ME Transmitter only!");	366	UART1_PutString("\n\r FOLLOW-ME Transmitter only!");
361	UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++\r\n\r\n");	367	UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++\r\n\r\n");
362	TransmitAlsoToFC = 0;	368	TransmitAlsoToFC = 0;
363	#else	369	#else
364	SPI0_GetFlightCtrlVersion();	370	SPI0_GetFlightCtrlVersion();
365	if(FC_Version.Compatible != FC_SPI_COMPATIBLE)	371	if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
366	{	372	{
367	UART1_PutString("\n\r Flight-Ctrl not compatible");	373	UART1_PutString("\n\r Flight-Ctrl not compatible");
368	LED_RED_ON;	374	LED_RED_ON;
369	}	375	}
370	#endif	376	#endif
371		377
372	GPS_Init();	378	GPS_Init();
373		379
374	// ---------- Prepare the isr driven	380	// ---------- Prepare the isr driven
375	// set to absolute lowest priority	381	// set to absolute lowest priority
376	VIC_Config(EXTIT3_ITLine, VIC_IRQ, PRIORITY_SW);	382	VIC_Config(EXTIT3_ITLine, VIC_IRQ, PRIORITY_SW);
377	// enable interrupts	383	// enable interrupts
378	VIC_ITCmd(EXTIT3_ITLine, ENABLE);	384	VIC_ITCmd(EXTIT3_ITLine, ENABLE);
379		385
380	Debug_OK("START");	386	Debug_OK("START");
381		387
382	for (;;) // the endless main loop	388	for (;;) // the endless main loop
383	{	389	{
384	UART0_ProcessRxData(); // process request	390	UART0_ProcessRxData(); // process request
385	UART1_ProcessRxData(); // process request	391	UART1_ProcessRxData(); // process request
386	USB_ProcessRxData(); // process request	392	USB_ProcessRxData(); // process request
387	UART0_TransmitTxData(); // send answer	393	UART0_TransmitTxData(); // send answer
388	UART1_TransmitTxData(); // send answer	394	UART1_TransmitTxData(); // send answer
389	UART2_TransmitTxData(); // send answer	395	UART2_TransmitTxData(); // send answer
390	USB_TransmitTxData(); // send answer	396	USB_TransmitTxData(); // send answer
391	SPI0_UpdateBuffer(); // handle new SPI Data	397	SPI0_UpdateBuffer(); // handle new SPI Data
392	// ---------------- Error Check Timing ----------------------------	398	// ---------------- Error Check Timing ----------------------------
393	if(CheckDelay(TimerCheckError))	399	if(CheckDelay(TimerCheckError))
394	{	400	{
395	TimerCheckError = SetDelay(1000);	401	TimerCheckError = SetDelay(1000);
396	if(CheckDelay(SPI0_Timeout)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected	402	if(CheckDelay(SPI0_Timeout)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected
397	CheckErrors();	403	CheckErrors();
398	if(FC.StatusFlags & FC_STATUS_FLY) NaviData.FlyingTime++; // we want to count the battery-time	404	if(FC.StatusFlags & FC_STATUS_FLY) NaviData.FlyingTime++; // we want to count the battery-time
399	// else NaviData.FlyingTime = 0; // not the time per flight	405	// else NaviData.FlyingTime = 0; // not the time per flight
400	if(SerialLinkOkay) SerialLinkOkay--;	406	if(SerialLinkOkay) SerialLinkOkay--;
401	if(SerialLinkOkay < 250 - 5) NCFlags \|= NC_FLAG_NOSERIALLINK; // 5 seconds timeout for serial communication	407	if(SerialLinkOkay < 250 - 5) NCFlags \|= NC_FLAG_NOSERIALLINK; // 5 seconds timeout for serial communication
402	else NCFlags &= ~NC_FLAG_NOSERIALLINK;	408	else NCFlags &= ~NC_FLAG_NOSERIALLINK;
403	if(StopNavigation && (Parameter.NaviGpsModeControl >= 50) && (Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) BeepTime = 1000;	409	if(StopNavigation && (Parameter.NaviGpsModeControl >= 50) && (Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) BeepTime = 1000;
404	}	410	}
405	// ---------------- Logging ---------------------------------------	411	// ---------------- Logging ---------------------------------------
406	Logging_Update(); // could be block some time for at max. 2 seconds, therefore move time critical part of the mainloop into the ISR of timer 1	412	Logging_Update(); // could be block some time for at max. 2 seconds, therefore move time critical part of the mainloop into the ISR of timer 1
-		413	/*
-		414	// test
-		415	if(CheckDelay(ftimer))
-		416	{
-		417
-		418	s8* filename = "test.txt";
-		419
-		420	ftimer = SetDelay(100);
-		421	if(FC.Poti[3]>100 && fstate == 0)
-		422	{
-		423	fstate = 1;
-		424	}
-		425	else if(FC.Poti[3]<100 && fstate == 2)
-		426	{
-		427	fstate = 3;
-		428	}
-		429
-		430	switch(fstate)
-		431	{
-		432	case 1:
-		433	sprintf(text,"\r\nStart writing file: %s", filename);
-		434	UART1_PutString(text);
-		435	f = fopen_(filename, 'a');
-		436	if(f== NULL) Fat16_Init();
-		437	fstate = 2;
-		438	break;
-		439
-		440	case 2:
-		441	fputs_("\r\ntest edins sdv dsivbds iv dsivb disbv idsv bisd bv d suiv dsibsivbdis fbvisdöb visdbvisdb vidbfibds ibv", f);
-		442	break;
-		443
-		444	case 3:
-		445	sprintf(text,"\r\nClosing file: %s", filename);
-		446	UART1_PutString(text);
-		447	fclose_(f);
-		448	fstate = 0;
-		449	break;
-		450
-		451	default:
-		452	break;
-		453	}
-		454	}
-		455	*/
407	}	456	}
408	}	457	}
409		458
410		459
411		460

Subversion Repositories NaviCtrl

(root)/tags/V2.07b_Beta/main.c @ 684 - Rev 320 → 324