Subversion Repositories NaviCtrl

/*#######################################################################################*/

/* !!! 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 <string.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 "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"

#include "canbus.h"

#include "triggerlog.h"

#include "CamCtrl.h"

#ifdef FOLLOW_ME

u8 TransmitAlsoToFC = 0;

#endif

u32 TimerCheckError;

u32 TimerSecond;

u8 ErrorCode = 0;

u16 BeepTime;

u8  NCFlags = 0;

s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg

u8 ErrorGpsFixLost = 0;

u8 FromFC_LoadWP_List = 0, FromFC_Load_SinglePoint = 0, FromFC_Save_SinglePoint = 0;

u8 ToFC_MaxWpListIndex = 4;

u8 ClearFCStatusFlags = 0;

u8 StopNavigation = 0;

volatile u32 PollingTimeout = 10000;

Param_t Parameter;

Partner_t Partner;

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 = 50,CountNewGpsDataIn5Sec = 25, FreqGpsNavProcessed = 0, FreqNewGpsData = 0;

u8 NewWPL_Name = 0;

u32 MaxWP_Radius_in_m = 0;

s8 ErrorMSG[25];

s8 PartnerErrorMSG[25] = "     ---                \0";

u8 OEM_String[OEM_NAME_LENGHT+1] = {0xff,' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',0x00};

u32 TimeSinceMotorStart = 0;

u8 IamMaster = NOTHING; // for Master/Slave Redundance

u16 ToFC_Parachute_Off;

u8 IO1_Function = 0;

s16 LuftdruckTemperaturKompensation = 0, LuftdruckTemperaturOffset = 0;

u8 BaroCalState = 0;

u8 ErrorOutSideOperationArea = 0; // I am outside the operation polygon area

u32 ShowNoFlyzoneErrorMessage = 0;

u8 ShowCalibrationErrorMessage = 0;

u8 TryAgain_UBX_Setup = 0;

u8 MenuBlinkBit;

//----------------------------------------------------------------------------------------------------

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,"\r\n NaviCtrl HW: V%d.%d SW: V%d.%02d%c", UART_VersionInfo.HWMajor/10, UART_VersionInfo.HWMajor%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(I2CBus(Compass_I2CPort)->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(CheckDelay(I2CBus(Compass_I2CPort)->Timeout))

        {

                LED_RED_ON;

                sprintf(ErrorMSG,"no compass communica");

                //Reset Compass communication

                if(Compass_I2CPort == NCMAG_PORT_EXTERN) I2CBus_Init(I2C0);     else I2CBus_Init(I2C1);

                Compass_Init();

                newErrorCode = 4;

                StopNavigation = 1;

                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX;

                DebugOut.StatusRed |= AMPEL_COMPASS;

        }

        else if(CompassValueErrorCount > 30)

        {

                LED_RED_ON;

                sprintf(ErrorMSG,"compass sensor error");

                newErrorCode = 34;

                StopNavigation = 1;

                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;

                if(Compass_I2CPort == NCMAG_PORT_EXTERN) I2CBus_Init(I2C0);     else I2CBus_Init(I2C1);

                Compass_Init();

        }

        else 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(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(ToFC_Parachute_Off)

        {

                LED_RED_ON;

                sprintf(ErrorMSG,"ERR: PARACHUTE");

                newErrorCode = 43;

        }

        else if(FC.Error[1] &  FC_ERROR1_SPI_RX)

        {

                LED_RED_ON;

                sprintf(ErrorMSG,"FC spi rx error ");

                newErrorCode = 8;

                StopNavigation = 1;

        }

        else if(CntSpiErrorPerSecond > 2)

        {

                LED_RED_ON;

                newErrorCode = 8;

                sprintf(ErrorMSG,"FC spi CRC error");

                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX;

        }

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

                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;

        }

        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[0] &  FC_ERROR0_CAREFREE)

        {

                LED_RED_ON;

                sprintf(ErrorMSG,"FC: Carefree Error");

                newErrorCode = 20;

        }

        else if(FC.BAT_Voltage < 45)

        {

         LED_RED_ON;

         sprintf(ErrorMSG,"ERR:Power Supply");

         newErrorCode = 41;

        }

        else

        if(FC.Error[1] & FC_ERROR1_RC_VOLTAGE)

        {

         LED_RED_ON;

         sprintf(ErrorMSG,"ERR: 5V RC-Supply");

         newErrorCode = 40;

        }

        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(NC_To_FC_Flags & NC_TO_FC_FLYING_RANGE)

        {

                LED_RED_ON;

                sprintf(ErrorMSG,"ERR:Flying range!");

                newErrorCode = 28;

        }

        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(((ErrorCheck_BL_MinOfMaxPWM == 40 && (TimeSinceMotorStart > 3))  || (ErrorCheck_BL_MinOfMaxPWM == 39)) && !ErrorCode)

        {

                LED_RED_ON;

                sprintf(ErrorMSG,"ERR:Motor restart  ");

                newErrorCode = 23;

                DebugOut.StatusRed |= AMPEL_BL;

        }

        else if(BL_MinOfMaxPWM && BL_MinOfMaxPWM < 30 && !ErrorCode)

        {

                u16 i;

                for(i = 0; i < 16; i++) if(Motor[i].MaxPWM == BL_MinOfMaxPWM) break; // find the motor number

                LED_RED_ON;

                sprintf(ErrorMSG,"ERR:BL%2d Test:%2d ",i+1,BL_MinOfMaxPWM);

                newErrorCode = 32;

                DebugOut.StatusRed |= AMPEL_BL;

        }

        else if(ErrorCheck_BL_MinOfMaxPWM < 248 && (TimeSinceMotorStart > 3) && !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 WP range ");

                newErrorCode = 25;

                DebugOut.StatusRed |= AMPEL_NC;

        }

        else if((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_5cm / 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 if((FreqNewGpsData <= 35 || FreqNewGpsData > 60) && TimeSinceMotorStart > 15)

        {

                LED_RED_ON;

                sprintf(ErrorMSG,"GPS Update rate ");

                newErrorCode = 38;

        }

        else if(NC_GPS_ModeCharacter == 'F')

        {                                                                

                sprintf(ErrorMSG,"FAILSAFE pos.!  ");

                newErrorCode = 35;

        }

        else if(ErrorOutSideOperationArea && FC_is_Calibrated)

        {

                LED_RED_ON;

                sprintf(ErrorMSG,"ERR:Outside Flyzone ");

                newErrorCode = 44;

                DebugOut.StatusRed |= AMPEL_NC;

        }

        else if(FC.StatusFlags3 & FC_STATUS3_REDUNDANCE_ERROR)

        {                                                                

                sprintf(ErrorMSG,"ERR:Redundancy  ");

                newErrorCode = 36;

        }

        else if(FC.StatusFlags3 & FC_STATUS3_REDUNDANCE_TEST)

        {                                                                

                sprintf(ErrorMSG,"Redundancy test ");

                newErrorCode = 37;

        }

        else if(CanbusTimeOut == 1)

        {                                                                

                sprintf(ErrorMSG,"ERR: Canbus");

                CanbusInit();

                newErrorCode = 39;

        }

        else

        if(FC.Error[1] & FC_ERROR1_ACC_NOT_CAL)

        {

         LED_RED_ON;

         sprintf(ErrorMSG,"ACC not calib.");

         newErrorCode = 42;

        }

        else

        if(ShowNoFlyzoneErrorMessage > 5)

        {

         LED_RED_ON;

         sprintf(ErrorMSG,"ERR: no Flyzone ");

         newErrorCode = 45;

        }

        else

        if(ShowCalibrationErrorMessage)

        {

         LED_RED_ON;

         sprintf(ErrorMSG,"ERR: Calibration ");

         newErrorCode = 46;

        }

        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;

 ErrorCheck_BL_MinOfMaxPWM = BL_MinOfMaxPWM;

}

unsigned char RAM_Checksum(unsigned char* pBuffer, u16 len)

{

        u8 crc = 0xAA;

        u16 i;

        for(i=0; i<len; i++) crc += pBuffer[i];

        return crc;

}

u8 Polling(void)

{

        static u8 running = 0, oldFcFlags = 0, count5sec, TimeoutGPS_Process, t20ms;

        static u32 old_ms = 0;

        if(running) {/*DebugOut.Analog[]++;*/ return(1);};

        running = 1;

        if(CountMilliseconds != old_ms)  // 1 ms

        {

        if(!t20ms--) { CalcNickServoValue(); t20ms = 20; };

                // Trigger internal Portpin or Flag -> will set the Hardware Triggeroutput on the GimbalCtrl

                if(((UART_VersionInfo.HWMajor >= 30) && TRIGGER_PP_INTERN) || ((UART_VersionInfo.HWMajor < 30) && (FC.StatusFlags2 & FC_STATUS2_OUT1_ACTIVE))) ToGimbalCtrl.BitCmd |= GIMBAL_CMD_OUT_TRIGGER;

                if(UART1_BaudrateFallbackTimeout)

                 {

                  if(--UART1_BaudrateFallbackTimeout == 0) UART1_Configure(Uart1Baudrate);

                 }

                if(CanbusTimeOut >= 2) CanbusTimeOut--;

                if(I2C0_Timeout)

                 {

                  I2C0_Timeout--;

                  if((I2C0_Timeout % 5000) == 0) I2CBus_StateReset(I2C0); // reset the I2C-Bus every 5 seconds

                 }

                if(LaserCtrlTimeout)

                 {

                  LaserCtrlTimeout--;

                 }

                 else

                 {

                   FromLaserCtrl.Distance = 0;

                   FromLaserCtrl.LaserStatus = 0;

                 };

                if(GimbalCtrlTimeout)

                 {

                  GimbalCtrlTimeout--;

                 }

                else

                 {

                   FromGimbalCtrl.Nick = -1;

                   FromGimbalCtrl.Roll = -1;

                   FromGimbalCtrl.Yaw = -1;

                   FromGimbalCtrl.GimbalStatus = 0;

                 }

                if(CamCtrlTimeout)

                 {

                  if(--CamCtrlTimeout == 1) CamCtrlCharacter = '?';

                  else if(CamCtrlTimeout == 20000) CamCtrlTimeout = 0; // this is used during NC startup-Time phase

                 }

                else

                 {

                  if(TrigLogging.CountExternal) CamCtrlCharacter = TrigLogging.CountExternal % 10 + '0';

                  FromCamCtrl.CamStatus = 0;

                 };

                old_ms = CountMilliseconds;

                Compass_Update();               // update compass communication

                Analog_Update();                // get new ADC values

                CalcHeadFree();

                if(CamCtrlTimeout > 1) CamCtrl_GetData(3);

                if(LaserCtrlTimeout > 1) LaserCtrl_GetData(3);

                if(GimbalCtrlTimeout > 1) GimbalCtrl_GetData(3);

                if(UART_VersionInfo.HWMajor >= 30) ProcessCanBus();

                if(!CheckDelay(SPI0_Timeout)) TimeoutGPS_Process = 0;

                else if(CountMilliseconds - SPI0_Timeout > 30000000L) SPI0_Timeout = CountMilliseconds; // avoid too long overflows

                if(++TimeoutGPS_Process >= 25)

                 {

                  GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected

                  TimeoutGPS_Process = 0;

                 }

        if(UART_VersionInfo.HWMajor >= 30)

         {

      switch(IO1_Function)

          {

           case IO1FUNC_PARACHUTE: // parachute

                        if(IO1_INPUT == 0)

                                  {

                                   if(ToFC_Parachute_Off < 2000) ToFC_Parachute_Off++;

                                  }

                                else ToFC_Parachute_Off = 0;

                                break;

           default:     ToFC_Parachute_Off = 0;

                                break; // no function

          }

         }

        }

        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

        if(!(FC.StatusFlags & FC_STATUS_MOTOR_RUN)) TimeSinceMotorStart = 0;

        if(HugeBlockFromFC.WhoAmI == 1) // contains eeprom-Data

         {

      u8 crc1, crc2;

//        static u8 first_time = 1;

          crc1 = HugeBlockFromFC.Data[sizeof(EE_Parameter)-1];                                                  // contains the EE_Parameter.crc

          crc2 = RAM_Checksum((u8 *)(&HugeBlockFromFC.Data), sizeof(EE_Parameter)-1);   // calculates the current crc

          if(crc1 == crc2)

           {

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

// + get EE_Parameters from FC

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

                memcpy(&EE_Parameter, (u8 *) HugeBlockFromFC.Data, sizeof(EE_Parameter));

                RequestConfigFromFC = 0;

                if(UART_VersionInfo.HWMajor >= 30) SendOemName = 1;

//              if(first_time)

                 {

                        WP_MissionParameter.ComingHomeAltitude = EE_Parameter.ComingHomeAltitude;

                        WP_MissionParameter.FailsafeAltitude = EE_Parameter.FailSafeAltitude;

                        WP_MissionParameter.TriggerMask = EE_Parameter.J16Bitmask;

                 }

           }

                HugeBlockFromFC.WhoAmI = 0;

         }

    if(FCCalibActive)

            {

             count5sec = 0;

             FreqNewGpsData = 50;

             CountNewGpsDataIn5Sec = 25;

             CountGpsProcessedIn5Sec = 0;

             TimerCheckError = SetDelay(1000);

                 NickServoValue = 128 * 64; //  Middle position

            }

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

