WebSVN - NaviCtrl - Diff - Rev 39 and 41

 // + Copyright (c) 2008 Ingo Busker, Holger Buss
 // + Nur für den privaten Gebrauch
 // + FOR NON COMMERCIAL USE ONLY
 // + www.MikroKopter.com
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
 // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
 // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
 // + bzgl. der Nutzungsbedingungen aufzunehmen.
 // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
 // + Verkauf von Luftbildaufnahmen, usw.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
 // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
 // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
 // + eindeutig als Ursprung verlinkt werden
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
 // + Benutzung auf eigene Gefahr
 // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Die PORTIERUNG der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
 // + mit unserer Zustimmung zulässig
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Teile, die nicht eindeutig mit unserer Lizenzvereinbarung gekennzeichnet sind, unterliegen u.U. eigenen Vereinbarungen:
 // + z.B. Die Funktion printf_P() unterliegt ihrer eigenen Lizenz
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Redistributions of source code (with or without modifications) must retain the above copyright notice,
 // + this list of conditions and the following disclaimer.
 // +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
 // +     from this software without specific prior written permission.
 // +   * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
 // +     for non-commercial use (directly or indirectly)
 // +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
 // +     with our written permission
 // +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
 // +     clearly linked as origin
 // +   * PORTING this software (or part of it) to systems (other than hardware from www.mikrokopter.de) is NOT allowed
 //
 // +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 // +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 // +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 // +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 // +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 // +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 // +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 // +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // +  POSSIBILITY OF SUCH DAMAGE.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+//#define MCLK96MHZ
 const unsigned long _Main_Crystal = 25000;
+#include <stdio.h>
+#include "91x_lib.h"
+#include "led.h"
-#include "main.h"
+#include "uart0.h"
+#include "uart1.h"
+#include "uart2.h"
+#include "GPS.h"
+#include "i2c.h"
+#include "timer.h"
+#include "spi_slave.h"
+#include "fat16.h"
+#include "usb.h"
+#include "sdc.h"
 #include "kml.h"
+#include "main.h"
+typedef enum
+{
+        LOGFILE_IDLE,
+        LOGFILE_CLOSED,
+        LOGFILE_OPENED,
+        LOGFILE_ERROR,
+} logfilestate_t;
 u32 TimerCompassUpdate;
-u32 TimerI2CReadDelay;
+u32 TimerKmlAddPointDelay;
-u32 TimerDebugDataDelay;
+u32 TimerCheckError;
+u32 ErrorCode = 0;
-u32 KmlAddPointDelay;
+u16 BeepTime;
-u8 BeepTime;
-void Init_Undef(void);
-u8 Parameter_UserParam1;
-u8 Parameter_UserParam2;
-u8 Parameter_UserParam3;
-u8 Parameter_UserParam4;
-u8 Parameter_UserParam5;
-u8 Parameter_UserParam6;
+u8 ClearMKFlags = 0;
-u8 Parameter_UserParam7;
+u8 StopNavigation = 0;
-u8 Parameter_UserParam8;
-//++++++++++++++++++++++++++++++++++++++++++++++++++
-s32 FC_StickNick;
-s32 FC_StickRoll;
-s32 FC_StickGier;
-s32 FC_StickGas;
-s32 FC_Poti1;
-s32 FC_Poti2;
-s32 FC_Poti3;
+Param_t Parameter;
-s32 FC_Poti4;
-//++++++++++++++++++++++++++++++++++++++++++++++++++
-s32 SenderOkay = 0;
+FC_t FC;
-s32 UBat = 0;
-s32 UndervoltageLevel = 0;
-s32 ActiveSetting = 1;
-//++++++++++++++++++++++++++++++++++++++++++++++++++
-s32 Parameter_NaviGpsModeControl = 128;
-s32 Parameter_NaviGpsGain = 0;
-s32 Parameter_NaviGpsP = 90;
-s32 Parameter_NaviGpsI = 0;
-s32 Parameter_NaviGpsD = 90;
-s32 Parameter_NaviGpsACC = 0;
-s32 Parameter_NaviGpsMinSat = 3;
-s32 Parameter_NaviStickThreshold = 8;
-s32 Parameter_RadiusAlert = 15;
-//++++++++++++++++++++++++++++++++++++++++++++++++++
-#define FLAG_MOTOR_RUN  1
-#define FLAG_FLY        2
-#define FLAG_CALIBRATE  4
-#define FLAG_START      8
-u32 MikroKopterFlags;
-u32 ClearFlags = 1;
-//++++++++++++++++++++++++++++++++++++++++++++++++++
-u8 text[20];
+s8 ErrorMSG[25];
 //----------------------------------------------------------------------------------------------------
