WebSVN - NaviCtrl - Diff - Rev 461 and 465


/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Software Nutzungsbedingungen (english version: see below)
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool 
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand 
// + des Mitverschuldens offen.
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
// +  Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Software LICENSING TERMS
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware 
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
// + The Software may only be used with the Licensor's products.
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
// + agreement shall be the property of the Licensor.
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
// + features that can be used to identify the program may not be altered or defaced by the customer.
// + The customer shall be responsible for taking reasonable precautions
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
// + #### END OF LICENSING TERMS ####
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//#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 "ncmag.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"
#include "ssc.h"
#include "sdc.h"
#include "uart1.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 ErrorGpsFixLost = 0;
 
u8 ClearFCStatusFlags = 0;
u8 StopNavigation = 0;
volatile u32 PollingTimeout = 10000;
Param_t Parameter;
volatile FC_t FC;
volatile u32 SPIWatchDog = 15000; // stop Navigation if this goes to zero
volatile u32 SD_WatchDog = 15000; // stop Logging if this goes to zero
u32 CountGpsProcessedIn5Sec = 0,CountNewGpsDataIn5Sec = 0, FreqGpsProcessedIn5Sec = 0, FreqNewGpsDataIn5Sec = 0;
 
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)
{
    static s32 no_error_delay = 0;
        s32 newErrorCode = 0;
        UART_VersionInfo.HardwareError[0] = 0;
 
        if(CheckDelay(I2C1_Timeout) || (Compass_Heading < 0)) DebugOut.StatusRed |= AMPEL_COMPASS;
        else DebugOut.StatusRed &= ~AMPEL_COMPASS; // MK3Mag green status
 
        if((FC.Error[1] & FC_ERROR1_I2C) || (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.StatusRed |= AMPEL_BL;
        else DebugOut.StatusRed &= ~AMPEL_BL; // BL-Ctrl green status
 
        if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.StatusRed |= AMPEL_NC;
        else DebugOut.StatusRed &= ~AMPEL_NC;
 
    if((FCCalibActive || CompassCalState) && FC_Version.Hardware)
     {
                sprintf(ErrorMSG,"Calibrate... ");
                newErrorCode = 0;
                ErrorCode = 0;
                no_error_delay = 1;
         }
        else
        if(CheckDelay(SPI0_Timeout))
        {
                LED_RED_ON;              
                sprintf(ErrorMSG,"no FC communication ");
                newErrorCode = 3;
                StopNavigation = 1;
                DebugOut.StatusGreen &= ~AMPEL_FC; // status of FC Present
                DebugOut.StatusGreen &= ~AMPEL_BL; // status of BL Present
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX;
        }
        else if(CheckDelay(I2C1_Timeout))
        {
                LED_RED_ON;              
                sprintf(ErrorMSG,"no compass communica");
                //Reset I2CBus
                I2C1_Deinit();
                I2C1_Init();
                newErrorCode = 4;
                StopNavigation = 1;
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX;
                DebugOut.StatusRed |= AMPEL_COMPASS;
        }
        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
                newErrorCode = 1;
                StopNavigation = 1;
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_FC_INCOMPATIBLE;
                DebugOut.StatusRed |= AMPEL_NC;
        }
 
        else if(FC.Error[0] & FC_ERROR0_GYRO_NICK)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Nick Gyro");
                newErrorCode = 10;
        }
        else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Roll Gyro");
                newErrorCode = 11;
        }
        else if(FC.Error[0] & FC_ERROR0_GYRO_YAW)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Yaw Gyro");
                newErrorCode = 12;
        }
        else if(FC.Error[0] & FC_ERROR0_ACC_NICK)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Nick ACC");
                newErrorCode = 13;
        }
        else if(FC.Error[0] & FC_ERROR0_ACC_ROLL)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Roll ACC");
                newErrorCode = 14;
        }
        else if(FC.Error[0] & FC_ERROR0_ACC_TOP)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR:FC Z-ACC");
                newErrorCode = 15;
        }
        else if(NC_To_FC_Flags & NC_TO_FC_FLYING_RANGE)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR:Flying range!");
                newErrorCode = 28;
        }
        else if(FC.Error[0] & FC_ERROR0_PRESSURE)
        {
                LED_RED_ON;              
                sprintf(ErrorMSG,"ERR:Pressure sensor");
                newErrorCode = 16;
        }
        else if(FC.Error[1] &  FC_ERROR1_I2C)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR:I2C FC to BL");
                newErrorCode = 17;
        }
        else if(FC.Error[1] &  FC_ERROR1_BL_MISSING)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: Bl Missing");
                newErrorCode = 18;
        }
        else if(FC.Error[1] &  FC_ERROR1_MIXER)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"Mixer Error");
                newErrorCode = 19;
        }
        else if(CheckDelay(UBX_Timeout) && Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)
        {
                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;
                  UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_GPS_PRESENT;
              newErrorCode = 5;
                 }
                StopNavigation = 1;