// + Baro Temperature offset

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

        if(!BaroCalState && LuftdruckTemperaturKompensation)

         {

           LuftdruckTemperaturOffset = (31 * (s32)LuftdruckTemperaturOffset + ((FC_Temperatur_raw - FC_StartTemperature*10) * LuftdruckTemperaturKompensation) / 100) / 32;

         }

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

// + one second timer

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

        if(CheckDelay(TimerSecond)) // one second Timer 

        {

         TimerSecond += 1000;

         if(MenuBlinkBit) MenuBlinkBit = 0; else MenuBlinkBit = 1;

         if(FC.StatusFlags & FC_STATUS_FLY)

                 {

                  NaviData.FlyingTime++; // we want to count the battery-time

                  TimeSinceMotorStart++;

                 }

    if((Parameter.ExtraConfig & CFG_FLYZONE_REQUIRED) && FlyzonePointCnt < 3 && FC_is_Calibrated) ShowNoFlyzoneErrorMessage++; else ShowNoFlyzoneErrorMessage = 0;

 if(ShowCalibrationErrorMessage) ShowCalibrationErrorMessage--;

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

// + FC 3.0 Temperature

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

     if(UART_VersionInfo.HWMajor >= 30)

      {

        if(FC_Temperatur == -1000 && FC_Temperatur_raw != -10000)  // init

                  {

                   FC_Temperatur = FC_Temperatur_raw/10;

                   FC_StartTemperature = FC_Temperatur;

              }

            else

            if(FC_Temperatur > FC_Temperatur_raw/10) FC_Temperatur--;

            else

            if(FC_Temperatur < FC_Temperatur_raw/10) FC_Temperatur++;

          }

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

// + Re-Init GPS-Module?

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

        if((Parameter.GlobalConfig & FC_CFG_GPS_AKTIV) && (UART0_Muxer != UART0_MK3MAG))

        {

         if(TryAgain_UBX_Setup)

          {

            if(--TryAgain_UBX_Setup == 0) UBX_Setup();

          }

          else

          if(CheckDelay(UBX_Timeout)) // no GPS communication

          {

                   GPS_Version = 0;

                   NCFlags &= ~NC_FLAG_GPS_OK;

                   TryAgain_UBX_Setup = 6; // Re-init GPS-Receiver in x seconds

          }

    }

  }

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

        // ---------------- Error Check Timing ----------------------------

        if(CheckDelay(TimerCheckError) || (FC.StatusFlags & FC_STATUS_START && !(oldFcFlags & FC_STATUS_START))) // Timer or FY wants to start

        {

                if(CheckDelay(TimerCheckError))

                {

                        if(!(FC.StatusFlags & FC_STATUS_FLY)) // do not change the Bit during flight

                         {

                          if(!BLITZ_CONNECTED) BlitzSchuhConnected = 1;