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.

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

#include <stdio.h>

#include <stdlib.h>

#include <stdarg.h>

#include <string.h>

#include "91x_lib.h"

#include "main.h"

#include "config.h"

#include "menu.h"

#include "GPS.h"

#include "i2c.h"

#include "uart0.h"

#include "uart1.h"

#include "uart2.h"

#include "timer1.h"

#include "timer2.h"

#include "analog.h"

#include "compass.h"

#include "waypoints.h"

#include "mkprotocol.h"

#include "params.h"

#include "fifo.h"

#include "debug.h"

#include "spi_slave.h"

#include "ftphelper.h"

#include "led.h"

#include "fat16.h"

#include "eeprom.h"

#define LIC_CMD_READ_LICENSE     1

#define LIC_CMD_WRITE_LICENSE    2

#define LIC_CMD_ERASE_LICENSE    3

#define FALSE   0

#define TRUE    1

#define ABO_TIMEOUT 8000 // disable abo after 8 seconds

u32 UART1_AboTimeOut = 0;

u8 UART1_Request_VersionInfo    = FALSE;

u8 UART1_Request_ExternalControl= FALSE;

u8 UART1_Request_Display                = FALSE;

u8 UART1_Request_Display1               = FALSE;

u8 UART1_Request_DebugData              = FALSE;

u8 UART1_Request_DebugLabel             = 255;

u8 UART1_Request_NaviData               = FALSE;

u8 UART1_Request_ErrorMessage   = FALSE;

u8 UART1_Request_WritePoint             = 0xFF;

u8 UART1_Request_ReadPoint              = 0;

u8 UART1_Request_Data3D             = FALSE;

u8 UART1_Request_MotorData          = FALSE;

u8 UART1_Request_Echo               = FALSE;

u8 UART1_Request_ParameterId    = 0;

u8 UART1_Request_WPLStore               = FALSE;

u8 UART1_Request_Parameter              = FALSE;

u8 UART1_Request_SystemTime             = FALSE;

u8 UART1_DisplayKeys                    = 0;

u8 UART1_DisplayLine                    = 0;

u8 UART1_ConfirmFrame                   = 0;

u8 UART1_Request_FTP                    = FALSE;

u8 UART1_Request_LicenseString  = FALSE;

u8 LastTransmittedFCStatusFlags2 = 0;

u8 UART1_ExternalControlConfirmFrame = FALSE;

u8 Send_NMEA_RMC = FALSE;

UART_TypeDef *DebugUART = UART1;

#ifdef FOLLOW_ME

#define FOLLOW_ME_INTERVAL 200 // 5 Hz

u32 UART1_FollowMe_Timer        = 0;

Point_t FollowMe;

#endif

// the primary rx fifo

#define UART1_RX_FIFO_LEN 1024

u8 UART1_rxfifobuffer[UART1_RX_FIFO_LEN];

fifo_t UART1_rx_fifo;

// the rx buffer

#define UART1_RX_BUFFER_LEN 1024

u8 UART1_rbuffer[UART1_RX_BUFFER_LEN];

Buffer_t UART1_rx_buffer;

// the tx buffer

#define UART1_TX_BUFFER_LEN 1024

u8 UART1_tbuffer[UART1_TX_BUFFER_LEN];

Buffer_t UART1_tx_buffer;

volatile u8 SerialLinkOkay = 0;

u8 text[200];

u8 *LicensePtr = UART1_tbuffer;

const u8 ANALOG_LABEL[32][16] =

{

   //1234567890123456

        "AngleNick       ", //0

        "AngleRoll       ",

        "AccNick         ",

        "AccRoll         ",

        "Altitude [0.1m] ",

        "FC-Flags        ", //5

        "NC-Flags        ",

        "Voltage  [0.1V] ",

        "Current  [0.1A] ",

        "GPS Data        ",

        "CompassHeading  ", //10

        "GyroHeading     ",

        "SPI Error       ", // achtung: muss auf 12 bleiben

        "SPI Okay        ",

        "I2C Error       ",

        "I2C Okay        ", //15

        "16              ",

        "17              ",

        "18              ",

        "19              ", // SD-Card-time

        "EarthMagnet [%] ", //20

        "Ground Speed    ", //  "Z_Speed         ",

        "N_Speed         ",

        "E_Speed         ",

        "Magnet X        ",

        "Magnet Y        ", //25

        "Magnet Z        ",

        "Distance N      ",

        "Distance E      ",

        "-GPS_Nick       ",

        "-GPS_Roll       ", //30

        "Used_Sats       "

};