//              UBX_Timeout = SetDelay(500);
        }
        else if(Compass_Heading < 0 && NCMAG_Present && !NCMAG_IsCalibrated)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"compass not calibr.");
                newErrorCode = 31;
                StopNavigation = 1;
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
        }
        else if(Compass_Heading < 0)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"bad compass value ");
                newErrorCode = 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 ");
                newErrorCode = 8;
                StopNavigation = 1;
        }
        else if(FC.Error[0] &  FC_ERROR0_CAREFREE)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"FC: Carefree Error");
                newErrorCode = 20;
        }
        else if(FC.Error[1] &  FC_ERROR1_PPM)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"RC Signal lost ");
                newErrorCode = 7;
        }
        else if(ErrorGpsFixLost)
        {
         LED_RED_ON;
         sprintf(ErrorMSG,"GPS Fix lost    ");
         newErrorCode = 21;
        }
        else if(ErrorDisturbedEarthMagnetField)
        {
         LED_RED_ON;
         sprintf(ErrorMSG,"Magnet error    ");
         newErrorCode = 22;
         DebugOut.StatusRed |= AMPEL_COMPASS | AMPEL_NC;
         UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
        }
        else if(BL_MinOfMaxPWM == 40 && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)
        {
         LED_RED_ON;
         sprintf(ErrorMSG,"ERR:Motor restart  ");
         newErrorCode = 23;
         DebugOut.StatusRed |= AMPEL_BL;
        }
        else if(BL_MinOfMaxPWM != 255 && (BL_MinOfMaxPWM != 250) && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)
        {
         LED_RED_ON;
         sprintf(ErrorMSG,"ERR:BL Limitation   ");
         newErrorCode = 24;
         DebugOut.StatusRed |= AMPEL_BL;
        }
        else if(NCFlags & NC_FLAG_RANGE_LIMIT && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)
        {
         LED_RED_ON;
         sprintf(ErrorMSG,"ERR:GPS range  ");
         newErrorCode = 25;
         DebugOut.StatusRed |= AMPEL_NC;
        }
        else if((!SD_SWITCH || (SDCardInfo.Valid == 0)) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START && !(FC.StatusFlags & FC_STATUS_FLY))
        {
         LED_RED_ON;
         sprintf(ErrorMSG,"ERR:No SD-Card  ");
         newErrorCode = 26;
         DebugOut.StatusRed |= AMPEL_NC;
        }
        else if((SD_LoggingError || (SD_WatchDog < 2000 && SD_WatchDog != 0)) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START)
        {
         LED_RED_ON;       
         sprintf(ErrorMSG,"ERR:SD Logging abort");
         newErrorCode = 27;
         DebugOut.StatusRed |= AMPEL_NC;
         SD_LoggingError = 0;
        }
        else if(((AbsoluteFlyingAltitude) && (NaviData.Altimeter / 20 >= AbsoluteFlyingAltitude)) && (FC.StatusFlags & FC_STATUS_FLY))
        {
         LED_RED_ON;
         sprintf(ErrorMSG,"ERR:Max Altitude ");
         newErrorCode = 29;
         DebugOut.StatusRed |= AMPEL_NC;
        }
        else if(Parameter.GlobalConfig3 & CFG3_NO_GPSFIX_NO_START && !(NCFlags & NC_FLAG_GPS_OK) && ((FC.StatusFlags & (FC_STATUS_START | FC_STATUS_MOTOR_RUN)) || (FC.StickGas < -50 && FC.StickYaw < -50)))
        {                                                                                                                                                  
         LED_RED_ON;
         sprintf(ErrorMSG,"No GPS Fix      ");
         newErrorCode = 30;
        }
        else // no error occured
        {
                StopNavigation = 0;
                LED_RED_OFF;
                if(no_error_delay) { no_error_delay--;  }
                else
                 {                                     
                  sprintf(ErrorMSG,"No Error            ");
                  ErrorCode = 0;
                 }
        }
 
    if(newErrorCode)
         {
          if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 8; // delay the errors if the motors are running
          ErrorCode = newErrorCode;
         }
 FC.Error[0] = 0;
 FC.Error[1] = 0;
 FC.Error[2] = 0;
 FC.Error[3] = 0;
 FC.Error[4] = 0;
 ErrorGpsFixLost = 0;
}
 
 
 
void Polling(void)
{
 static u8 running = 0, oldFcFlags = 0, count5sec;
 if(running) return;
 running = 1;
                SPI0_UpdateBuffer();    // also calls the GPS-functions
                UART0_ProcessRxData();  // GPS process request
                UART0_TransmitTxData(); // GPS send answer
                UART1_ProcessRxData();  // PC process request
                UART1_TransmitTxData(); // PC send answer
                UART2_TransmitTxData(); // FC send answer
                CalcHeadFree();
                // ---------------- Error Check Timing ----------------------------
                if(CheckDelay(TimerCheckError) || (FC.StatusFlags & FC_STATUS_START && !(oldFcFlags & FC_STATUS_START))) // Timer or FY wants to start
                {
//CreateNmeaGGA();
                        if(CheckDelay(TimerCheckError))
                         {
                          TimerCheckError = SetDelay(1000);
                          if(++count5sec == 5)
                           {
                                count5sec = 0;
                                FreqGpsProcessedIn5Sec = CountGpsProcessedIn5Sec * 2;
                                FreqNewGpsDataIn5Sec = CountNewGpsDataIn5Sec * 2;
                                CountGpsProcessedIn5Sec = 0;
                                CountNewGpsDataIn5Sec = 0;  
                           }
                         }
                        oldFcFlags = FC.StatusFlags;
                        if(CheckDelay(SPI0_Timeout) && (DebugUART == UART1)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected
       
                        if(!CheckDelay(SPI0_Timeout) || (DebugUART == UART1)) 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;
                }
 running = 0;
}
 
// 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
        Compass_Update();               // update compass communication
        Analog_Update();                // get new ADC values
 
        if(!PollingTimeout)
        {
                PollingTimeout = 5;
                Polling();
        }
 
        VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit
        VIC_ITCmd(EXTIT3_ITLine, ENABLE); // enable irq
        IDISABLE;
        VIC1->VAR = 0xFF; // write any value to VIC0 Vector address register
}
 
//----------------------------------------------------------------------------------------------------
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)
        I2C0_Init();
        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();
 
        LED_GRN_ON;
        TimerCheckError = SetDelay(3000);
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
        UART1_PutString("\n\r Version information:");
 
        GetNaviCtrlVersion();
        DebugOut.StatusGreen = AMPEL_NC | AMPEL_COMPASS; // NC and MK3Mag
        DebugOut.StatusRed = 0x00;
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
 
        Compass_Init();
 
        GPS_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\n\r");
                LED_RED_ON;
        }
#endif
        // ---------- 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");
        UART1_PutString("\r\n");
        fifo_purge(&UART1_rx_fifo); // flush the whole fifo init buffer
        LED_GRN_ON;
        LED_RED_OFF;
    Settings_GetParamValue(PID_SEND_NMEA, &NMEA_Interval);
        UART1_PutString("\r\n");
        for (;;) // the endless main loop
        {
                Polling();
                PollingTimeout = 15;
                // ---------------- Logging  ---------------------------------------
                if(SD_WatchDog)
                {
                        SD_WatchDog = 30000;
                        if(SDCardInfo.Valid == 1) 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
                        else if(FC.StatusFlags & FC_STATUS_START) SD_LoggingError = 100;
                        if(!SD_WatchDog) UART1_PutString("\n\rSD-Watchdog - Logging aborted\n\r");
                }
               
/*             
                if(CheckDelay(ftimer))
                {
 
                        static s8 filename[35];
                        static u8 i = 0;
                        s8 dbgmsg[40];
                       
                       
 
                        ftimer = SetDelay(100);
                        if(FC.Poti[3]>100 && fstate == 0)
                        {
                                fstate = 1;
                                sprintf(filename, "/toast/toasta/toast%02i.txt",i++);
                        }
                        else if(FC.Poti[3]<100 && fstate == 2)
                        {
                                fstate = 3;
                        }
 
                        switch(fstate)
                        {
                                case 1:
                                        sprintf(dbgmsg,"\r\nStart writing file: %s", filename);
                                        Debug(dbgmsg);
                                        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(dbgmsg,"\r\nClosing file: %s", filename);
                                        Debug(dbgmsg);
                                        fclose_(f);
                                        fstate = 0;
                                        break;
 
                                default:
                                        break;
                        }
                }
*/             
        }
}
//DebugOut.Analog[16]
 