-void Leds_Init(void)
+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)
-{
-    GPIO_InitTypeDef GPIO_InitStructure;
-        SCU_APBPeriphClockConfig(__GPIO6, ENABLE);
-    SCU_APBPeriphClockConfig(__GPIO5, ENABLE);
+        SCU_MCLKSourceConfig(SCU_MCLK_PLL);     // set master clock source to PLL
-        //GPIO_DeInit(GPIO5);
-    GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
-    GPIO_InitStructure.GPIO_Pin =               GPIO_Pin_6 | GPIO_Pin_7;
+}
-    GPIO_InitStructure.GPIO_Type =              GPIO_Type_PushPull ;
-    GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt1  ;
+//----------------------------------------------------------------------------------------------------
-    GPIO_Init(GPIO5, &GPIO_InitStructure);
-        //SD_SWITCH
-    GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
-    GPIO_InitStructure.GPIO_Pin =               GPIO_Pin_3;
+void GetNaviCtrlVersion(void)
+{
-    GPIO_InitStructure.GPIO_Type =              GPIO_Type_PushPull;
+        u8 msg[25];
-    GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1;
+        sprintf(msg,"\n\r NaviCtrl V%d.%d%c",  VERSION_MAJOR,  VERSION_MINOR, 'a'+ VERSION_PATCH);
+        SerialPutString(msg);
-    GPIO_Init(GPIO5, &GPIO_InitStructure);
+}
+//----------------------------------------------------------------------------------------------------
+s8* GenerateGPSLogFileName(void)
-}
+{
+        static u16 filenum = 0; // file name counter
+        static s8 filename[30];
-  //
+        static DateTime_t LastTime = {0,0,0,0,0,0,0,0};
+        if(SystemTime.Valid)
-void OutputStartupData(void)
+        {
+                // if the day has been changed
+                if((LastTime.Year != SystemTime.Year) || (LastTime.Month != SystemTime.Month) || (LastTime.Day != SystemTime.Day))
+                {
+                        LastTime.Year = SystemTime.Year;
-{
+                        LastTime.Month = SystemTime.Month;
+                        LastTime.Day = SystemTime.Day;
+                        LastTime.Valid = 1;
+                        filenum = 0; // reset file counter
+                }
+                sprintf(filename, "LOG/%04i%02i%02i/GPS%05i.KML", SystemTime.Year, SystemTime.Month, SystemTime.Day, filenum);
+                filenum++;
-   char tex[20];
+                return filename;
+        }
+        else return NULL;
+}
+//----------------------------------------------------------------------------------------------------
+void CheckErrors(void)
+{
+        if(!CheckSPIOkay)
+        {
+                LED_RED_ON;
+                sprintf(ErrorMSG,"no FC communication ");
+                ErrorCode = 3;
+                StopNavigation = 1;
+        }
+        else if(!CheckI2COkay)
+        {
+                LED_RED_ON;
+                sprintf(ErrorMSG,"no MK3Mag communication ");
+                ErrorCode = 4;
+                StopNavigation = 1;
+        }
+        else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
+        {
+                LED_RED_ON;
+                sprintf(ErrorMSG,"FC not compatible ");
+                ErrorCode = 1;
+                StopNavigation = 1;
+        }
+        else if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
+        {
+                sprintf(ErrorMSG,"MK3Mag not compatible ");
+                LED_RED_ON;
+                ErrorCode = 2;
+                StopNavigation = 1;
+        }
+        else if(!CheckGPSOkay)
+        {
+                LED_RED_ON;
+                sprintf(ErrorMSG,"no GPS communication ");
+                ErrorCode = 5;
+                StopNavigation = 1;
+        }
+        else if(I2C_Heading.Heading < 0)
+        {
+                LED_RED_ON;
+                sprintf(ErrorMSG,"bad compass value ");
+                ErrorCode = 6;
+                StopNavigation = 1;
+        }
+        else if(FC.RC_Quality < 100)
+        {
+                LED_RED_ON;
+                sprintf(ErrorMSG,"RC Signal lost ");
+                ErrorCode = 7;
+        }
+        else // no error occured
+        {
+                sprintf(ErrorMSG,"No Error               ");
+                ErrorCode = 0;
+                StopNavigation = 0;
-   SerialPutString("\n\r--------------\n\r");
+                LED_RED_OFF;
+        }
+        CheckSPIOkay = 0;
         CheckGPSOkay = 0;
-   sprintf(text,"NaviCtrl V%d.%d\n\r",  VersionInfo.Hauptversion,  VersionInfo.Nebenversion); SerialPutString(text);
+        CheckI2COkay = 0;
+}
+//----------------------------------------------------------------------------------------------------
+int main(void)
+{
+        logfilestate_t logfilestate = LOGFILE_IDLE;
+        KML_Document_t gpslogfile;
+        s8* gpslogfilename = NULL;
+        /* Configure the system clocks */
+        SCU_Config();
+        KML_DocumentInit(&gpslogfile);                  // Initialize the new kml-document for further use.
+        /* 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.
+        // initialize the interrupt handler
+        Interrupt_Init();
+        // initialize timer 1
+        TIMER1_Init();
+        // initialize the LEDs (needs Timer 1)
+        Led_Init();
+        // initialize the debug UART1
+        UART1_Init();
+        SerialPutString("\r\n---------------------------------------------");
+        // initialize UART2 to FLIGHTCTRL
+        UART2_Init();
+        // initialize UART0 (to MKGPS or MK3MAG)
+        UART0_Init();
+        // initialize usb
+        USB_ConfigInit();
+        // initialize SPI0 to FC
+        SPI0_Init();
+        // initialize i2c bus to MK3MAG (needs Timer 1)
+        I2C1_Init();
+        // initialize the gps position controller (needs Timer 1)
+        GPS_Init();
+        // initialize fat16 partition on sd card (needs Timer 1)
-}
+        Fat16_Init();
+        TimerCompassUpdate = SetDelay(5);
+        TimerKmlAddPointDelay = SetDelay(250);
+        TimerCheckError = SetDelay(3000);
+        SerialPutString("\r\n---------------------------------------------");
+        SerialPutString("\n\r Version information:");
+        GetNaviCtrlVersion();
+        SPI0_GetFlightCtrlVersion();
+        if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
+        {
+                //sprintf(ErrorMSG,"\r\n %d <-> %d", FlightCtrlVersion.NaviCompatible, FC_SPI_COMPATIBLE);
+                //SerialPutString(ErrorMSG);
+                SerialPutString("\n\r Flight-Ctrl not compatible");
+                LED_RED_ON;
+        }
-//----------------------------------------------------------------------------------------------------
-int main(void)
-{
-   static u8 oldCompassCalState = 0;
+        I2C1_GetMK3MagVersion();
-   static u8 kml_state = 0;
-   u8 kml_cnt = 0;
-   KML_Document_t       mydocument;
-   File *file1;
-   u8 str[80];
-   KML_DocumentInit(&mydocument);                                       // Initialise the new kml-document for further use.
-  // SCU_MCLKSourceConfig(SCU_MCLK_OSC);
-  // SCU_PCLKDivisorConfig(SCU_PCLK_Div2);
-  // SCU_MCLKSourceConfig(SCU_MCLK_PLL);
-   SCU_MCLKSourceConfig(SCU_MCLK_OSC);
-   SCU_PLLCmd(DISABLE); // PLL aus
-   SCU_RCLKDivisorConfig(SCU_RCLK_Div1);
-   SCU_HCLKDivisorConfig(SCU_HCLK_Div1);
-   SCU_PCLKDivisorConfig(SCU_PCLK_Div2);
-  // SCU_FMICLKDivisorConfig(SCU_FMICLK_Div1);
-  // FMI_Config(FMI_READ_WAIT_STATE_3,FMI_WRITE_WAIT_STATE_0, FMI_PWD_ENABLE, FMI_LVD_ENABLE,FMI_FREQ_HIGH);
-   SCU_PLLFactorsConfig(192,25,3); // PLL = 48 MHz
-   SCU_PLLCmd(ENABLE); // PLL Enabled
-   SCU_MCLKSourceConfig(SCU_MCLK_PLL); // MCLK = PLL
-   VersionInfo.Hauptversion = VERSION_HAUPTVERSION;
-   VersionInfo.Nebenversion = VERSION_NEBENVERSION;
-   VersionInfo.PCKompatibel = VERSION_KOMPATIBEL;
+        if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
-   SCU_AHBPeriphClockConfig(__VIC,ENABLE);
-   SCU_AHBPeriphReset(__VIC,DISABLE);
-   VIC_DeInit();
-   InitInterrupt();
-   Leds_Init();
-   Debug_UART1_Init();
-   OutputStartupData();
-   GPS_UART0_Init();
-   USB_ConfigInit();
-   SPI0_Init();
-   TIMER1_Init();
-   I2C1_Init();
-   I2C_Version.Hauptversion = 0xff;
-   SendI2C_Command(I2C_CMD_VERSION);
-   TimerCompassUpdate = SetDelay(50);
-   while (!CheckDelay(TimerCompassUpdate));
-   TimerCompassUpdate = SetDelay(5);
-   TimerI2CReadDelay = SetDelay(5);
+        {
-   KmlAddPointDelay = SetDelay(250);
+                SerialPutString("\n\r MK3Mag not compatible");
-   SerialPutString("\r\n---------------------------------------------");
+                LED_RED_ON;
+        }
-//   ReadSetting(1);
-   SerialPutString("\r\n---------------------------------------------\r\n\r\n");
-   InitFat16();
-   for (;;)
-   {
-    if(NeuerDatensatzEmpfangen)
-          {
-                 BearbeiteRxDaten();
-             DatenUebertragung();
-//               if (GPIO_ReadBit(GPIO5, GPIO_Pin_3)) SerialPutString("keine SD\n\r");    else SerialPutString("SD eingesteckt!\n\r");
-      }
-          UpdateSPI_Buffer();
+        /*
-          SPI_CheckSlaveSelect();
-          UART1_Transmit();
-   // ----------- I2C Timing -------------------------
-          if (CheckDelay(TimerCompassUpdate))
-          {
-                if(oldCompassCalState != CompassCalState)
-                  {
-                    oldCompassCalState = CompassCalState;
-                    TimerCompassUpdate = SetDelay(25);
-            SendI2C_Command(I2C_CMD_WRITE_CAL);
-                  }
-                  else
-                  {
-                   GPIO_ToggleBit(GPIO5, GPIO_Pin_6);
-                   GPIO_ToggleBit(GPIO5, GPIO_Pin_7);
-               TimerCompassUpdate = SetDelay(25);
-           SendI2C_Command(I2C_CMD_READ_HEADING);
         ReadSetting(1);
-           }
-          if (CheckDelay(TimerI2CReadDelay))
+        */
         SerialPutString("\r\n---------------------------------------------\r\n\r\n");
-            if (I2C_ReadRequest)
-                {
-                  I2C_ReadRequest= 0;
-                  I2C_Direction = I2C_MODE_RECEIVER;
+        for (;;)
-                  I2C_GenerateStart(I2C1, ENABLE);
-                }
+        {
-                TimerI2CReadDelay = SetDelay(1000);
-          }
+                if(rxd_buffer_locked) // new incomming command
-          if (CheckDelay(TimerDebugDataDelay))
+                {
-          {  SendGPSPosAnforderung = 1;
-         DatenUebertragung();
-             TimerDebugDataDelay = SetDelay(500);
+                         UART1_ProcessRxData();         // process request
+                }
+                UART1_TransmitTxData();         // send answer
+                SPI0_UpdateBuffer();
+                UART1_Transmit(); // empty txd buffer
+                // ------------------------- I2C Timing --------------------------------
+                if(I2C_State != I2C_OFF)
+                {
-   // ----------- KML Timing -------------------------
+                        if (CheckDelay(TimerCompassUpdate))
+                        {
+                                // check for hanging I2C bus
+                                if(CheckDelay(I2C1_Timeout))
-          if(CheckDelay(KmlAddPointDelay) && 0)
+                                {       // reset I2C
+                                        SerialPutString("\n\rCommunication-Error: MK3Mag\n\r");
+                                        I2C1_Deinit();
                                         I2C1_Init();
-                KmlAddPointDelay = SetDelay(500);
-                switch(kml_state)
-                {
+                                }
-                        case 0:
-                                if((GPS_Data.Flags & GPS_FIX) && (GPS_Data.Used_Sat >= GPS_SAT_MIN))
                                 else
-                                        if(str_Volume.state == INITIALIZED)                                                                                     // has the volume and filesystem been initialized?
+                                {       // check for incomming compass calibration request
                                         // update CalByte from spi input queue
-                                                if(KML_DocumentOpen("flight01.kml",&mydocument))                                                // create a new kml-document on the memorycard.
+                                        fifo_get(&CompassCalcStateFiFo, (u8 *)&(I2C_WriteCal.CalByte));
-                                                {
-                                                        SerialPutString("\r\nopening kml-file\r\nadding data");
+                                        // send new calstate
+                                        if(I2C_ReadCal.CalByte != I2C_WriteCal.CalByte)
                                                         kml_state = 1;
+                                                I2C1_SendCommand(I2C_CMD_WRITE_CAL);
+                                        }
+                                        else // request current heading
+                                        {
+                                                I2C1_SendCommand(I2C_CMD_READ_HEADING);
+                                        }
+                                        TimerCompassUpdate = SetDelay(25);    // every 25 ms
-                                                }
+                                }
+                        }
+                }
+                // ---------------- Error Check Timing ----------------------------
+                if(CheckDelay(TimerCheckError))
+                {
+                        if(!CheckSPIOkay) GPS_Navigation(); // process the GPS data even if the FC is not connected
-                                        }
+                        CheckErrors();
-                                        else
+                        TimerCheckError = SetDelay(1000);
+                        Request_SendFollowMe = TRUE;
-                                        {
+                }
-                                                InitFat16();
+                // ---------------- KML Timing ------------------------------------
+                if(CheckDelay(TimerKmlAddPointDelay))
+                {
+                        TimerKmlAddPointDelay = SetDelay(500); // every 500 ms
+                        if(FC.MKFlags & MKFLAG_FLY) // model is flying
+                        {
+                                switch(logfilestate)
+                                {
+                                        case LOGFILE_IDLE:
+                                        case LOGFILE_CLOSED:
+                                                // find unused logfile name
+                                                do
+                                                {        // try to generate a new logfile name
+                                                         gpslogfilename = GenerateGPSLogFileName();
+                                                }while((gpslogfilename != NULL) && fexist_(gpslogfilename));
+                                                // if logfilename exist
+                                                if(gpslogfilename != NULL)
+                                                {
+                                                        // try to create the log file
+                                                        if(KML_DocumentOpen(gpslogfilename, &gpslogfile))
+                                                        {
+                                                                logfilestate = LOGFILE_OPENED; // goto next step
+                                                                SerialPutString("\r\nOpening kml-file: ");
+                                                                SerialPutString(gpslogfilename);
+                                                                SerialPutString("\r\n");
+                                                        }
+                                                        else
+                                                        {
+                                                                logfilestate = LOGFILE_ERROR;
+                                                                SerialPutString("\r\nError opening kml-file: ");
+                                                                SerialPutString(gpslogfilename);
+                                                                SerialPutString("\r\n");
+                                                                TimerKmlAddPointDelay = SetDelay(10);  // try again in open logfile in 10 mili sec
+                                                        }
+                                                }
-                                                if(str_Volume.state != INITIALIZED) KmlAddPointDelay = SetDelay(5000);  // try again in 5 sek
+                                                // else retry in next loop
+                                                break;
+                                        case LOGFILE_OPENED:
+                                                // append new gps log data
+                                                if((GPSData.Status != INVALID) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.SatFix == SATFIX_3D))
+                                                {
+                                                        if(!KML_LoggGPSCoordinates(&(GPSData.Position), &gpslogfile))
+                                                        {       // error logging data
+                                                                SerialPutString("\r\nError Logging GPS data\r\n");
+                                                                KML_DocumentClose(&gpslogfile);
+                                                                logfilestate = LOGFILE_ERROR;
+                                                        }
+                                                }
+                                                break;
+                                        case LOGFILE_ERROR:
+                                                // try to reinitialize the fat16 filesystem
+                                                if(0 == Fat16_Init())
+                                                {
+                                                        logfilestate = LOGFILE_IDLE;
+                                                        TimerKmlAddPointDelay = SetDelay(10);  // try again in open logfile in 10 mili sec
+                                                }
+                                                else
+                                                {
+                                                        logfilestate = LOGFILE_ERROR;
+                                                        TimerKmlAddPointDelay = SetDelay(5000);  // try again in 5 sec
-                                }
+                                                }
+                                                break;
                         break;                                                                                                                                                          // document.state will be changed to DOC_OPENED automatic.
+                                        default:
+                                                break;
-                        case 1:                                                                                                                                                         // linestring opened, so datapoint (gps_coordinates) can be added to the document.
+                                }
+                        } // EOF model is flying
+                        else // model is not flying
-                                if(kml_cnt++ < 20)                                                                                                                              // add 10 datapoints to the document.
+                        {   // close log file if opened
+                                if(logfilestate ==  LOGFILE_OPENED)
+                                {
+                                        if(KML_DocumentClose(&gpslogfile))
+                                        {
-                                        KML_LoggGPSCoordinates(GPS_Data,&mydocument);
+                                                SerialPutString("\r\nClosing kml-file\r\n");
-                                }
-                                else                                                                                                                                                    // after the datapoints have been written to the document close the document.
-                                {
+                                                logfilestate = LOGFILE_CLOSED;
-                                        KML_DocumentClose(&mydocument);
                                         SerialPutString("\r\nfile closed");
-                                        kml_state = 2;
+                                        else
-                                }

Subversion Repositories NaviCtrl

(root)/tags/V2.14a/main.c @ 811 - Rev 39 → 41