typedef struct

{

        u8 Index;

        u8 Status;

} __attribute__((packed)) WPL_Answer_t;

WPL_Answer_t WPL_Answer;

DebugOut_t DebugOut;

ExternControl_t ExternControl;

UART_VersionInfo_t UART_VersionInfo;

NaviData_t NaviData;

Data3D_t Data3D;

u16 Echo; // 2 bytes recieved will be sent back as echo

u32 UART1_DebugData_Timer = 0;

u32 UART1_DebugData_Interval = 0;       // in ms

u32 UART1_NaviData_Timer = 0;

u32 UART1_NaviData_Interval = 0;        // in ms

u32 UART1_Data3D_Timer = 0;

u32 UART1_Data3D_Interval = 0;          // in ms

u32 UART1_MotorData_Timer = 0;

u32 UART1_MotorData_Interval = 0;               // in ms

u32 UART1_Display_Timer = 0;

u32 UART1_Display_Interval = 0;         // in ms

u32 NMEA_Timer = 0;

u32 NMEA_Interval = 0;// in ms

/********************************************************/

/*            Initialization the UART1                  */

/********************************************************/

void UART1_Init (void)

{

        GPIO_InitTypeDef GPIO_InitStructure;

        UART_InitTypeDef UART_InitStructure;

        // initialize txd buffer

        Buffer_Init(&UART1_tx_buffer, UART1_tbuffer, UART1_TX_BUFFER_LEN);

        // initialize rxd buffer

        Buffer_Init(&UART1_rx_buffer, UART1_rbuffer, UART1_RX_BUFFER_LEN);

        // initialize the rx fifo, block UART IRQ geting a byte from fifo

        fifo_init(&UART1_rx_fifo, UART1_rxfifobuffer, UART1_RX_FIFO_LEN, NO_ITLine, UART1_ITLine);

        SCU_APBPeriphClockConfig(__UART1, ENABLE);  // Enable the UART1 Clock

        SCU_APBPeriphClockConfig(__GPIO3, ENABLE);  // Enable the GPIO3 Clock

        /*Configure UART1_Rx pin GPIO3.2*/

        GPIO_StructInit(&GPIO_InitStructure);

        GPIO_InitStructure.GPIO_Direction =     GPIO_PinInput;

        GPIO_InitStructure.GPIO_Pin =                   GPIO_Pin_2;

        GPIO_InitStructure.GPIO_Type =                  GPIO_Type_PushPull;

        GPIO_InitStructure.GPIO_IPInputConnected =      GPIO_IPInputConnected_Enable;

        GPIO_InitStructure.GPIO_Alternate =     GPIO_InputAlt1; // UART1_RxD

        GPIO_Init(GPIO3, &GPIO_InitStructure);

        /*Configure UART1_Tx pin GPIO3.3*/

        GPIO_StructInit(&GPIO_InitStructure);

        GPIO_InitStructure.GPIO_Direction =     GPIO_PinOutput;

        GPIO_InitStructure.GPIO_Pin =                   GPIO_Pin_3;

        GPIO_InitStructure.GPIO_Type =                  GPIO_Type_PushPull;

        GPIO_InitStructure.GPIO_Alternate =     GPIO_OutputAlt2; // UART1_TX

        GPIO_Init(GPIO3, &GPIO_InitStructure);

        /* UART1 configured as follow:

        - Word Length = 8 Bits

        - One Stop Bit

        - No parity

        - BaudRate = 57600 baud

        - Hardware flow control Disabled

        - Receive and transmit enabled

        - Receive and transmit FIFOs are Disabled

        */

        UART_StructInit(&UART_InitStructure);

        UART_InitStructure.UART_WordLength =                    UART_WordLength_8D;

        UART_InitStructure.UART_StopBits =                              UART_StopBits_1;

        UART_InitStructure.UART_Parity =                                UART_Parity_No ;

        UART_InitStructure.UART_BaudRate =                              UART1_BAUD_RATE;

        UART_InitStructure. UART_HardwareFlowControl =  UART_HardwareFlowControl_None;

        UART_InitStructure.UART_Mode =                                  UART_Mode_Tx_Rx;

        UART_InitStructure.UART_FIFO =                                  UART_FIFO_Enable;

        UART_InitStructure.UART_TxFIFOLevel =                   UART_FIFOLevel_1_2;

        UART_InitStructure.UART_RxFIFOLevel =                   UART_FIFOLevel_1_2;      // FIFO size 16 bytes, FIFO level 8 bytes

        UART_DeInit(UART1); // reset uart 1     to default

        UART_Init(UART1, &UART_InitStructure); // initialize uart 1

        // enable uart 1 interrupts selective

        UART_ITConfig(UART1, UART_IT_Receive | UART_IT_ReceiveTimeOut, ENABLE);

        UART_Cmd(UART1, ENABLE); // enable uart 1

        // configure the uart 1 interupt line

        VIC_Config(UART1_ITLine, VIC_IRQ, PRIORITY_UART1);

        // enable the uart 1 IRQ

        VIC_ITCmd(UART1_ITLine, ENABLE);

        // initialize the debug timer

        UART1_DebugData_Timer = SetDelay(UART1_DebugData_Interval);

        UART1_NaviData_Timer = SetDelay(UART1_NaviData_Interval)+500;

        NMEA_Timer = SetDelay(14000);

        // Fill Version Info Structure

        UART_VersionInfo.SWMajor = VERSION_MAJOR;

        UART_VersionInfo.SWMinor = VERSION_MINOR;

        UART_VersionInfo.SWPatch = VERSION_PATCH;

        UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;

    UART_VersionInfo.HWMajor = Version_HW & 0x7F;

    UART_VersionInfo.BL_Firmware = 255;

        UART_VersionInfo.Flags = 0;

        UART_VersionInfo.Reserved1 = 0;

        NaviData.Version = NAVIDATA_VERSION;

        UART1_PutString("\r\n UART1 init...ok");

}