Rev 461	Rev 465
1	/#######################################################################################/	1	/#######################################################################################/
2	/* !!! THIS IS NOT FREE SOFTWARE !!! */	2	/* !!! THIS IS NOT FREE SOFTWARE !!! */
3	/#######################################################################################/	3	/#######################################################################################/
4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5	// + www.MikroKopter.com	5	// + www.MikroKopter.com
6	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	6	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7	// + Software Nutzungsbedingungen (english version: see below)	7	// + Software Nutzungsbedingungen (english version: see below)
8	// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -	8	// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
9	// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den	9	// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
10	// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool	10	// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
11	// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.	11	// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
12	// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.	12	// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
13	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	13	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14	// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im	14	// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
15	// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.	15	// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
16	// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie	16	// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
17	// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.	17	// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
18	// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren	18	// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
19	// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.	19	// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
20	// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren	20	// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
21	// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand	21	// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
22	// + des Mitverschuldens offen.	22	// + des Mitverschuldens offen.
23	// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.	23	// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
24	// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.	24	// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
25	// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.	25	// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
26	// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang	26	// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
27	// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.	27	// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
28	// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.	28	// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
29	// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'	29	// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
30	// + Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.	30	// + Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
31	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	31	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
32	// + Software LICENSING TERMS	32	// + Software LICENSING TERMS
33	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	33	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
34	// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -	34	// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
35	// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware	35	// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
36	// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.	36	// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
37	// + The Software may only be used with the Licensor's products.	37	// + The Software may only be used with the Licensor's products.
38	// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this	38	// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
39	// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this	39	// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
40	// + agreement shall be the property of the Licensor.	40	// + agreement shall be the property of the Licensor.
41	// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other	41	// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
42	// + features that can be used to identify the program may not be altered or defaced by the customer.	42	// + features that can be used to identify the program may not be altered or defaced by the customer.
43	// + The customer shall be responsible for taking reasonable precautions	43	// + The customer shall be responsible for taking reasonable precautions
44	// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the	44	// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
45	// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and	45	// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
46	// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product	46	// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
47	// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.	47	// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
48	// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test	48	// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
49	// + the software for his purpose before any operational usage. The customer will backup his data before using the software.	49	// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
50	// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data	50	// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
51	// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.	51	// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
52	// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.	52	// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
53	// + #### END OF LICENSING TERMS ####	53	// + #### END OF LICENSING TERMS ####
54	// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.	54	// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
55	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	55	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
56	//#define MCLK96MHZ	56	//#define MCLK96MHZ
57	const unsigned long _Main_Crystal = 25000;	57	const unsigned long _Main_Crystal = 25000;
58	#include <stdio.h>	58	#include <stdio.h>
59	#include "91x_lib.h"	59	#include "91x_lib.h"
60	#include "led.h"	60	#include "led.h"
61	#include "uart0.h"	61	#include "uart0.h"
62	#include "uart1.h"	62	#include "uart1.h"
63	#include "uart2.h"	63	#include "uart2.h"
64	#include "gps.h"	64	#include "gps.h"
65	#include "i2c.h"	65	#include "i2c.h"
66	#include "compass.h"	66	#include "compass.h"
67	#include "ncmag.h"	67	#include "ncmag.h"
68	#include "timer1.h"	68	#include "timer1.h"
69	#include "timer2.h"	69	#include "timer2.h"
70	#include "analog.h"	70	#include "analog.h"
71	#include "spi_slave.h"	71	#include "spi_slave.h"
72	#include "fat16.h"	72	#include "fat16.h"
73	#include "usb.h"	73	#include "usb.h"
74	#include "sdc.h"	74	#include "sdc.h"
75	#include "logging.h"	75	#include "logging.h"
76	#include "params.h"	76	#include "params.h"
77	#include "settings.h"	77	#include "settings.h"
78	#include "config.h"	78	#include "config.h"
79	#include "main.h"	79	#include "main.h"
80	#include "debug.h"	80	#include "debug.h"
81	#include "eeprom.h"	81	#include "eeprom.h"
82	#include "ssc.h"	82	#include "ssc.h"
83	#include "sdc.h"	83	#include "sdc.h"
84	#include "uart1.h"	84	#include "uart1.h"
85		85
86		86
87		87
88	#ifdef FOLLOW_ME	88	#ifdef FOLLOW_ME
89	u8 TransmitAlsoToFC = 0;	89	u8 TransmitAlsoToFC = 0;
90	#endif	90	#endif
91	u32 TimerCheckError;	91	u32 TimerCheckError;
92	u8 ErrorCode = 0;	92	u8 ErrorCode = 0;
93	u16 BeepTime;	93	u16 BeepTime;
94	u8 NCFlags = 0;	94	u8 NCFlags = 0;
95	s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg	95	s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg
96	u8 ErrorGpsFixLost = 0;	96	u8 ErrorGpsFixLost = 0;
97		97
98	u8 ClearFCStatusFlags = 0;	98	u8 ClearFCStatusFlags = 0;
99	u8 StopNavigation = 0;	99	u8 StopNavigation = 0;
100	volatile u32 PollingTimeout = 10000;	100	volatile u32 PollingTimeout = 10000;
101	Param_t Parameter;	101	Param_t Parameter;
102	volatile FC_t FC;	102	volatile FC_t FC;
103	volatile u32 SPIWatchDog = 15000; // stop Navigation if this goes to zero	103	volatile u32 SPIWatchDog = 15000; // stop Navigation if this goes to zero
104	volatile u32 SD_WatchDog = 15000; // stop Logging if this goes to zero	104	volatile u32 SD_WatchDog = 15000; // stop Logging if this goes to zero
105	u32 CountGpsProcessedIn5Sec = 0,CountNewGpsDataIn5Sec = 0, FreqGpsProcessedIn5Sec = 0, FreqNewGpsDataIn5Sec = 0;	105	u32 CountGpsProcessedIn5Sec = 0,CountNewGpsDataIn5Sec = 0, FreqGpsProcessedIn5Sec = 0, FreqNewGpsDataIn5Sec = 0;
106		106
107	s8 ErrorMSG[25];	107	s8 ErrorMSG[25];
108		108
109	//----------------------------------------------------------------------------------------------------	109	//----------------------------------------------------------------------------------------------------
110	void SCU_Config(void)	110	void SCU_Config(void)
111	{	111	{
112	/* configure PLL and set it as master clock source */	112	/* configure PLL and set it as master clock source */
113	SCU_MCLKSourceConfig(SCU_MCLK_OSC); // set master clock source to external oscillator clock (25MHz) before diabling the PLL	113	SCU_MCLKSourceConfig(SCU_MCLK_OSC); // set master clock source to external oscillator clock (25MHz) before diabling the PLL
114	SCU_PLLCmd(DISABLE); // now disable the PLL	114	SCU_PLLCmd(DISABLE); // now disable the PLL
115	#ifdef MCLK96MHZ	115	#ifdef MCLK96MHZ
116	SCU_BRCLKDivisorConfig(SCU_BRCLK_Div2); // set BRCLK to MCLK/2 = 48MHz	116	SCU_BRCLKDivisorConfig(SCU_BRCLK_Div2); // set BRCLK to MCLK/2 = 48MHz
117	SCU_PCLKDivisorConfig(SCU_PCLK_Div4); // set PCLK (APB bus clock) divisor to 4 (half Reference Clock)	117	SCU_PCLKDivisorConfig(SCU_PCLK_Div4); // set PCLK (APB bus clock) divisor to 4 (half Reference Clock)
118	SCU_RCLKDivisorConfig(SCU_RCLK_Div2); // set RCLK (Reference Clock) divisor to 1 (full PPL clock)	118	SCU_RCLKDivisorConfig(SCU_RCLK_Div2); // set RCLK (Reference Clock) divisor to 1 (full PPL clock)
119	SCU_HCLKDivisorConfig(SCU_HCLK_Div2); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock)	119	SCU_HCLKDivisorConfig(SCU_HCLK_Div2); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock)
120	SCU_PLLFactorsConfig(192,25,2); // PLL = 96 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=2	120	SCU_PLLFactorsConfig(192,25,2); // PLL = 96 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=2
121	#else	121	#else
122	SCU_BRCLKDivisorConfig(SCU_BRCLK_Div1); // set BRCLK to MCLK = 48MHz	122	SCU_BRCLKDivisorConfig(SCU_BRCLK_Div1); // set BRCLK to MCLK = 48MHz
123	SCU_PCLKDivisorConfig(SCU_PCLK_Div2); // set PCLK (APB bus clock) divisor to 2 (half Reference Clock)	123	SCU_PCLKDivisorConfig(SCU_PCLK_Div2); // set PCLK (APB bus clock) divisor to 2 (half Reference Clock)
124	SCU_RCLKDivisorConfig(SCU_RCLK_Div1); // set RCLK (Reference Clock) divisor to 1 (full PPL clock)	124	SCU_RCLKDivisorConfig(SCU_RCLK_Div1); // set RCLK (Reference Clock) divisor to 1 (full PPL clock)
125	SCU_HCLKDivisorConfig(SCU_HCLK_Div1); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock)	125	SCU_HCLKDivisorConfig(SCU_HCLK_Div1); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock)
126	SCU_PLLFactorsConfig(192,25,3); // PLL = 48 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=3	126	SCU_PLLFactorsConfig(192,25,3); // PLL = 48 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=3
127	#endif	127	#endif
128	SCU_PLLCmd(ENABLE); // Enable PLL (is disabled by SCU_PLLFactorsConfig)	128	SCU_PLLCmd(ENABLE); // Enable PLL (is disabled by SCU_PLLFactorsConfig)
129	SCU_MCLKSourceConfig(SCU_MCLK_PLL); // set master clock source to PLL	129	SCU_MCLKSourceConfig(SCU_MCLK_PLL); // set master clock source to PLL
130	}	130	}
131		131
132	//----------------------------------------------------------------------------------------------------	132	//----------------------------------------------------------------------------------------------------
133	void GetNaviCtrlVersion(void)	133	void GetNaviCtrlVersion(void)
134	{	134	{
135	u8 msg[25];	135	u8 msg[25];
136		136
137	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);	137	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);
138	UART1_PutString(msg);	138	UART1_PutString(msg);
139	}	139	}
140		140
141	//----------------------------------------------------------------------------------------------------	141	//----------------------------------------------------------------------------------------------------
142		142
143	void CheckErrors(void)	143	void CheckErrors(void)
144	{	144	{
145	static s32 no_error_delay = 0;	145	static s32 no_error_delay = 0;
146	s32 newErrorCode = 0;	146	s32 newErrorCode = 0;
147	UART_VersionInfo.HardwareError[0] = 0;	147	UART_VersionInfo.HardwareError[0] = 0;
148		148
149	if(CheckDelay(I2C1_Timeout) \|\| (Compass_Heading < 0)) DebugOut.StatusRed \|= AMPEL_COMPASS;	149	if(CheckDelay(I2C1_Timeout) \|\| (Compass_Heading < 0)) DebugOut.StatusRed \|= AMPEL_COMPASS;
150	else DebugOut.StatusRed &= ~AMPEL_COMPASS; // MK3Mag green status	150	else DebugOut.StatusRed &= ~AMPEL_COMPASS; // MK3Mag green status
151		151
152	if((FC.Error[1] & FC_ERROR1_I2C) \|\| (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.StatusRed \|= AMPEL_BL;	152	if((FC.Error[1] & FC_ERROR1_I2C) \|\| (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.StatusRed \|= AMPEL_BL;
153	else DebugOut.StatusRed &= ~AMPEL_BL; // BL-Ctrl green status	153	else DebugOut.StatusRed &= ~AMPEL_BL; // BL-Ctrl green status
154		154
155	if(UART_VersionInfo.HardwareError[0] \|\| UART_VersionInfo.HardwareError[1]) DebugOut.StatusRed \|= AMPEL_NC;	155	if(UART_VersionInfo.HardwareError[0] \|\| UART_VersionInfo.HardwareError[1]) DebugOut.StatusRed \|= AMPEL_NC;
156	else DebugOut.StatusRed &= ~AMPEL_NC;	156	else DebugOut.StatusRed &= ~AMPEL_NC;
157		157
158	if((FCCalibActive \|\| CompassCalState) && FC_Version.Hardware)	158	if((FCCalibActive \|\| CompassCalState) && FC_Version.Hardware)
159	{	159	{
160	sprintf(ErrorMSG,"Calibrate... ");	160	sprintf(ErrorMSG,"Calibrate... ");
161	newErrorCode = 0;	161	newErrorCode = 0;
162	ErrorCode = 0;	162	ErrorCode = 0;
163	no_error_delay = 1;	163	no_error_delay = 1;
164	}	164	}
165	else	165	else
166	if(CheckDelay(SPI0_Timeout))	166	if(CheckDelay(SPI0_Timeout))
167	{	167	{
168	LED_RED_ON;	168	LED_RED_ON;
169	sprintf(ErrorMSG,"no FC communication ");	169	sprintf(ErrorMSG,"no FC communication ");
170	newErrorCode = 3;	170	newErrorCode = 3;
171	StopNavigation = 1;	171	StopNavigation = 1;
172	DebugOut.StatusGreen &= ~AMPEL_FC; // status of FC Present	172	DebugOut.StatusGreen &= ~AMPEL_FC; // status of FC Present
173	DebugOut.StatusGreen &= ~AMPEL_BL; // status of BL Present	173	DebugOut.StatusGreen &= ~AMPEL_BL; // status of BL Present
174	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_SPI_RX;	174	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_SPI_RX;
175	}	175	}
176	else if(CheckDelay(I2C1_Timeout))	176	else if(CheckDelay(I2C1_Timeout))
177	{	177	{
178	LED_RED_ON;	178	LED_RED_ON;
179	sprintf(ErrorMSG,"no compass communica");	179	sprintf(ErrorMSG,"no compass communica");
180	//Reset I2CBus	180	//Reset I2CBus
181	I2C1_Deinit();	181	I2C1_Deinit();
182	I2C1_Init();	182	I2C1_Init();
183	newErrorCode = 4;	183	newErrorCode = 4;
184	StopNavigation = 1;	184	StopNavigation = 1;
185	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_RX;	185	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_RX;
186	DebugOut.StatusRed \|= AMPEL_COMPASS;	186	DebugOut.StatusRed \|= AMPEL_COMPASS;
187	}	187	}
188	else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)	188	else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
189	{	189	{
190	LED_RED_ON;	190	LED_RED_ON;
191	#ifndef FOLLOW_ME	191	#ifndef FOLLOW_ME
192	sprintf(ErrorMSG,"FC not compatible ");	192	sprintf(ErrorMSG,"FC not compatible ");
193	#else	193	#else
194	sprintf(ErrorMSG,"! FollowMe only ! ");	194	sprintf(ErrorMSG,"! FollowMe only ! ");
195	#endif	195	#endif
196	newErrorCode = 1;	196	newErrorCode = 1;
197	StopNavigation = 1;	197	StopNavigation = 1;
198	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_FC_INCOMPATIBLE;	198	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_FC_INCOMPATIBLE;
199	DebugOut.StatusRed \|= AMPEL_NC;	199	DebugOut.StatusRed \|= AMPEL_NC;
200	}	200	}
201		201
202	else if(FC.Error[0] & FC_ERROR0_GYRO_NICK)	202	else if(FC.Error[0] & FC_ERROR0_GYRO_NICK)
203	{	203	{
204	LED_RED_ON;	204	LED_RED_ON;
205	sprintf(ErrorMSG,"ERR: FC Nick Gyro");	205	sprintf(ErrorMSG,"ERR: FC Nick Gyro");
206	newErrorCode = 10;	206	newErrorCode = 10;
207	}	207	}
208	else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL)	208	else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL)
209	{	209	{
210	LED_RED_ON;	210	LED_RED_ON;
211	sprintf(ErrorMSG,"ERR: FC Roll Gyro");	211	sprintf(ErrorMSG,"ERR: FC Roll Gyro");
212	newErrorCode = 11;	212	newErrorCode = 11;
213	}	213	}
214	else if(FC.Error[0] & FC_ERROR0_GYRO_YAW)	214	else if(FC.Error[0] & FC_ERROR0_GYRO_YAW)
215	{	215	{
216	LED_RED_ON;	216	LED_RED_ON;
217	sprintf(ErrorMSG,"ERR: FC Yaw Gyro");	217	sprintf(ErrorMSG,"ERR: FC Yaw Gyro");
218	newErrorCode = 12;	218	newErrorCode = 12;
219	}	219	}
220	else if(FC.Error[0] & FC_ERROR0_ACC_NICK)	220	else if(FC.Error[0] & FC_ERROR0_ACC_NICK)
221	{	221	{
222	LED_RED_ON;	222	LED_RED_ON;
223	sprintf(ErrorMSG,"ERR: FC Nick ACC");	223	sprintf(ErrorMSG,"ERR: FC Nick ACC");
224	newErrorCode = 13;	224	newErrorCode = 13;
225	}	225	}
226	else if(FC.Error[0] & FC_ERROR0_ACC_ROLL)	226	else if(FC.Error[0] & FC_ERROR0_ACC_ROLL)
227	{	227	{
228	LED_RED_ON;	228	LED_RED_ON;
229	sprintf(ErrorMSG,"ERR: FC Roll ACC");	229	sprintf(ErrorMSG,"ERR: FC Roll ACC");
230	newErrorCode = 14;	230	newErrorCode = 14;
231	}	231	}
232	else if(FC.Error[0] & FC_ERROR0_ACC_TOP)	232	else if(FC.Error[0] & FC_ERROR0_ACC_TOP)
233	{	233	{
234	LED_RED_ON;	234	LED_RED_ON;
235	sprintf(ErrorMSG,"ERR:FC Z-ACC");	235	sprintf(ErrorMSG,"ERR:FC Z-ACC");
236	newErrorCode = 15;	236	newErrorCode = 15;
237	}	237	}
238	else if(NC_To_FC_Flags & NC_TO_FC_FLYING_RANGE)	238	else if(NC_To_FC_Flags & NC_TO_FC_FLYING_RANGE)
239	{	239	{
240	LED_RED_ON;	240	LED_RED_ON;
241	sprintf(ErrorMSG,"ERR:Flying range!");	241	sprintf(ErrorMSG,"ERR:Flying range!");
242	newErrorCode = 28;	242	newErrorCode = 28;
243	}	243	}
244	else if(FC.Error[0] & FC_ERROR0_PRESSURE)	244	else if(FC.Error[0] & FC_ERROR0_PRESSURE)
245	{	245	{
246	LED_RED_ON;	246	LED_RED_ON;
247	sprintf(ErrorMSG,"ERR:Pressure sensor");	247	sprintf(ErrorMSG,"ERR:Pressure sensor");
248	newErrorCode = 16;	248	newErrorCode = 16;
249	}	249	}
250	else if(FC.Error[1] & FC_ERROR1_I2C)	250	else if(FC.Error[1] & FC_ERROR1_I2C)
251	{	251	{
252	LED_RED_ON;	252	LED_RED_ON;
253	sprintf(ErrorMSG,"ERR:I2C FC to BL");	253	sprintf(ErrorMSG,"ERR:I2C FC to BL");
254	newErrorCode = 17;	254	newErrorCode = 17;
255	}	255	}
256	else if(FC.Error[1] & FC_ERROR1_BL_MISSING)	256	else if(FC.Error[1] & FC_ERROR1_BL_MISSING)
257	{	257	{
258	LED_RED_ON;	258	LED_RED_ON;
259	sprintf(ErrorMSG,"ERR: Bl Missing");	259	sprintf(ErrorMSG,"ERR: Bl Missing");
260	newErrorCode = 18;	260	newErrorCode = 18;
261	}	261	}
262	else if(FC.Error[1] & FC_ERROR1_MIXER)	262	else if(FC.Error[1] & FC_ERROR1_MIXER)
263	{	263	{
264	LED_RED_ON;	264	LED_RED_ON;
265	sprintf(ErrorMSG,"Mixer Error");	265	sprintf(ErrorMSG,"Mixer Error");
266	newErrorCode = 19;	266	newErrorCode = 19;
267	}	267	}
268	else if(CheckDelay(UBX_Timeout) && Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)	268	else if(CheckDelay(UBX_Timeout) && Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)
269	{	269	{
270	LED_RED_ON;	270	LED_RED_ON;
271	// if(!(Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) sprintf(ErrorMSG,"GPS disconnected ");	271	// if(!(Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) sprintf(ErrorMSG,"GPS disconnected ");
272	// else	272	// else
273	{	273	{
274	sprintf(ErrorMSG,"no GPS communication");	274	sprintf(ErrorMSG,"no GPS communication");
275	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_GPS_RX;	275	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_GPS_RX;
276	UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_GPS_PRESENT;	276	UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_GPS_PRESENT;
277	newErrorCode = 5;	277	newErrorCode = 5;
278	}	278	}
279	StopNavigation = 1;	279	StopNavigation = 1;
280	// UBX_Timeout = SetDelay(500);	280	// UBX_Timeout = SetDelay(500);
281	}	281	}
282	else if(Compass_Heading < 0 && NCMAG_Present && !NCMAG_IsCalibrated)	282	else if(Compass_Heading < 0 && NCMAG_Present && !NCMAG_IsCalibrated)
283	{	283	{
284	LED_RED_ON;	284	LED_RED_ON;
285	sprintf(ErrorMSG,"compass not calibr.");	285	sprintf(ErrorMSG,"compass not calibr.");
286	newErrorCode = 31;	286	newErrorCode = 31;
287	StopNavigation = 1;	287	StopNavigation = 1;
288	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_VALUE;	288	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_VALUE;
289	}	289	}
290	else if(Compass_Heading < 0)	290	else if(Compass_Heading < 0)
291	{	291	{
292	LED_RED_ON;	292	LED_RED_ON;
293	sprintf(ErrorMSG,"bad compass value ");	293	sprintf(ErrorMSG,"bad compass value ");
294	newErrorCode = 6;	294	newErrorCode = 6;
295	StopNavigation = 1;	295	StopNavigation = 1;
296	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_VALUE;	296	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_VALUE;
297	}	297	}
298	else if((FC.Error[1] & FC_ERROR1_SPI_RX))	298	else if((FC.Error[1] & FC_ERROR1_SPI_RX))
299	{	299	{
300	LED_RED_ON;	300	LED_RED_ON;
301	sprintf(ErrorMSG,"FC spi rx error ");	301	sprintf(ErrorMSG,"FC spi rx error ");
302	newErrorCode = 8;	302	newErrorCode = 8;
303	StopNavigation = 1;	303	StopNavigation = 1;
304	}	304	}
305	else if(FC.Error[0] & FC_ERROR0_CAREFREE)	305	else if(FC.Error[0] & FC_ERROR0_CAREFREE)
306	{	306	{
307	LED_RED_ON;	307	LED_RED_ON;
308	sprintf(ErrorMSG,"FC: Carefree Error");	308	sprintf(ErrorMSG,"FC: Carefree Error");
309	newErrorCode = 20;	309	newErrorCode = 20;
310	}	310	}
311	else if(FC.Error[1] & FC_ERROR1_PPM)	311	else if(FC.Error[1] & FC_ERROR1_PPM)
312	{	312	{
313	LED_RED_ON;	313	LED_RED_ON;
314	sprintf(ErrorMSG,"RC Signal lost ");	314	sprintf(ErrorMSG,"RC Signal lost ");
315	newErrorCode = 7;	315	newErrorCode = 7;
316	}	316	}
317	else if(ErrorGpsFixLost)	317	else if(ErrorGpsFixLost)
318	{	318	{
319	LED_RED_ON;	319	LED_RED_ON;
320	sprintf(ErrorMSG,"GPS Fix lost ");	320	sprintf(ErrorMSG,"GPS Fix lost ");
321	newErrorCode = 21;	321	newErrorCode = 21;
322	}	322	}
323	else if(ErrorDisturbedEarthMagnetField)	323	else if(ErrorDisturbedEarthMagnetField)
324	{	324	{
325	LED_RED_ON;	325	LED_RED_ON;
326	sprintf(ErrorMSG,"Magnet error ");	326	sprintf(ErrorMSG,"Magnet error ");
327	newErrorCode = 22;	327	newErrorCode = 22;
328	DebugOut.StatusRed \|= AMPEL_COMPASS \| AMPEL_NC;	328	DebugOut.StatusRed \|= AMPEL_COMPASS \| AMPEL_NC;
329	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_VALUE;	329	UART_VersionInfo.HardwareError[0] \|= NC_ERROR0_COMPASS_VALUE;
330	}	330	}
331	else if(BL_MinOfMaxPWM == 40 && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)	331	else if(BL_MinOfMaxPWM == 40 && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)
332	{	332	{
333	LED_RED_ON;	333	LED_RED_ON;
334	sprintf(ErrorMSG,"ERR:Motor restart ");	334	sprintf(ErrorMSG,"ERR:Motor restart ");
335	newErrorCode = 23;	335	newErrorCode = 23;
336	DebugOut.StatusRed \|= AMPEL_BL;	336	DebugOut.StatusRed \|= AMPEL_BL;
337	}	337	}
338	else if(BL_MinOfMaxPWM != 255 && (BL_MinOfMaxPWM != 250) && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)	338	else if(BL_MinOfMaxPWM != 255 && (BL_MinOfMaxPWM != 250) && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)
339	{	339	{
340	LED_RED_ON;	340	LED_RED_ON;
341	sprintf(ErrorMSG,"ERR:BL Limitation ");	341	sprintf(ErrorMSG,"ERR:BL Limitation ");
342	newErrorCode = 24;	342	newErrorCode = 24;
343	DebugOut.StatusRed \|= AMPEL_BL;	343	DebugOut.StatusRed \|= AMPEL_BL;
344	}	344	}
345	else if(NCFlags & NC_FLAG_RANGE_LIMIT && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)	345	else if(NCFlags & NC_FLAG_RANGE_LIMIT && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)
346	{	346	{
347	LED_RED_ON;	347	LED_RED_ON;
348	sprintf(ErrorMSG,"ERR:GPS range ");	348	sprintf(ErrorMSG,"ERR:GPS range ");
349	newErrorCode = 25;	349	newErrorCode = 25;
350	DebugOut.StatusRed \|= AMPEL_NC;	350	DebugOut.StatusRed \|= AMPEL_NC;
351	}	351	}
352	else if((!SD_SWITCH \|\| (SDCardInfo.Valid == 0)) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START && !(FC.StatusFlags & FC_STATUS_FLY))	352	else if((!SD_SWITCH \|\| (SDCardInfo.Valid == 0)) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START && !(FC.StatusFlags & FC_STATUS_FLY))
353	{	353	{
354	LED_RED_ON;	354	LED_RED_ON;
355	sprintf(ErrorMSG,"ERR:No SD-Card ");	355	sprintf(ErrorMSG,"ERR:No SD-Card ");
356	newErrorCode = 26;	356	newErrorCode = 26;
357	DebugOut.StatusRed \|= AMPEL_NC;	357	DebugOut.StatusRed \|= AMPEL_NC;
358	}	358	}
359	else if((SD_LoggingError \|\| (SD_WatchDog < 2000 && SD_WatchDog != 0)) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START)	359	else if((SD_LoggingError \|\| (SD_WatchDog < 2000 && SD_WatchDog != 0)) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START)
360	{	360	{
361	LED_RED_ON;	361	LED_RED_ON;
362	sprintf(ErrorMSG,"ERR:SD Logging abort");	362	sprintf(ErrorMSG,"ERR:SD Logging abort");
363	newErrorCode = 27;	363	newErrorCode = 27;
364	DebugOut.StatusRed \|= AMPEL_NC;	364	DebugOut.StatusRed \|= AMPEL_NC;
365	SD_LoggingError = 0;	365	SD_LoggingError = 0;
366	}	366	}
367	else if(((AbsoluteFlyingAltitude) && (NaviData.Altimeter / 20 >= AbsoluteFlyingAltitude)) && (FC.StatusFlags & FC_STATUS_FLY))	367	else if(((AbsoluteFlyingAltitude) && (NaviData.Altimeter / 20 >= AbsoluteFlyingAltitude)) && (FC.StatusFlags & FC_STATUS_FLY))
368	{	368	{
369	LED_RED_ON;	369	LED_RED_ON;
370	sprintf(ErrorMSG,"ERR:Max Altitude ");	370	sprintf(ErrorMSG,"ERR:Max Altitude ");
371	newErrorCode = 29;	371	newErrorCode = 29;
372	DebugOut.StatusRed \|= AMPEL_NC;	372	DebugOut.StatusRed \|= AMPEL_NC;
373	}	373	}
374	else if(Parameter.GlobalConfig3 & CFG3_NO_GPSFIX_NO_START && !(NCFlags & NC_FLAG_GPS_OK) && ((FC.StatusFlags & (FC_STATUS_START \| FC_STATUS_MOTOR_RUN)) \|\| (FC.StickGas < -50 && FC.StickYaw < -50)))	374	else if(Parameter.GlobalConfig3 & CFG3_NO_GPSFIX_NO_START && !(NCFlags & NC_FLAG_GPS_OK) && ((FC.StatusFlags & (FC_STATUS_START \| FC_STATUS_MOTOR_RUN)) \|\| (FC.StickGas < -50 && FC.StickYaw < -50)))
375	{	375	{
376	LED_RED_ON;	376	LED_RED_ON;
377	sprintf(ErrorMSG,"No GPS Fix ");	377	sprintf(ErrorMSG,"No GPS Fix ");
378	newErrorCode = 30;	378	newErrorCode = 30;
379	}	379	}
380	else // no error occured	380	else // no error occured
381	{	381	{
382	StopNavigation = 0;	382	StopNavigation = 0;
383	LED_RED_OFF;	383	LED_RED_OFF;
384	if(no_error_delay) { no_error_delay--; }	384	if(no_error_delay) { no_error_delay--; }
385	else	385	else
386	{	386	{
387	sprintf(ErrorMSG,"No Error ");	387	sprintf(ErrorMSG,"No Error ");
388	ErrorCode = 0;	388	ErrorCode = 0;
389	}	389	}
390	}	390	}
391		391
392	if(newErrorCode)	392	if(newErrorCode)
393	{	393	{
394	if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 8; // delay the errors if the motors are running	394	if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 8; // delay the errors if the motors are running
395	ErrorCode = newErrorCode;	395	ErrorCode = newErrorCode;
396	}	396	}
397	FC.Error[0] = 0;	397	FC.Error[0] = 0;
398	FC.Error[1] = 0;	398	FC.Error[1] = 0;
399	FC.Error[2] = 0;	399	FC.Error[2] = 0;
400	FC.Error[3] = 0;	400	FC.Error[3] = 0;
401	FC.Error[4] = 0;	401	FC.Error[4] = 0;
402	ErrorGpsFixLost = 0;	402	ErrorGpsFixLost = 0;
403	}	403	}
404		404
405		405
406		406
407	void Polling(void)	407	void Polling(void)
408	{	408	{
409	static u8 running = 0, oldFcFlags = 0, count5sec;	409	static u8 running = 0, oldFcFlags = 0, count5sec;
410	if(running) return;	410	if(running) return;
411	running = 1;	411	running = 1;
412	SPI0_UpdateBuffer(); // also calls the GPS-functions	412	SPI0_UpdateBuffer(); // also calls the GPS-functions
413	UART0_ProcessRxData(); // GPS process request	413	UART0_ProcessRxData(); // GPS process request
414	UART0_TransmitTxData(); // GPS send answer	414	UART0_TransmitTxData(); // GPS send answer
415	UART1_ProcessRxData(); // PC process request	415	UART1_ProcessRxData(); // PC process request
416	UART1_TransmitTxData(); // PC send answer	416	UART1_TransmitTxData(); // PC send answer
417	UART2_TransmitTxData(); // FC send answer	417	UART2_TransmitTxData(); // FC send answer
418	CalcHeadFree();	418	CalcHeadFree();
419	// ---------------- Error Check Timing ----------------------------	419	// ---------------- Error Check Timing ----------------------------
420	if(CheckDelay(TimerCheckError) \|\| (FC.StatusFlags & FC_STATUS_START && !(oldFcFlags & FC_STATUS_START))) // Timer or FY wants to start	420	if(CheckDelay(TimerCheckError) \|\| (FC.StatusFlags & FC_STATUS_START && !(oldFcFlags & FC_STATUS_START))) // Timer or FY wants to start
421	{	421	{
422	//CreateNmeaGGA();	422	//CreateNmeaGGA();
423	if(CheckDelay(TimerCheckError))	423	if(CheckDelay(TimerCheckError))
424	{	424	{
425	TimerCheckError = SetDelay(1000);	425	TimerCheckError = SetDelay(1000);
426	if(++count5sec == 5)	426	if(++count5sec == 5)
427	{	427	{
428	count5sec = 0;	428	count5sec = 0;
429	FreqGpsProcessedIn5Sec = CountGpsProcessedIn5Sec * 2;	429	FreqGpsProcessedIn5Sec = CountGpsProcessedIn5Sec * 2;
430	FreqNewGpsDataIn5Sec = CountNewGpsDataIn5Sec * 2;	430	FreqNewGpsDataIn5Sec = CountNewGpsDataIn5Sec * 2;
431	CountGpsProcessedIn5Sec = 0;	431	CountGpsProcessedIn5Sec = 0;
432	CountNewGpsDataIn5Sec = 0;	432	CountNewGpsDataIn5Sec = 0;
433	}	433	}
434	}	434	}
435	oldFcFlags = FC.StatusFlags;	435	oldFcFlags = FC.StatusFlags;
436	if(CheckDelay(SPI0_Timeout) && (DebugUART == UART1)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected	436	if(CheckDelay(SPI0_Timeout) && (DebugUART == UART1)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected
437		437
438	if(!CheckDelay(SPI0_Timeout) \|\| (DebugUART == UART1)) CheckErrors();	438	if(!CheckDelay(SPI0_Timeout) \|\| (DebugUART == UART1)) CheckErrors();
439		439
440	if(FC.StatusFlags & FC_STATUS_FLY) NaviData.FlyingTime++; // we want to count the battery-time	440	if(FC.StatusFlags & FC_STATUS_FLY) NaviData.FlyingTime++; // we want to count the battery-time
441	// else NaviData.FlyingTime = 0; // not the time per flight	441	// else NaviData.FlyingTime = 0; // not the time per flight
442	if(SerialLinkOkay) SerialLinkOkay--;	442	if(SerialLinkOkay) SerialLinkOkay--;
443	if(SerialLinkOkay < 250 - 5) NCFlags \|= NC_FLAG_NOSERIALLINK; // 5 seconds timeout for serial communication	443	if(SerialLinkOkay < 250 - 5) NCFlags \|= NC_FLAG_NOSERIALLINK; // 5 seconds timeout for serial communication
444	else NCFlags &= ~NC_FLAG_NOSERIALLINK;	444	else NCFlags &= ~NC_FLAG_NOSERIALLINK;
445	if(StopNavigation && (Parameter.NaviGpsModeControl >= 50) && (Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) BeepTime = 1000;	445	if(StopNavigation && (Parameter.NaviGpsModeControl >= 50) && (Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) BeepTime = 1000;
446	}	446	}
447	running = 0;	447	running = 0;
448	}	448	}
449		449
450	// the handler will be cyclic called by the timer 1 ISR	450	// the handler will be cyclic called by the timer 1 ISR
451	// used is for critical timing parts that normaly would handled	451	// used is for critical timing parts that normaly would handled
452	// within the main loop that could block longer at logging activities	452	// within the main loop that could block longer at logging activities
453	void EXTIT3_IRQHandler(void)	453	void EXTIT3_IRQHandler(void)
454	{	454	{
455	IENABLE;	455	IENABLE;
456	VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq	456	VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq
457	Compass_Update(); // update compass communication	457	Compass_Update(); // update compass communication
458	Analog_Update(); // get new ADC values	458	Analog_Update(); // get new ADC values
459		459
460	if(!PollingTimeout)	460	if(!PollingTimeout)
461	{	461	{
462	PollingTimeout = 5;	462	PollingTimeout = 5;
463	Polling();	463	Polling();
464	}	464	}
465		465
466	VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit	466	VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit
467	VIC_ITCmd(EXTIT3_ITLine, ENABLE); // enable irq	467	VIC_ITCmd(EXTIT3_ITLine, ENABLE); // enable irq
468	IDISABLE;	468	IDISABLE;
469	VIC1->VAR = 0xFF; // write any value to VIC0 Vector address register	469	VIC1->VAR = 0xFF; // write any value to VIC0 Vector address register
470	}	470	}
471		471
472	//----------------------------------------------------------------------------------------------------	472	//----------------------------------------------------------------------------------------------------
473	int main(void)	473	int main(void)
474	{	474	{
475		475
476	static u32 ftimer =0;	476	static u32 ftimer =0;
477	static u8 fstate = 0;	477	static u8 fstate = 0;
478	// static File_t* f = NULL;	478	// static File_t* f = NULL;
479		479
480		480
481	/* Configure the system clocks */	481	/* Configure the system clocks */
482	SCU_Config();	482	SCU_Config();
483	/* init VIC (Vectored Interrupt Controller) */	483	/* init VIC (Vectored Interrupt Controller) */
484	SCU_AHBPeriphClockConfig(__VIC,ENABLE); // enable AHB bus clock for VIC	484	SCU_AHBPeriphClockConfig(__VIC,ENABLE); // enable AHB bus clock for VIC
485	SCU_AHBPeriphReset(__VIC, DISABLE); // disable reset state for VIC	485	SCU_AHBPeriphReset(__VIC, DISABLE); // disable reset state for VIC
486	VIC_DeInit(); // deinitializes the VIC module registers to their default reset values.	486	VIC_DeInit(); // deinitializes the VIC module registers to their default reset values.
487	VIC_InitDefaultVectors();	487	VIC_InitDefaultVectors();
488		488
489	// initialize timer 1 for System Clock and delay rountines	489	// initialize timer 1 for System Clock and delay rountines
490	TIMER1_Init();	490	TIMER1_Init();
491	// initialize the LEDs (needs Timer 1)	491	// initialize the LEDs (needs Timer 1)
492	Led_Init();	492	Led_Init();
493	// initialize the debug UART1	493	// initialize the debug UART1
494	UART1_Init();	494	UART1_Init();
495	UART1_PutString("\r\n---------------------------------------------");	495	UART1_PutString("\r\n---------------------------------------------");
496	// initialize usb	496	// initialize usb
497	USB_ConfigInit();	497	USB_ConfigInit();
498	// initialize timer 2 for servo outputs	498	// initialize timer 2 for servo outputs
499	//TIMER2_Init();	499	//TIMER2_Init();
500	// initialize UART2 to FLIGHTCTRL	500	// initialize UART2 to FLIGHTCTRL

Subversion Repositories NaviCtrl

(root)/tags/V2.04a/main.c @ 669 - Rev 461 → 465