WebSVN - NaviCtrl - Diff - Rev 488 and 489

 /*#######################################################################################*/
 /* !!! 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 "i2c0.h"
-#include "i2c1.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 "ncmag.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.%02d%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((I2C_CompassPort == I2C_INTERN_1 && CheckDelay(I2C1_Timeout)) || (I2C_CompassPort == I2C_EXTERN_0 && CheckDelay(I2C0_Timeout)) || (Compass_Heading < 0))
-         DebugOut.StatusRed |= AMPEL_COMPASS;
+        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(I2C_CompassPort == I2C_EXTERN_0) LED_RED_OFF_T;
-        if(CheckDelay(I2C1_Timeout) && (I2C_CompassPort == I2C_INTERN_1))
+        if(CheckDelay(I2CBus(Compass_I2CPort)->Timeout))
+        {
                 LED_RED_ON;
-                sprintf(ErrorMSG,"no compass communica");
-                //Reset I2CBus
+                sprintf(ErrorMSG,"no compass communica");
-                I2C1_Deinit();
+                //Reset Compass communication
-                I2C1_Init();
+                Compass_Init();
                 newErrorCode = 4;
                 StopNavigation = 1;
                 UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX;
                 DebugOut.StatusRed |= AMPEL_COMPASS;
+        }
-        else if(CheckDelay(I2C0_Timeout) && (I2C_CompassPort == I2C_EXTERN_0))
-        {
-                LED_RED_ON;
-//LED_RED_ON_T;
-                sprintf(ErrorMSG,"no ext. compass ");
-                //Reset I2CBus
-                I2C0_Deinit();
-                I2C0_Init();
-                NCMAG_Update(1);
-                newErrorCode = 33;
-                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(I2C_CompassPort == I2C_INTERN_1)
-                {
-                 I2C1_Deinit();
-                 I2C1_Init();
+                Compass_Init();
-                }
-                else
-                {
-                 I2C0_Deinit();
-                 I2C0_Init();
-                }
+        }
-        else
-    if((FCCalibActive || CompassCalState) && FC_Version.Hardware)
+        else if((FCCalibActive || CompassCalState) && FC_Version.Hardware)
-     {
+        {
                 sprintf(ErrorMSG,"Calibrate... ");
                 newErrorCode = 0;
                 ErrorCode = 0;
                 no_error_delay = 1;
-         }
+        }
-        else
-        if(CheckDelay(SPI0_Timeout))
+        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(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");
+                        sprintf(ErrorMSG,"no GPS communication");
-                  UART_VersionInfo.HardwareError[0] |= NC_ERROR0_GPS_RX;
+                        UART_VersionInfo.HardwareError[0] |= NC_ERROR0_GPS_RX;
-                  UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_GPS_PRESENT;
+                        UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_GPS_PRESENT;
-              newErrorCode = 5;
+                        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;
+                LED_RED_ON;
-         sprintf(ErrorMSG,"GPS Fix lost    ");
+                sprintf(ErrorMSG,"GPS Fix lost    ");
-         newErrorCode = 21;
+                newErrorCode = 21;
+        }
         else if(ErrorDisturbedEarthMagnetField)
+        {
-         LED_RED_ON;
+                LED_RED_ON;
-         sprintf(ErrorMSG,"Magnet error    ");
+                sprintf(ErrorMSG,"Magnet error    ");
-         newErrorCode = 22;
+                newErrorCode = 22;
-         DebugOut.StatusRed |= AMPEL_COMPASS | AMPEL_NC;
+                DebugOut.StatusRed |= AMPEL_COMPASS | AMPEL_NC;
-         UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
+                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
+        }
         else if(((BL_MinOfMaxPWM == 40 && (FC.StatusFlags & FC_STATUS_FLY)) || BL_MinOfMaxPWM == 39) && !ErrorCode)
+        {
-         LED_RED_ON;
+                LED_RED_ON;
-         sprintf(ErrorMSG,"ERR:Motor restart  ");
+                sprintf(ErrorMSG,"ERR:Motor restart  ");
-         newErrorCode = 23;
+                newErrorCode = 23;
-         DebugOut.StatusRed |= AMPEL_BL;
+                DebugOut.StatusRed |= AMPEL_BL;
+        }
         else if(BL_MinOfMaxPWM < 30 && !ErrorCode)
+        {
-     unsigned int i;
+                u16 i;
-         for(i = 0; i < 12; i++) if(Motor[i].MaxPWM == BL_MinOfMaxPWM) break;
+                for(i = 0; i < 12; i++) if(Motor[i].MaxPWM == BL_MinOfMaxPWM) break;
-         LED_RED_ON;
+                LED_RED_ON;
-         sprintf(ErrorMSG,"ERR:BL%2d Test:%2d ",i+1,BL_MinOfMaxPWM);
+                sprintf(ErrorMSG,"ERR:BL%2d Test:%2d ",i+1,BL_MinOfMaxPWM);
-         newErrorCode = 32;
+                newErrorCode = 32;
-         DebugOut.StatusRed |= AMPEL_BL;
+                DebugOut.StatusRed |= AMPEL_BL;
+        }
         else if(BL_MinOfMaxPWM < 248 && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)
+        {
-         LED_RED_ON;
+                LED_RED_ON;
-         sprintf(ErrorMSG,"ERR:BL Limitation   ");
+                sprintf(ErrorMSG,"ERR:BL Limitation   ");
-         newErrorCode = 24;
+                newErrorCode = 24;
-         DebugOut.StatusRed |= AMPEL_BL;
+                DebugOut.StatusRed |= AMPEL_BL;
+        }
         else if(NCFlags & NC_FLAG_RANGE_LIMIT && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)
+        {
-         LED_RED_ON;
+                LED_RED_ON;
-         sprintf(ErrorMSG,"ERR:GPS range  ");
+                sprintf(ErrorMSG,"ERR:GPS range  ");
-         newErrorCode = 25;
+                newErrorCode = 25;
-         DebugOut.StatusRed |= AMPEL_NC;
+                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;
+                LED_RED_ON;
-         sprintf(ErrorMSG,"ERR:No SD-Card  ");
+                sprintf(ErrorMSG,"ERR:No SD-Card  ");
-         newErrorCode = 26;
+                newErrorCode = 26;
-         DebugOut.StatusRed |= AMPEL_NC;
+                DebugOut.StatusRed |= AMPEL_NC;
+        }
         else if((SD_LoggingError || (SD_WatchDog < 2000 && SD_WatchDog != 0)) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START)
+        {
-         LED_RED_ON;
+                LED_RED_ON;
-         sprintf(ErrorMSG,"ERR:SD Logging abort");
+                sprintf(ErrorMSG,"ERR:SD Logging abort");
-         newErrorCode = 27;
+                newErrorCode = 27;
-         DebugOut.StatusRed |= AMPEL_NC;
+                DebugOut.StatusRed |= AMPEL_NC;
-         SD_LoggingError = 0;
+                SD_LoggingError = 0;
+        }
         else if(((AbsoluteFlyingAltitude) && (NaviData.Altimeter / 20 >= AbsoluteFlyingAltitude)) && (FC.StatusFlags & FC_STATUS_FLY))
+        {
-         LED_RED_ON;
+                LED_RED_ON;
-         sprintf(ErrorMSG,"ERR:Max Altitude ");
+                sprintf(ErrorMSG,"ERR:Max Altitude ");
-         newErrorCode = 29;
+                newErrorCode = 29;
-         DebugOut.StatusRed |= AMPEL_NC;
+                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;
+                LED_RED_ON;
-         sprintf(ErrorMSG,"No GPS Fix      ");
+                sprintf(ErrorMSG,"No GPS Fix      ");
-         newErrorCode = 30;
+                newErrorCode = 30;
+        }
         else // no error occured
+        {
                 StopNavigation = 0;
                 LED_RED_OFF;
                 if(no_error_delay) { no_error_delay--;  }
                 else
-                 {
+                {
-                  sprintf(ErrorMSG,"No Error            ");
+                        sprintf(ErrorMSG,"No Error            ");
-                  ErrorCode = 0;
+                        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;
+}
 u8 Polling(void)
+{
- static u8 running = 0, oldFcFlags = 0, count5sec;
+        static u8 running = 0, oldFcFlags = 0, count5sec;
- static u32 old_ms = 0;
+        static u32 old_ms = 0;
- if(running) {/*DebugOut.Analog[18]++;*/ return(1);};
+        if(running) {/*DebugOut.Analog[18]++;*/ return(1);};
- running = 1;
+        running = 1;
-   if(CountMilliseconds != old_ms)  // 1 ms
+        if(CountMilliseconds != old_ms)  // 1 ms
-    {
+        {
                 old_ms = CountMilliseconds;
                 Compass_Update();               // update compass communication
                 Analog_Update();                // get new ADC values
                 CalcHeadFree();
+        }
-                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
-                // ---------------- Error Check Timing ----------------------------
+        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
-//DebugOut.Analog[16] = CompassValueErrorCount;
+        // ---------------- Error Check Timing ----------------------------
+        if(CheckDelay(TimerCheckError) || (FC.StatusFlags & FC_STATUS_START && !(oldFcFlags & FC_STATUS_START))) // Timer or FY wants to start
+        {
-                if(CheckDelay(TimerCheckError) || (FC.StatusFlags & FC_STATUS_START && !(oldFcFlags & FC_STATUS_START))) // Timer or FY wants to start
+                if(CheckDelay(TimerCheckError))
-                {
-                        if(CheckDelay(TimerCheckError))
-                         {
+                {
-                          TimerCheckError = SetDelay(1000);
+                        TimerCheckError = SetDelay(1000);
-                          if(CompassValueErrorCount) CompassValueErrorCount--;
+                        if(CompassValueErrorCount) CompassValueErrorCount--;
-                          if(++count5sec == 5)
+                        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
-                        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;
+                }
+                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
+                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;
+        running = 0;
-if(!PollingTimeout) DebugOut.Analog[17]++;
+        if(!PollingTimeout) DebugOut.Analog[17]++;
- return(0);
+        return(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)  // 1ms - Takt
+{
         IENABLE;
         VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq
-//      Compass_Update();               // update compass communication
+        //Compass_Update();             // update compass communication
-//      Analog_Update();                // get new ADC values
+        //Analog_Update();              // get new ADC values
         if(PollingTimeout == 0)
+        {
-        PollingTimeout = 5;
+                PollingTimeout = 5;
-//              if(Polling() == 0) DebugOut.Analog[16]++;
+                //if(Polling() == 0) DebugOut.Analog[16]++;
-        Polling();
+                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 u32 ftimer =0;
-        static u8 fstate = 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 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 busses (needs Timer 1)
-        I2C0_Init();
+        I2CBus_Init(I2C0);
-        I2C1_Init();
+        I2CBus_Init(I2C1);
         // 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");
         CompassValueErrorCount = 0;
         for (;;) // the endless main loop
+        {
                 PollingTimeout = 5;
                 Polling();
-                // ---------------- Logging  ---------------------------------------
+                // ---------------- 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]

Subversion Repositories NaviCtrl

(root)/tags/V2.06f/main.c @ 632 - Rev 488 → 489