/****************************************************************/

/*               USART1 receiver ISR                            */

/****************************************************************/

void UART1_IRQHandler(void)

{

        static u8 abortState = 0;

        u8 c;

        if((UART_GetITStatus(UART1, UART_IT_Receive) != RESET) || (UART_GetITStatus(UART1, UART_IT_ReceiveTimeOut) != RESET) )

        {

                // clear the pending bits!

                UART_ClearITPendingBit(UART1, UART_IT_Receive);

                UART_ClearITPendingBit(UART1, UART_IT_ReceiveTimeOut);

                // if debug UART is not UART1

                if (DebugUART != UART1)

                {       // forward received data to the debug UART tx buffer

                        while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)

                        {

                                // move the byte from the rx buffer of UART1 to the tx buffer of DebugUART

                                c = UART_ReceiveData(UART1);

                                // check for abort condition (ESC ESC 0x55 0xAA 0x00)

                                switch (abortState)

                                {

                                        case 0:

                                                if (c == 27) abortState++;

                                                break;

                                        case 1:

                                                if (c == 27) abortState++;

                                                else abortState = 0;

                                        break;

                                        case 2:

                                                if (c == 0x55) abortState++;

                                                else abortState = 0;

                                                break;

                                        case 3:

                                                if (c == 0xAA) abortState++;

                                                else abortState = 0;

                                                break;

                                        case 4:

                                                if (c == 0x00)

                                                {

                                                        if(DebugUART == UART0)

                                                        {

                                                                UART0_Connect_to_MKGPS(UART0_BAUD_RATE);

                                                                TIMER2_Init(); // enbable servo outputs

                                                                fifo_purge(&UART1_rx_fifo); // flush the whole fifo init buffer

                                                        }

                                                        DebugUART = UART1;

                                                }

                                                abortState = 0;

                                                break;

                                } // end switch abort state

                                // if the Debug uart is not UART1, redirect input to the Debug UART

                                if (DebugUART != UART1)

                                {

                                        // wait for space in the tx buffer of the DebugUART

                                        while(UART_GetFlagStatus(DebugUART, UART_FLAG_TxFIFOFull) == SET) {};

                                        // move byte to the tx fifo of the debug uart

                                        UART_SendData(DebugUART, c);

                                }

                        }

                }

                else  // DebugUART == UART1 (normal operation)

                {

                        while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)

                        { // some byes in the hardware fifo

                            // get byte from hardware fifo

                        c = UART_ReceiveData(UART1);

                                // put into the software fifo

                                if(!fifo_put(&UART1_rx_fifo, c))

                                {       // fifo overflow

                                        //fifo_purge(&UART1_rx_fifo); // flush the whole buffer

                                }

                        } // EOF while some byes in the hardware fifo

                } // eof DebugUart = UART1

        }

        VIC1->VAR = 0xFF; // write any value to VIC1 Vector address register

}

