Subversion Repositories NaviCtrl

/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 2008 Ingo Busker, Holger Buss
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
// + FOR NON COMMERCIAL USE ONLY
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Teile, die nicht eindeutig mit unserer Lizenzvereinbarung gekennzeichnet sind, unterliegen u.U. eigenen Vereinbarungen:
// + z.B. Die Funktion printf_P() unterliegt ihrer eigenen Lizenz
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// +     from this software without specific prior written permission.
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
// +     for non-commercial use (directly or indirectly)
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// +     with our written permission
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// +     clearly linked as origin
// +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
//
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// +  POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//#define MCLK96MHZ
const unsigned long _Main_Crystal = 25000;
#include <stdio.h>
#include "91x_lib.h"
#include "led.h"
#include "uart0.h"
#include "uart1.h"
#include "uart2.h"
#include "GPS.h"
#include "i2c.h"
#include "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 TimerKmlAddPointDelay;
u32 TimerCheckError;
u32 ErrorCode = 0;
u16 BeepTime;

u8 ClearMKFlags = 0;
u8 StopNavigation = 0;
Param_t Parameter;
FC_t FC;

s8 ErrorMSG[25];

//----------------------------------------------------------------------------------------------------
void SCU_Config(void)
{
        /* configure PLL and set it as master clock source */
        SCU_MCLKSourceConfig(SCU_MCLK_OSC);             // set master clock source to external oscillator clock (25MHz) before diabling the PLL
        SCU_PLLCmd(DISABLE);                                    // now disable the PLL
        #ifdef MCLK96MHZ
        SCU_BRCLKDivisorConfig(SCU_BRCLK_Div2); // set BRCLK to MCLK/2 = 48MHz
        SCU_PCLKDivisorConfig(SCU_PCLK_Div4);   // set PCLK     (APB bus clock) divisor to 4 (half Reference Clock)
        SCU_RCLKDivisorConfig(SCU_RCLK_Div2);   // set RCLK     (Reference Clock) divisor to 1 (full PPL clock)
        SCU_HCLKDivisorConfig(SCU_HCLK_Div2);   // set HCLK     (AHB bus clock) divisor to 1 (full Reference Clock)
        SCU_PLLFactorsConfig(192,25,2);                 // PLL = 96 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=2
        #else
        SCU_BRCLKDivisorConfig(SCU_BRCLK_Div1); // set BRCLK to MCLK = 48MHz
        SCU_PCLKDivisorConfig(SCU_PCLK_Div2);   // set PCLK     (APB bus clock) divisor to 2 (half Reference Clock)
        SCU_RCLKDivisorConfig(SCU_RCLK_Div1);   // set RCLK     (Reference Clock) divisor to 1 (full PPL clock)
        SCU_HCLKDivisorConfig(SCU_HCLK_Div1);   // set HCLK     (AHB bus clock) divisor to 1 (full Reference Clock)
        SCU_PLLFactorsConfig(192,25,3);                 // PLL = 48 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=3
        #endif
        SCU_PLLCmd(ENABLE);                                     // Enable PLL (is disabled by SCU_PLLFactorsConfig)
        SCU_MCLKSourceConfig(SCU_MCLK_PLL);     // set master clock source to PLL
}

//----------------------------------------------------------------------------------------------------
void GetNaviCtrlVersion(void)
{
        u8 msg[25];

        sprintf(msg,"\n\r NaviCtrl V%d.%d%c",  VERSION_MAJOR,  VERSION_MINOR, 'a'+ VERSION_PATCH);
        SerialPutString(msg);
}


//----------------------------------------------------------------------------------------------------
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)
        {
                // 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++;
                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;
                LED_RED_OFF;
        }
        CheckSPIOkay = 0;
        CheckGPSOkay = 0;
        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;
        }
        I2C1_GetMK3MagVersion();
        if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
        {
                SerialPutString("\n\r MK3Mag not compatible");
                LED_RED_ON;
        }

        /*
        ReadSetting(1);
        */
        SerialPutString("\r\n---------------------------------------------\r\n\r\n");

        for (;;)
        {
                if(rxd_buffer_locked) // new incomming command
                {
                         UART1_ProcessRxData();         // process request
                }
                UART1_TransmitTxData();         // send answer

                SPI0_UpdateBuffer();
                UART1_Transmit(); // empty txd buffer

                // ------------------------- I2C Timing --------------------------------
                if(I2C_State != I2C_OFF)
                {
                        if (CheckDelay(TimerCompassUpdate))
                        {
                                // check for hanging I2C bus
                                if(CheckDelay(I2C1_Timeout))
                                {       // reset I2C
                                        SerialPutString("\n\rCommunication-Error: MK3Mag\n\r");
                                        I2C1_Deinit();
                                        I2C1_Init();
                                }
                                else
                                {       // check for incomming compass calibration request
                                        // update CalByte from spi input queue
                                        fifo_get(&CompassCalcStateFiFo, (u8 *)&(I2C_WriteCal.CalByte));
                                        // send new calstate
                                        if(I2C_ReadCal.CalByte != I2C_WriteCal.CalByte)
                                        {
                                                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();
                        TimerCheckError = SetDelay(1000);
                        Request_SendFollowMe = TRUE;
                }
                // ---------------- 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
                                                        }
                                                }
                                                // 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;

                                        default:
                                                break;
                                }
                        } // EOF model is flying
                        else // model is not flying
                        {   // close log file if opened
                                if(logfilestate ==  LOGFILE_OPENED)
                                {
                                        if(KML_DocumentClose(&gpslogfile))
                                        {
                                                SerialPutString("\r\nClosing kml-file\r\n");
                                                logfilestate = LOGFILE_CLOSED;
                                        }
                                        else
                                        {
                                                logfilestate =  LOGFILE_ERROR;
                                        }
                                }
                        } //EOF model is not flying
                }
        }
}