/**************************************************************/

/* Process incomming data from debug uart                     */

/**************************************************************/

void UART1_ProcessRxData(void)

{

        // return on forwarding uart  or unlocked rx buffer

        u8 c;

        if(DebugUART != UART1) return;

        // if rx buffer is not locked

        if(UART1_rx_buffer.Locked == FALSE)

        {

                //collect data from primary rx fifo

                while(fifo_get(&UART1_rx_fifo, &c))

                {

                        // break if complete frame is collected

                        if(MKProtocol_CollectSerialFrame(&UART1_rx_buffer, c)) break;

                }

        }

        if(UART1_rx_buffer.Locked == FALSE) return;

        Point_t * pPoint = NULL;

        SerialMsg_t SerialMsg;

        // analyze header first

        MKProtocol_DecodeSerialFrameHeader(&UART1_rx_buffer, &SerialMsg);

        if( SerialMsg.Address == FC_ADDRESS )

        {

                switch(SerialMsg.CmdID)

                {

//                      case 'v': // version

                        case 'b': // extern control

                                          UART1_ExternalControlConfirmFrame = 1;

                        case 'y': // serial poti values

                                Buffer_Copy(&UART1_rx_buffer, &UART2_tx_buffer); //forward to FC

                                Buffer_Clear(&UART1_rx_buffer); // free rc buffer for next frame

                                return; //end process rx data

                        break;

                }

        }

        MKProtocol_DecodeSerialFrameData(&UART1_rx_buffer, &SerialMsg); // decode serial frame in rxd buffer

    if(SerialMsg.CmdID != 'z') SerialLinkOkay = 250;      // reset SerialTimeout, but not in case of the "ping"

        switch(SerialMsg.Address) // check for Slave Address

        {

                case NC_ADDRESS:  // own Slave Address

                switch(SerialMsg.CmdID)

                {

                        case 't': // request for the GPS time

                                UART1_Request_SystemTime  = TRUE;

                                break;

                        case 'm': // request for the license string

                                UART1_Request_LicenseString = SerialMsg.pData[0];

                                if((UART1_Request_LicenseString == LIC_CMD_WRITE_LICENSE) && (UART_VersionInfo.HWMajor >= 20))

                                 {

                                  memcpy(LicensePtr, &SerialMsg.pData[1],LICENSE_SIZE_TEXT); // copy ftp parameter

                                 }

                                break;

                        case 'f': // ftp command

                                UART1_Request_FTP = SerialMsg.pData[0];

                                //if (UART1_Request_FTP == FTP_CMD_SET_CWD || UART1_Request_FTP == FTP_CMD_GET_FILE)

                                memcpy(&FTP_data, &SerialMsg.pData[1], sizeof(FTP_data)); // copy ftp parameter

                        break;

                        case 'z': // connection checker

                                memcpy(&Echo, SerialMsg.pData, sizeof(Echo)); // copy echo pattern

                                UART1_Request_Echo = TRUE;

                                break;

                        case 'e': // request for the text of the error status

                                UART1_Request_ErrorMessage = TRUE;

                                break;

                        case 's'://  new target position

                                pPoint = (Point_t*)SerialMsg.pData;

                                if(pPoint->Position.Status == NEWDATA)

                                {

                                        //if(!(FC.StatusFlags & FC_STATUS_FLY)) PointList_Clear(); // flush the list

                                        //pPoint->Index = 1; // must be one after empty list

                                        PointList_SetAt(pPoint);

                                        if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);

                                        GPS_pWaypoint = PointList_WPBegin(); // updates POI index

                                        BeepTime = 50;

                                }

                                else

                                if((pPoint->Position.Status == SIMULATION) && !(FC.RealStatusFlags & FC_STATUS_MOTOR_RUN))

                                {

                                if(pPoint->Event_Flag & SIMULATION_MOTOR_ON) GPSPos_Copy(&(pPoint->Position), &SimulationPosition); // update hold position

                                if(!(SimulationFlags & SIMULATION_MOTOR_ON) && (pPoint->Event_Flag & SIMULATION_MOTOR_ON))

                                 {

                                  SimulationFlags = pPoint->Event_Flag | SIMULATION_MOTOR_START; // dann steht da noch nicht "SIMULATION_MOTOR_ON" drin

                                  SpeakHoTT = SPEAK_STARTING;

                                 }

                                else

                                if(!(pPoint->Event_Flag & SIMULATION_MOTOR_ON) && (SimulationFlags & SIMULATION_MOTOR_ON))

                                 {

                                  SimulationFlags = pPoint->Event_Flag;

                                  SpeakHoTT = SPEAK_MK_OFF;

                                  BeepTime = 50;

                                 }

                                 else

                                  SimulationFlags = pPoint->Event_Flag | (SimulationFlags & SIMULATION_MOTOR_START);

                                }

                                break;

                        case 'u': // redirect debug uart

                                switch(SerialMsg.pData[0])

                                {

                                        case UART_FLIGHTCTRL:

                                                UART2_Init();                           // initialize UART2 to FC pins

                                                fifo_purge(&UART1_rx_fifo);

                                                TIMER2_Deinit();                        // reduce irq load

                                                DebugUART = UART2;

                                                break;

                                        case UART_MK3MAG:

                                                if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) break; // not if the motors are running

                                                UART0_Connect_to_MK3MAG();      // mux UART0 to MK3MAG pins

                                                GPSData.Status = INVALID;

                                                fifo_purge(&UART1_rx_fifo);

                                                DebugUART = UART0;

                                                break;

                                        case UART_MKGPS:

                                                if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) break; // not if the motors are running

                                                TIMER2_Deinit();                        // disable servo outputs to reduce irq load

                                                UART0_Connect_to_MKGPS(UART0_BAUD_RATE);        // connect UART0 to MKGPS pins

                                                GPSData.Status = INVALID;

                                                fifo_purge(&UART1_rx_fifo);

                                                DebugUART = UART0;

                                                break;

                                        default:

                                                break;

                                }

                                break;

                        case 'w'://  Set point in list at index

                                {

                                        pPoint = (Point_t*)SerialMsg.pData;

                                        if((pPoint->Position.Status == INVALID) && (pPoint->Index == 0))

                                        {

                                                PointList_Clear();

                                                GPS_pWaypoint = PointList_WPBegin();

                                                UART1_Request_WritePoint = 0; // return new point count

                                                NewWaypointsReceived = 1;

                                        }

                                        else

                                        {  // update WP in list at index

                                                if(pPoint->Index > MaxNumberOfWaypoints)

                                                {

                                                        UART1_Request_WritePoint = 254;

                                                        pPoint->Index = MaxNumberOfWaypoints;

                                                }

                                                else UART1_Request_WritePoint = PointList_SetAt(pPoint);

                                                if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);

                                                SpeakWaypointRached = 1;        // Speak once when the last Point is reached

                                                if(UART1_Request_WritePoint == pPoint->Index)

                                                {

                                                        BeepTime = 500;

                                                    if(UART1_Request_WritePoint == 1) SpeakNextWaypoint = 1;            // Speak once as soon as the Points are active

                                                }

                                        }

                                }

                                break;

                        case 'x'://  Read Waypoint from List

                                UART1_Request_ReadPoint = SerialMsg.pData[0];

                                break;

                        case 'i':// Store WP List to file

                                memcpy((u8*)&WPL_Store, SerialMsg.pData, sizeof(WPL_Store_t));

                                WPL_Store.Name[11] = 0; // make sure the name string is terminated

                                WPL_Answer.Index = WPL_Store.Index; // echo Index in cmd answer

                                WPL_Answer.Status = PointList_WriteToFile(&WPL_Store);

                                UART1_Request_WPLStore = TRUE;

                                break;

                        case 'j':// Set/Get NC-Parameter

                                switch(SerialMsg.pData[0])

                                {

                                        case 0: // get

                                        break;

                                        case 1: // set

                                        {

                                                s16 value;

                                                value = SerialMsg.pData[2] + (s16)SerialMsg.pData[3] * 0x0100;

                                                NCParams_SetValue(SerialMsg.pData[1], &value);

                                        }

                                        break;

                                        default:

                                        break;

                                }

                                UART1_Request_ParameterId = SerialMsg.pData[1];

                                UART1_Request_Parameter = TRUE;

                                break;

                        default:

                                // unsupported command recieved

                                break;

                } // case NC_ADDRESS

                // "break;" is missing here to fall thru to the common commands

                default:  // and any other Slave Address

                switch(SerialMsg.CmdID) // check CmdID

                {

                        case 'a':// request for the labels of the analog debug outputs

                                UART1_Request_DebugLabel = SerialMsg.pData[0];

                                if(UART1_Request_DebugLabel > 31) UART1_Request_DebugLabel = 31;

                                break;

                        /*

                        case 'b': // submit extern control

                                memcpy(&ExternControl, SerialMsg.pData, sizeof(ExternControl));

                                UART1_ConfirmFrame = ExternControl.Frame;

                                break;

                        */

                        case 'd': // request for debug data;

                                UART1_DebugData_Interval = (u32) SerialMsg.pData[0] * 10;

                                if(UART1_DebugData_Interval > 0) UART1_Request_DebugData = TRUE;

                                UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);

                                break;

                        case 'c': // request for 3D data;

                                UART1_Data3D_Interval = (u32) SerialMsg.pData[0] * 10;

                                if(UART1_Data3D_Interval > 0) UART1_Request_Data3D = TRUE;

                                UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);

                                break;

                        case 'k': // request for Motor data;

                                UART1_MotorData_Interval = (u32) SerialMsg.pData[0] * 10;

                                if(UART1_MotorData_Interval > 0) UART1_Request_MotorData = TRUE;

                                UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);

                                break;

                        case 'h':// reqest for display line

                                if((SerialMsg.pData[0]& 0x80) == 0x00)// old format

                                {

                                        UART1_DisplayLine = 2;

                                        UART1_Display_Interval = 0;

                                }

                                else

                                {

                                        UART1_DisplayKeys |= ~SerialMsg.pData[0];

                                        UART1_Display_Interval = (u32) SerialMsg.pData[1] * 10;

                                        UART1_DisplayLine = 4;

                                        UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);

                                }

                                UART1_Request_Display = TRUE;

                                break;

                        case 'l':// reqest for display columns

                                MenuItem = SerialMsg.pData[0];

                                UART1_Request_Display1 = TRUE;

                                break;

                        case 'o': // request for navigation information

                                UART1_NaviData_Interval = (u32) SerialMsg.pData[0] * 10;

                                if(UART1_NaviData_Interval > 0) UART1_Request_NaviData = TRUE;

                                UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);

                                break;

                        case 'v': // request for version info

                                if(SerialMsg.DataLen > 0 && SerialMsg.pData[0] == 1) UART1_Request_VersionInfo = 1;

                                else UART1_Request_VersionInfo = 2;

                                break;

                        default:

                                // unsupported command recieved

                                break;

                }

                break; // default:

        }

        Buffer_Clear(&UART1_rx_buffer); // free rc buffer for next frame

}

/*****************************************************/

/*                   Send a character                */

/*****************************************************/

s16 UART1_Putchar(char c)

{

        u32 timeout = 10000;

        if (c == '\n') UART1_Putchar('\r');

        // wait until txd fifo is not full

        while(UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != RESET) if(--timeout == 0) return(0);

        // transmit byte

        UART_SendData(UART1, c);

        #ifdef FOLLOW_ME

        if(TransmitAlsoToFC) UART_SendData(UART2, c);

        #endif

        return (0);

}

/*****************************************************/

/*       Send a string to the debug uart              */

/*****************************************************/

void UART1_PutString(u8 *s)

{

        if(s == NULL) return;

        while (*s != '\0' && DebugUART == UART1)

        {

                UART1_Putchar(*s);

                s ++;

        }

}

/**************************************************************/

/*         Transmit tx buffer via debug uart                  */

/**************************************************************/

void UART1_Transmit(void)

{

        u8 tmp_tx;

        if(DebugUART != UART1) return;

        // if something has to be send and the txd fifo is not full

        if(UART1_tx_buffer.Locked == TRUE)

        {

                // while there is some space in the tx fifo

                while(UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != SET)

                {

                        tmp_tx = UART1_tx_buffer.pData[UART1_tx_buffer.Position++]; // read next byte from txd buffer

                        UART_SendData(UART1, tmp_tx); // put character to txd fifo

                        #ifdef FOLLOW_ME

                        if(TransmitAlsoToFC)

                        {

                                UART_SendData(UART2, tmp_tx); // put character to txd fifo

                        }

                        #endif

                        // if terminating character or end of txd buffer reached

                        if((tmp_tx == '\0') || (UART1_tx_buffer.Position == UART1_tx_buffer.DataBytes))

                        {

                                Buffer_Clear(&UART1_tx_buffer); // clear txd buffer

                                #ifdef FOLLOW_ME

                                TransmitAlsoToFC = 0;

                                #endif

                                break; // end while loop

                        }

                }

        }

}

//$GPGGA,HHMMSS.ss,BBBB.BBBB,b,LLLLL.LLLL,l,Q,NN,D.D,H.H,h,G.G,g,A.A,RRRR*PP

//$GPGGA,191410,4735.5634,N,00739.3538,E,1,04,4.4,351.5,M,48.0,M,,*45

//$GPGGA,092120.20,,,,,0,00,99.99,,,,,,*6C

//http://www.kowoma.de/gps/zusatzerklaerungen/NMEA.htm

void CreateNmeaGGA(void)

{

        u8 array[200], i = 0, crc = 0, x;

        s32 tmp1, tmp2;

        i += sprintf(array, "$GPGGA,");

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

        if(SystemTime.Valid)

        {

                i += sprintf(&array[i], "%02d%02d%02d.%02d,", SystemTime.Hour, SystemTime.Min, SystemTime.Sec, SystemTime.mSec/10);

        }

        else

        {

                i += sprintf(&array[i], ",");

        }

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

        if(GPSData.Flags & FLAG_GPSFIXOK)

        {

                tmp1 = abs(GPSData.Position.Latitude)/10000000L;

                i += sprintf(&array[i],"%02d",(s16)tmp1);

                tmp1 = abs(GPSData.Position.Latitude)%10000000L;

                tmp1 *= 6; // in Minuten

                tmp2 = tmp1 / 1000000L;

                i += sprintf(&array[i],"%02d", (s16)tmp2);

                tmp2 = tmp1 % 1000000L;

                tmp2 /= 10; // eine Stelle zu viel

                i += sprintf(&array[i],".%05d,", (s16)tmp2);

                if(GPSData.Position.Latitude >= 0) i += sprintf(&array[i],"N,");

                else i += sprintf(&array[i],"S,");

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

                tmp1 = abs(GPSData.Position.Longitude)/10000000L;

                i += sprintf(&array[i],"%03d", (s16)tmp1);

                tmp1 = abs(GPSData.Position.Longitude)%10000000L;

                tmp1 *= 6; // in Minuten

                tmp2 = tmp1 / 1000000L;

                i += sprintf(&array[i],"%02d", (s16)tmp2);