WebSVN - NaviCtrl - Diff - Rev 95 and 96


/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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
// +  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.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <stdio.h>
#include <stdarg.h>
#include <string.h>
 
#include "91x_lib.h"
#include "ramfunc.h"
#include "menu.h"
#include "printf_P.h"
#include "GPS.h"
#include "i2c.h"
#include "uart0.h"
#include "uart1.h"
#include "uart2.h"
#include "timer.h"
#include "usb.h"
#include "main.h"
#include "waypoints.h"
#include "GPS.h"
 
// slave addresses
#define FC_ADDRESS 1
#define NC_ADDRESS 2
#define MK3MAG_ADDRESS 3
 
#define FALSE   0
#define TRUE    1
 
typedef struct
{
        u8 SWMajor;
        u8 SWMinor;
        u8 ProtoMajor;
        u8 ProtoMinor;
        u8 SWPatch;
        u8 Reserved[5];
}  __attribute__((packed)) UART_VersionInfo_t;
 
 
u8 Request_SendFollowMe         = FALSE;
u8 Request_VerInfo                      = FALSE;
u8 Request_ExternalControl      = FALSE;
u8 Request_Display                      = FALSE;
u8 Request_Display1             = FALSE;
u8 Request_DebugData            = FALSE;
u8 Request_DebugLabel           = 255;
u8 Request_ChannelOnly          = FALSE;
u8 Request_NaviData                     = FALSE;
u8 Request_ErrorMessage     = FALSE;
u8 Request_NewWaypoint          = FALSE;
u8 Request_ReadWaypoint         = 255;
u8 Request_Data3D                   = FALSE;
u8 Request_Echo                 = FALSE;
 
u8 DisplayLine = 0;
 
UART_TypeDef *DebugUART = UART1;
 
volatile u8 txd_buffer[TXD_BUFFER_LEN];
volatile u8 rxd_buffer_locked = FALSE;
volatile u8 rxd_buffer[RXD_BUFFER_LEN];
volatile u8 txd_complete = TRUE;
volatile u8 ReceivedBytes = 0;
volatile u8 CntCrcError = 0;
volatile u8 *pRxData = NULL;
volatile u8 RxDataLen = 0;
volatile u8 SerialLinkOkay = 0;
 
u8 text[100];
 
u8 PcAccess = 100;
u8 MotorTest[4] = {0,0,0,0};
u8 ConfirmFrame = 0;
 
DebugOut_t DebugOut;
ExternControl_t ExternControl;
UART_VersionInfo_t UART_VersionInfo;
NaviData_t NaviData;
Waypoint_t FollowMe;
Data3D_t Data3D;
u16 Echo; // 2 bytes recieved will be sent back as echo
 
u32 DebugData_Timer;
u32 DebugData_Interval = 5000;  // in ms
u32 NaviData_Timer;
u32 NaviData_Interval = 5000;   // in ms
u32 Data3D_Timer = 0;                   // in ms
u32 Data3D_Interval = 0;
 
static u16 ptr_txd_buffer = 0;
 
const u8 ANALOG_LABEL[32][16] =
{
   //1234567890123456
        "AngleNick       ", //0
        "AngleRoll       ",
        "AccNick         ",
        "AccRoll         ",
        "                ",
        "MK-Flags        ", //5
        "NC-Flags        ",
        "                ",
        "                ",
        "GPS Data        ",
        "CompassHeading  ", //10
        "GyroHeading     ",
        "SPI Error       ",
        "SPI Okay        ",
        "I2C Error       ",
        "I2C Okay        ", //15
        "                ",//    "FC_Kalman_K     ",
        "ACC_Speed_N     ",
        "ACC_Speed_E     ",
        "                ",//    "GPS ACC         ",
        "                ",//    "MAXDrift        ", //20
        "N_Speed         ",
        "E_Speed         ",
        "                ",//    "KalmDist_N      ",
        "                ",//    "KalmDist_E      ",
        "                ",//25
        "                ",
        "Distance N      ",
        "Distance E      ",
        "GPS_Nick        ",
        "GPS_Roll        ", //30
        "Used_Sats       "
};
 
 
/********************************************************/
/*            Initialization the UART1                  */
/********************************************************/
void UART1_Init (void)
{
        GPIO_InitTypeDef GPIO_InitStructure;
        UART_InitTypeDef UART_InitStructure;
 
        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_IPConnected =   GPIO_IPConnected_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 =                              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;
 
        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 as an IRQ with priority 4 (0 is highest)
        VIC_Config(UART1_ITLine, VIC_IRQ, 4);
        // enable the uart 1 IRQ
        VIC_ITCmd(UART1_ITLine, ENABLE);
        // initialize the debug timer
        DebugData_Timer = SetDelay(DebugData_Interval);
        NaviData_Timer = SetDelay(NaviData_Interval)+500;
        // unlock rxd_buffer
        rxd_buffer_locked = FALSE;
        pRxData = NULL;
        RxDataLen = 0;
        // no bytes to send
        txd_complete = TRUE;
        // 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.ProtoMinor = VERSION_SERIAL_MINOR;
 
        NaviData.Version = NAVIDATA_VERSION;
 
        SerialPutString("\r\nUART1 init...ok");
}
 
 
/****************************************************************/
/*               USART1 receiver ISR                            */
/****************************************************************/
void UART1_IRQHandler(void)
{
        static u16 crc;
        static u8 ptr_rxd_buffer = 0;
        static u8 crc1, crc2;
        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)
                        {
                                // wait for space in the tx buffer of the DebugUART
                                while(UART_GetFlagStatus(DebugUART, UART_FLAG_TxFIFOFull) == 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)
                                           {
                                                    DebugUART = UART1;
                                                        UART0_Connect_to_MKGPS();
                                                   }
                                          abortState = 0;
                                                break;
                                }
 
                                if (DebugUART != UART1) UART_SendData(DebugUART, c);
                        }
                }
                else  // DebugUART == UART1 (normal operation)
                {
                        while ((UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET) && (!rxd_buffer_locked))
                        { // some byes in the fifo and rxd buffer not locked
                            // get byte from fifo
                        c = UART_ReceiveData(UART1);
                                if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received
                                {
                                        rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
                                        crc = c; // init crc
                                }
                                #if 0
                                else if (ptr_rxd_buffer == 1) // handle address
                                {
                                        rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
                                        crc += c; // update crc
                                }
                                #endif
                                else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // rxd buffer not full
                                {
                                        if (c != '\r') // no termination character received
                                        {
                                                rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
                                                crc += c; // update crc
                                        }
                                        else // termination character received
                                        {
                                                // the last 2 bytes are no subject for checksum calculation
                                                // they are the checksum itself
                                                crc -= rxd_buffer[ptr_rxd_buffer-2];
                                                crc -= rxd_buffer[ptr_rxd_buffer-1];
                                                // calculate checksum from transmitted data
                                                crc %= 4096;
                                                crc1 = '=' + crc / 64;
                                                crc2 = '=' + crc % 64;
                                                // compare checksum to transmitted checksum bytes
                                                if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))
                                                {   // checksum valid
                                                        rxd_buffer_locked = TRUE;               // lock the rxd buffer
                                                        ReceivedBytes = ptr_rxd_buffer + 1;     // store number of received bytes
                                                        rxd_buffer[ptr_rxd_buffer] = '\r';      // set termination character
                                                        // if 2nd byte is an 'R' start bootloader
                                                        if(rxd_buffer[2] == 'R')
                                                        {
                                                                PowerOff();
                                                                VIC_DeInit();
                                                                Execute_Bootloader(); // Reset-Commando - Bootloader starten
                                                        }
                                                } // eof checksum valid
                                                else
                                                {       // checksum invalid
                                                        rxd_buffer_locked = FALSE; // unlock rxd buffer
                                                }  // eof checksum invalid
                                                ptr_rxd_buffer = 0; // reset rxd buffer pointer
                                        } // eof termination character received
                                } // rxd buffer not full
                                else // rxd buffer overrun
                                {
                                        ptr_rxd_buffer = 0; // reset rxd buffer pointer
                                        rxd_buffer_locked = FALSE; // unlock rxd buffer
                                } // eof rxd buffer overrrun
                        } // some byes in the fifo and rxd buffer not locked
                } // eof DebugUart = UART1
        }
}
 
/**************************************************************/
/*         Transmit tx buffer via debug uart                  */
/**************************************************************/
void UART1_Transmit(void)
{
        u8 tmp_tx;
        // if something has to be send and the txd fifo is not full
        if((!txd_complete) && (UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) == RESET))
        {
                tmp_tx = txd_buffer[ptr_txd_buffer]; // read byte from txd buffer
                // if terminating character or end of txd buffer reached
                if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN))
                {
                        ptr_txd_buffer = 0;     // reset txd buffer pointer
                        txd_complete = TRUE;// set complete flag
                }
                UART_SendData(UART1, tmp_tx); // put character to txd fifo
                // set pointer to next byte
                ptr_txd_buffer++;
        }
}
 
/**************************************************************/
/* Add CRC and initiate transmission via debug uart           */
/**************************************************************/
void AddCRC(u16 datalen)
{
        u16 tmpCRC = 0, i;
        for(i = 0; i < datalen; i++)
        {
                tmpCRC += txd_buffer[i];
        }
        tmpCRC %= 4096;
        txd_buffer[i++] = '=' + tmpCRC / 64;
        txd_buffer[i++] = '=' + tmpCRC % 64;
        txd_buffer[i++] = '\r';
 
        ptr_txd_buffer = 0;
        txd_complete = FALSE;
        UART_SendData(UART1, txd_buffer[ptr_txd_buffer++]);     // send first byte, to be continued in the txd irq
}
 
 
 
/**************************************************************/
/* Code output data                                           */
/**************************************************************/
void SendOutData(u8 cmd, u8 Address, u8 numofbuffers , ...) //u8 *data, u8 len, ....
{
        va_list ap;
 
        u16 pt = 0;
        u8 a,b,c;
        u8 ptr = 0;
 
        u8* pdata = NULL;
        int len = 0;
 
        txd_buffer[pt++] = '#';                         // Start character
        txd_buffer[pt++] = 'a' + Address;       // Address (a=0; b=1,...)
        txd_buffer[pt++] = cmd;                         // Command
 
        va_start(ap, numofbuffers);
        if(numofbuffers)
        {
                pdata = va_arg(ap, u8*);
                len = va_arg(ap, int);
                ptr = 0;
                numofbuffers--;
        }
        while(len)
        {
                if(len)
                {
                        a = pdata[ptr++];
                        len--;
                        if((!len) && numofbuffers) // try to jump to next buffer
                        {
                                pdata = va_arg(ap, u8*);
                                len = va_arg(ap, int);
                                ptr = 0;
                                numofbuffers--;
                        }
                }
                else a = 0;
                if(len)
                {
                        b = pdata[ptr++];
                        len--;
                        if((!len) && numofbuffers) // try to jump to next buffer
                        {
                                pdata = va_arg(ap, u8*);
                                len = va_arg(ap, int);
                                ptr = 0;
                                numofbuffers--;
                        }
                }
                else b = 0;
                if(len)
                {
                        c = pdata[ptr++];
                        len--;
                        if((!len) && numofbuffers) // try to jump to next buffer
                        {
                                pdata = va_arg(ap, u8*);
                                len = va_arg(ap, int);
                                ptr = 0;
                                numofbuffers--;
                        }
                }
                else c = 0;
                txd_buffer[pt++] = '=' + (a >> 2);
                txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
                txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
                txd_buffer[pt++] = '=' + ( c & 0x3f);
        }
        va_end(ap);
        AddCRC(pt);     // add checksum after data block and initates the transmission
}
 
 
/**************************************************************/
/* Decode data                                                */
/**************************************************************/
void Decode64(void)
{
        u8 a,b,c,d;
        u8 x,y,z;
        u8 ptrIn = 3; // start with first data byte in rx buffer
        u8 ptrOut = 3;
        u8 len = ReceivedBytes - 6;      // must be a multiple of 4 (3 bytes at begin and 3 bytes at end are no payload )
        while(len)
        {
                a = rxd_buffer[ptrIn++] - '=';
                b = rxd_buffer[ptrIn++] - '=';
                c = rxd_buffer[ptrIn++] - '=';
                d = rxd_buffer[ptrIn++] - '=';
                //if(ptrIn > ReceivedBytes - 3) break;
 
                x = (a << 2) | (b >> 4);
                y = ((b & 0x0f) << 4) | (c >> 2);
                z = ((c & 0x03) << 6) | d;
 
                if(len--) rxd_buffer[ptrOut++] = x; else break;
                if(len--) rxd_buffer[ptrOut++] = y; else break;
                if(len--) rxd_buffer[ptrOut++] = z; else break;
        }
        pRxData = &rxd_buffer[3];
        RxDataLen = ptrOut - 3;
}
 
/**************************************************************/
/* Process incomming data from debug uart                     */
/**************************************************************/
void UART1_ProcessRxData(void)
{
        // if data in the rxd buffer are not locked immediately return
        if((!rxd_buffer_locked) || (DebugUART != UART1) ) return;
        Waypoint_t * pWaypoint = NULL;
 
 
 
        PcAccess = 255;
        Decode64(); // decode data block in rxd buffer
        switch(rxd_buffer[1] - 'a') // check for Slave Address
        {
                case NC_ADDRESS:  // own Slave Address
 
                switch(rxd_buffer[2])
                {
                                case 'z': // connection checker
                                memcpy(&Echo, (u8*)pRxData, sizeof(Echo)); // copy echo pattern
                                SerialLinkOkay = 250;            // reset SerialTimeout
                                Request_Echo = TRUE;
                                break;
 
                        case 'e': // request for the text of the error status
                                Request_ErrorMessage = TRUE;
                                break;
 
                        case 's'://  new target position
                                pWaypoint = (Waypoint_t*)&pRxData[0];
                                BeepTime = 300;
                                if(pWaypoint->Position.Status == NEWDATA)
                                {
                                        WPList_Clear(); // empty WPList
                                        WPList_Append(pWaypoint);
                                        GPS_pWaypoint = WPList_Begin();
                                }
                                break;
 
                        case 'u': // redirect debug uart
                                switch(pRxData[0])
                                {
                                        case UART_FLIGHTCTRL:
                                                UART2_Init();                           // initialize UART2 to FC pins
                                                DebugUART = UART2;
                                                break;
                                        case UART_MK3MAG:
                                                if(FC.MKFlags & MKFLAG_MOTOR_RUN) break; // not if the motors are running
                                                UART0_Connect_to_MK3MAG();      // mux UART0 to MK3MAG pins
                                                GPSData.Status = INVALID;
                                                DebugUART = UART0;
                                                break;
                                        case UART_MKGPS:
                                                if(FC.MKFlags & MKFLAG_MOTOR_RUN) break; // not if the motors are running
                                                UART0_Connect_to_MKGPS();       // connect UART0 to MKGPS pins
                                                GPSData.Status = INVALID;
                                                DebugUART = UART0;
                                                break;
                                }
                                break;
 
                        case 'w'://  Append Waypoint to List
                                pWaypoint = (Waypoint_t*)&pRxData[0];
                                if(pWaypoint->Position.Status == INVALID)
                                {  // clear WP List
                                        WPList_Clear();
                                        GPS_pWaypoint = WPList_Begin();
                                        //SerialPutString("\r\nClear WP List\r\n");
                                }
                                else if (pWaypoint->Position.Status == NEWDATA)
                                {  // app current WP to the list
                                        WPList_Append(pWaypoint);
                                        BeepTime = 500;
                                        //SerialPutString("\r\nAdd WP to List\r\n");
                                }
                                Request_NewWaypoint = TRUE;
                                break;
 
                        case 'x'://  Read Waypoint from List
                                Request_ReadWaypoint = pRxData[0];
                                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(rxd_buffer[2]) // check CmdID
                {
                        case 'a':// request for the labels of the analog debug outputs
                                Request_DebugLabel = pRxData[0];
                                if(Request_DebugLabel > 31) Request_DebugLabel = 31;
                                break;
 
                        case 'b': // submit extern control
                                memcpy(&ExternControl, (u8*)&pRxData[0], sizeof(ExternControl));
                                ConfirmFrame = ExternControl.Frame;
                                break;
 
                        case 'd': // request for debug data;
                                DebugData_Interval = (u32) pRxData[0] * 10;
                                if(DebugData_Interval > 0) Request_DebugData = TRUE;
                                break;
 
                        case 'c': // request for 3D data;
                                Data3D_Interval = (u32) pRxData[0] * 10;
                                if(Data3D_Interval > 0) Request_Data3D = TRUE;
                                break;
 
                        case 'g':// request for external control data
                                Request_ExternalControl = TRUE;
                                break;
 
                        case 'h':// reqest for display line
                                RemoteKeys |= pRxData[0];
                                if(RemoteKeys != 0) DisplayLine = 0;
                                Request_Display = TRUE;
                                break;
 
                        case 'l':// reqest for display columns
                                MenuItem = pRxData[0];
                                Request_Display1 = TRUE;
                                break;
 
                        case 'o': // request for navigation information
                                NaviData_Interval = (u32) pRxData[0] * 10;
                                if(NaviData_Interval > 0) Request_NaviData = TRUE;
                                break;
 
                        case 'v': // request for version info
                                Request_VerInfo = TRUE;
                                break;
                        default:
                                // unsupported command recieved
                                break;
                }
                break; // default:
        }
        // unlock the rxd buffer after processing
        pRxData = NULL;
        RxDataLen = 0;
        rxd_buffer_locked = FALSE;
}
 
 
/*****************************************************/
/*                   Send a character                */
/*****************************************************/
s16 uart_putchar (char c)
{
        if (c == '\n') uart_putchar('\r');
        // wait until txd fifo is not full
        while (UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != RESET);
        // transmit byte
        UART_SendData(UART1, c);
        return (0);
}
 
/*****************************************************/
/*       Send a string to the debug uart              */
/*****************************************************/
void SerialPutString(u8 *s)
{
        if(s == NULL) return;
        while (*s != '\0' && DebugUART == UART1)
        {
                uart_putchar(*s);
                s ++;
        }
}
 
 
 
/**************************************************************/
/* Send the answers to incomming commands at the debug uart   */
/**************************************************************/
void UART1_TransmitTxData(void)
{
        if(!txd_complete || (DebugUART != UART1) ) return;
 
        if(Request_Echo && txd_complete)
        {
                SendOutData('Z', NC_ADDRESS, 1, &Echo, sizeof(Echo)); // answer the echo request
                Echo = 0; // reset echo value
                Request_Echo = FALSE;
        }
        if(Request_DebugLabel != 0xFF)
        {
                SendOutData('A', NC_ADDRESS, 2, &Request_DebugLabel, sizeof(Request_DebugLabel), (u8 *) ANALOG_LABEL[Request_DebugLabel], 16);
                Request_DebugLabel = 0xFF;
        }
        if(ConfirmFrame && txd_complete)
        {
                SendOutData('B', NC_ADDRESS, 1, &ConfirmFrame, sizeof(ConfirmFrame));
                ConfirmFrame = 0;
        }
        if( (( (DebugData_Interval > 0) && CheckDelay(DebugData_Timer)) || Request_DebugData) && txd_complete)
        {
                SendOutData('D', NC_ADDRESS, 1,(u8 *)&DebugOut, sizeof(DebugOut));
                DebugData_Timer = SetDelay(DebugData_Interval);
                Request_DebugData = FALSE;
        }
 
        if((( (Data3D_Interval > 0) && CheckDelay(Data3D_Timer) ) || Request_Data3D) && txd_complete)
        {
                SendOutData('C', NC_ADDRESS, 1,(u8 *)&Data3D, sizeof(Data3D));
                Data3D_Timer = SetDelay(Data3D_Interval);
                Request_Data3D = FALSE;
        }
 
        if(Request_ExternalControl && txd_complete)
        {
                SendOutData('G', NC_ADDRESS, 1, (u8 *)&ExternControl, sizeof(ExternControl));
                Request_ExternalControl = FALSE;
        }
        if(Request_Display && txd_complete)
        {
                LCD_PrintMenu();
                SendOutData('H', NC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), (u8*)&DisplayBuff[DisplayLine * 20], 20);
                DisplayLine++;
                if(DisplayLine >= 4) DisplayLine = 0;
                Request_Display = FALSE;
        }
        if(Request_Display1 && txd_complete)
        {
                LCD_PrintMenu();
                SendOutData('L', NC_ADDRESS, 3, (u8*)&MenuItem, sizeof(MenuItem), (u8*)&MaxMenuItem, sizeof(MaxMenuItem),(u8*)DisplayBuff, sizeof(DisplayBuff));
                Request_Display1 = FALSE;
        }
        if(Request_VerInfo && txd_complete)
        {
                SendOutData('V', NC_ADDRESS,1, (u8 *)&UART_VersionInfo, sizeof(UART_VersionInfo));
                Request_VerInfo = FALSE;
        }
        if(( (NaviData_Interval && CheckDelay(NaviData_Timer) ) || Request_NaviData) && txd_complete)
        {
                NaviData.Errorcode = ErrorCode;
                SendOutData('O', NC_ADDRESS,1, (u8 *)&NaviData, sizeof(NaviData));
                if (DebugUART == UART1) SendOutData0('O', NC_ADDRESS,1, (u8 *)&NaviData, sizeof(NaviData));
                NaviData_Timer = SetDelay(NaviData_Interval);
                Request_NaviData = FALSE;
        }
        if(Request_ErrorMessage && txd_complete)
        {
                SendOutData('E', NC_ADDRESS, 1, (u8 *)&ErrorMSG, sizeof(ErrorMSG));
                Request_ErrorMessage = FALSE;
        }
        if(Request_SendFollowMe && txd_complete && (GPSData.NumOfSats >= 4))              // sending for "Follow me"
        {
                GPS_CopyPosition(&(GPSData.Position),&(FollowMe.Position));
                FollowMe.Position.Status = NEWDATA;
                FollowMe.Heading = -1;
                FollowMe.ToleranceRadius = 1;
                FollowMe.HoldTime = 60;
                FollowMe.Event_Flag = 0;
                FollowMe.reserve[0] = 0;                // reserve
                FollowMe.reserve[1] = 0;                // reserve
                FollowMe.reserve[2] = 0;                // reserve
                FollowMe.reserve[3] = 0;                // reserve
                SendOutData('s', NC_ADDRESS, 1, (u8 *)&FollowMe, sizeof(FollowMe));
                Request_SendFollowMe = FALSE;
        }
 
        if(Request_NewWaypoint && txd_complete)
        {
                u8 WPNumber = WPList_GetCount();
                SendOutData('W', NC_ADDRESS, 1, &WPNumber, sizeof(WPNumber));
                Request_NewWaypoint = FALSE;
        }
 
        if((Request_ReadWaypoint != 0xFF) && txd_complete)
        {
                u8 WPNumber = WPList_GetCount();
                if (Request_ReadWaypoint < WPNumber)
                {
                        SendOutData('X', NC_ADDRESS, 3, &WPNumber, 1, &Request_ReadWaypoint, 1, WPList_GetAt(Request_ReadWaypoint), sizeof(Waypoint_t));
                }
                else
                {
                        SendOutData('X', NC_ADDRESS, 1, &WPNumber, sizeof(WPNumber));
                }
                Request_ReadWaypoint = 0xFF;
        }
 
}
 
 

Rev 95	Rev 96
1	/#######################################################################################/	1	/#######################################################################################/
2	/* !!! THIS IS NOT FREE SOFTWARE !!! */	2	/* !!! THIS IS NOT FREE SOFTWARE !!! */
3	/#######################################################################################/	3	/#######################################################################################/
4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5	// + Copyright (c) 2008 Ingo Busker, Holger Buss	5	// + Copyright (c) 2008 Ingo Busker, Holger Buss
6	// + Nur für den privaten Gebrauch	6	// + Nur für den privaten Gebrauch
7	// + FOR NON COMMERCIAL USE ONLY	7	// + FOR NON COMMERCIAL USE ONLY
8	// + www.MikroKopter.com	8	// + www.MikroKopter.com
9	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	9	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
10	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),	10	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
11	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.	11	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
12	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt	12	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
13	// + bzgl. der Nutzungsbedingungen aufzunehmen.	13	// + bzgl. der Nutzungsbedingungen aufzunehmen.
14	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,	14	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
15	// + Verkauf von Luftbildaufnahmen, usw.	15	// + Verkauf von Luftbildaufnahmen, usw.
16	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	16	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,	17	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
18	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen	18	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts	20	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
21	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"	21	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
22	// + eindeutig als Ursprung verlinkt werden	22	// + eindeutig als Ursprung verlinkt werden
23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion	24	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
25	// + Benutzung auf eigene Gefahr	25	// + Benutzung auf eigene Gefahr
26	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden	26	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
27	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	27	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28	// + Die PORTIERUNG der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur	28	// + Die PORTIERUNG der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
29	// + mit unserer Zustimmung zulässig	29	// + mit unserer Zustimmung zulässig
30	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	30	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen	31	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
32	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	32	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
33	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,	33	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
34	// + this list of conditions and the following disclaimer.	34	// + this list of conditions and the following disclaimer.
35	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived	35	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
36	// + from this software without specific prior written permission.	36	// + from this software without specific prior written permission.
37	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted	37	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
38	// + for non-commercial use (directly or indirectly)	38	// + for non-commercial use (directly or indirectly)
39	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted	39	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
40	// + with our written permission	40	// + with our written permission
41	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be	41	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
42	// + clearly linked as origin	42	// + clearly linked as origin
43	// + * PORTING this software (or part of it) to systems (other than hardware from www.mikrokopter.de) is NOT allowed	43	// + * PORTING this software (or part of it) to systems (other than hardware from www.mikrokopter.de) is NOT allowed
44	//	44	//
45	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"	45	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
46	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE	46	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
47	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE	47	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
48	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE	48	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
49	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR	49	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
50	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF	50	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
51	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS	51	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
52	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN	52	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
53	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)	53	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
54	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE	54	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
55	// + POSSIBILITY OF SUCH DAMAGE.	55	// + POSSIBILITY OF SUCH DAMAGE.
56	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	56	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
57	#include <stdio.h>	57	#include <stdio.h>
58	#include <stdarg.h>	58	#include <stdarg.h>
59	#include <string.h>	59	#include <string.h>
60		60
61	#include "91x_lib.h"	61	#include "91x_lib.h"
62	#include "ramfunc.h"	62	#include "ramfunc.h"
63	#include "menu.h"	63	#include "menu.h"
64	#include "printf_P.h"	64	#include "printf_P.h"
65	#include "GPS.h"	65	#include "GPS.h"
66	#include "i2c.h"	66	#include "i2c.h"
67	#include "uart0.h"	67	#include "uart0.h"
68	#include "uart1.h"	68	#include "uart1.h"
69	#include "uart2.h"	69	#include "uart2.h"
70	#include "timer.h"	70	#include "timer.h"
71	#include "usb.h"	71	#include "usb.h"
72	#include "main.h"	72	#include "main.h"
73	#include "waypoints.h"	73	#include "waypoints.h"
74	#include "GPS.h"	74	#include "GPS.h"
75		75
76	// slave addresses	76	// slave addresses
77	#define FC_ADDRESS 1	77	#define FC_ADDRESS 1
78	#define NC_ADDRESS 2	78	#define NC_ADDRESS 2
79	#define MK3MAG_ADDRESS 3	79	#define MK3MAG_ADDRESS 3
80		80
81	#define FALSE 0	81	#define FALSE 0
82	#define TRUE 1	82	#define TRUE 1
83		83
84	typedef struct	84	typedef struct
85	{	85	{
86	u8 SWMajor;	86	u8 SWMajor;
87	u8 SWMinor;	87	u8 SWMinor;
88	u8 ProtoMajor;	88	u8 ProtoMajor;
89	u8 ProtoMinor;	89	u8 ProtoMinor;
90	u8 SWPatch;	90	u8 SWPatch;
91	u8 Reserved[5];	91	u8 Reserved[5];
92	} __attribute__((packed)) UART_VersionInfo_t;	92	} __attribute__((packed)) UART_VersionInfo_t;
93		93
94		94
95	u8 Request_SendFollowMe = FALSE;	95	u8 Request_SendFollowMe = FALSE;
96	u8 Request_VerInfo = FALSE;	96	u8 Request_VerInfo = FALSE;
97	u8 Request_ExternalControl = FALSE;	97	u8 Request_ExternalControl = FALSE;
98	u8 Request_Display = FALSE;	98	u8 Request_Display = FALSE;
99	u8 Request_Display1 = FALSE;	99	u8 Request_Display1 = FALSE;
100	u8 Request_DebugData = FALSE;	100	u8 Request_DebugData = FALSE;
101	u8 Request_DebugLabel = 255;	101	u8 Request_DebugLabel = 255;
102	u8 Request_ChannelOnly = FALSE;	102	u8 Request_ChannelOnly = FALSE;
103	u8 Request_NaviData = FALSE;	103	u8 Request_NaviData = FALSE;
104	u8 Request_ErrorMessage = FALSE;	104	u8 Request_ErrorMessage = FALSE;
105	u8 Request_NewWaypoint = FALSE;	105	u8 Request_NewWaypoint = FALSE;
106	u8 Request_ReadWaypoint = 255;	106	u8 Request_ReadWaypoint = 255;
107	u8 Request_Data3D = FALSE;	107	u8 Request_Data3D = FALSE;
108	u8 Request_Echo = FALSE;	108	u8 Request_Echo = FALSE;
109		109
110	u8 DisplayLine = 0;	110	u8 DisplayLine = 0;
111		111
112	UART_TypeDef *DebugUART = UART1;	112	UART_TypeDef *DebugUART = UART1;
113		113
114	volatile u8 txd_buffer[TXD_BUFFER_LEN];	114	volatile u8 txd_buffer[TXD_BUFFER_LEN];
115	volatile u8 rxd_buffer_locked = FALSE;	115	volatile u8 rxd_buffer_locked = FALSE;
116	volatile u8 rxd_buffer[RXD_BUFFER_LEN];	116	volatile u8 rxd_buffer[RXD_BUFFER_LEN];
117	volatile u8 txd_complete = TRUE;	117	volatile u8 txd_complete = TRUE;
118	volatile u8 ReceivedBytes = 0;	118	volatile u8 ReceivedBytes = 0;
119	volatile u8 CntCrcError = 0;	119	volatile u8 CntCrcError = 0;
120	volatile u8 *pRxData = NULL;	120	volatile u8 *pRxData = NULL;
121	volatile u8 RxDataLen = 0;	121	volatile u8 RxDataLen = 0;
122	volatile u8 SerialLinkOkay = 0;	122	volatile u8 SerialLinkOkay = 0;
123		123
124	u8 text[100];	124	u8 text[100];
125		125
126	u8 PcAccess = 100;	126	u8 PcAccess = 100;
127	u8 MotorTest[4] = {0,0,0,0};	127	u8 MotorTest[4] = {0,0,0,0};
128	u8 ConfirmFrame = 0;	128	u8 ConfirmFrame = 0;
129		129
130	DebugOut_t DebugOut;	130	DebugOut_t DebugOut;
131	ExternControl_t ExternControl;	131	ExternControl_t ExternControl;
132	UART_VersionInfo_t UART_VersionInfo;	132	UART_VersionInfo_t UART_VersionInfo;
133	NaviData_t NaviData;	133	NaviData_t NaviData;
134	Waypoint_t FollowMe;	134	Waypoint_t FollowMe;
135	Data3D_t Data3D;	135	Data3D_t Data3D;
136	u16 Echo; // 2 bytes recieved will be sent back as echo	136	u16 Echo; // 2 bytes recieved will be sent back as echo
137		137
138	u32 DebugData_Timer;	138	u32 DebugData_Timer;
139	u32 DebugData_Interval = 5000; // in ms	139	u32 DebugData_Interval = 5000; // in ms
140	u32 NaviData_Timer;	140	u32 NaviData_Timer;
141	u32 NaviData_Interval = 5000; // in ms	141	u32 NaviData_Interval = 5000; // in ms
142	u32 Data3D_Timer = 0; // in ms	142	u32 Data3D_Timer = 0; // in ms
143	u32 Data3D_Interval = 0;	143	u32 Data3D_Interval = 0;
144		144
145	static u16 ptr_txd_buffer = 0;	145	static u16 ptr_txd_buffer = 0;
146		146
147	const u8 ANALOG_LABEL[32][16] =	147	const u8 ANALOG_LABEL[32][16] =
148	{	148	{
149	//1234567890123456	149	//1234567890123456
150	"AngleNick ", //0	150	"AngleNick ", //0
151	"AngleRoll ",	151	"AngleRoll ",
152	"AccNick ",	152	"AccNick ",
153	"AccRoll ",	153	"AccRoll ",
154	" ",	154	" ",
155	"MK-Flags ", //5	155	"MK-Flags ", //5
156	"NC-Flags ",	156	"NC-Flags ",
157	" ",	157	" ",
158	" ",	158	" ",
159	"GPS Data ",	159	"GPS Data ",
160	"CompassHeading ", //10	160	"CompassHeading ", //10
161	"GyroHeading ",	161	"GyroHeading ",
162	"SPI Error ",	162	"SPI Error ",
163	"SPI Okay ",	163	"SPI Okay ",
164	"I2C Error ",	164	"I2C Error ",
165	"I2C Okay ", //15	165	"I2C Okay ", //15
166	" ",// "FC_Kalman_K ",	166	" ",// "FC_Kalman_K ",
167	"ACC_Speed_N ",	167	"ACC_Speed_N ",
168	"ACC_Speed_E ",	168	"ACC_Speed_E ",
169	" ",// "GPS ACC ",	169	" ",// "GPS ACC ",
170	" ",// "MAXDrift ", //20	170	" ",// "MAXDrift ", //20
171	"N_Speed ",	171	"N_Speed ",
172	"E_Speed ",	172	"E_Speed ",
173	" ",// "KalmDist_N ",	173	" ",// "KalmDist_N ",
174	" ",// "KalmDist_E ",	174	" ",// "KalmDist_E ",
175	" ",//25	175	" ",//25
176	" ",	176	" ",
177	"Distance N ",	177	"Distance N ",
178	"Distance E ",	178	"Distance E ",
179	"GPS_Nick ",	179	"GPS_Nick ",
180	"GPS_Roll ", //30	180	"GPS_Roll ", //30
181	"Used_Sats "	181	"Used_Sats "
182	};	182	};
183		183
184		184
185	/********************************************************/	185	/********************************************************/
186	/* Initialization the UART1 */	186	/* Initialization the UART1 */
187	/********************************************************/	187	/********************************************************/
188	void UART1_Init (void)	188	void UART1_Init (void)
189	{	189	{
190	GPIO_InitTypeDef GPIO_InitStructure;	190	GPIO_InitTypeDef GPIO_InitStructure;
191	UART_InitTypeDef UART_InitStructure;	191	UART_InitTypeDef UART_InitStructure;
192		192
193	SCU_APBPeriphClockConfig(__UART1, ENABLE); // Enable the UART1 Clock	193	SCU_APBPeriphClockConfig(__UART1, ENABLE); // Enable the UART1 Clock
194	SCU_APBPeriphClockConfig(__GPIO3, ENABLE); // Enable the GPIO3 Clock	194	SCU_APBPeriphClockConfig(__GPIO3, ENABLE); // Enable the GPIO3 Clock
195		195
196	/Configure UART1_Rx pin GPIO3.2/	196	/Configure UART1_Rx pin GPIO3.2/
197	GPIO_StructInit(&GPIO_InitStructure);	197	GPIO_StructInit(&GPIO_InitStructure);
198	GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;	198	GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
199	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;	199	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
200	GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull;	200	GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull;
201	GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;	201	GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;
202	GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1; // UART1_RxD	202	GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1; // UART1_RxD
203	GPIO_Init(GPIO3, &GPIO_InitStructure);	203	GPIO_Init(GPIO3, &GPIO_InitStructure);
204		204
205	/Configure UART1_Tx pin GPIO3.3/	205	/Configure UART1_Tx pin GPIO3.3/
206	GPIO_StructInit(&GPIO_InitStructure);	206	GPIO_StructInit(&GPIO_InitStructure);
207	GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;	207	GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
208	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;	208	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
209	GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull;	209	GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull;
210	GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2; // UART1_TX	210	GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2; // UART1_TX
211	GPIO_Init(GPIO3, &GPIO_InitStructure);	211	GPIO_Init(GPIO3, &GPIO_InitStructure);
212		212
213	/* UART1 configured as follow:	213	/* UART1 configured as follow:
214	- Word Length = 8 Bits	214	- Word Length = 8 Bits
215	- One Stop Bit	215	- One Stop Bit
216	- No parity	216	- No parity
217	- BaudRate = 57600 baud	217	- BaudRate = 57600 baud
218	- Hardware flow control Disabled	218	- Hardware flow control Disabled
219	- Receive and transmit enabled	219	- Receive and transmit enabled
220	- Receive and transmit FIFOs are Disabled	220	- Receive and transmit FIFOs are Disabled
221	*/	221	*/
222	UART_StructInit(&UART_InitStructure);	222	UART_StructInit(&UART_InitStructure);
223	UART_InitStructure.UART_WordLength = UART_WordLength_8D;	223	UART_InitStructure.UART_WordLength = UART_WordLength_8D;
224	UART_InitStructure.UART_StopBits = UART_StopBits_1;	224	UART_InitStructure.UART_StopBits = UART_StopBits_1;
225	UART_InitStructure.UART_Parity = UART_Parity_No ;	225	UART_InitStructure.UART_Parity = UART_Parity_No ;
226	UART_InitStructure.UART_BaudRate = BAUD_RATE;	226	UART_InitStructure.UART_BaudRate = BAUD_RATE;
227	UART_InitStructure. UART_HardwareFlowControl = UART_HardwareFlowControl_None;	227	UART_InitStructure. UART_HardwareFlowControl = UART_HardwareFlowControl_None;
228	UART_InitStructure.UART_Mode = UART_Mode_Tx_Rx;	228	UART_InitStructure.UART_Mode = UART_Mode_Tx_Rx;
229	UART_InitStructure.UART_FIFO = UART_FIFO_Enable;	229	UART_InitStructure.UART_FIFO = UART_FIFO_Enable;
230	UART_InitStructure.UART_TxFIFOLevel = UART_FIFOLevel_1_2;	230	UART_InitStructure.UART_TxFIFOLevel = UART_FIFOLevel_1_2;
231	UART_InitStructure.UART_RxFIFOLevel = UART_FIFOLevel_1_2;	231	UART_InitStructure.UART_RxFIFOLevel = UART_FIFOLevel_1_2;
232		232
233	UART_DeInit(UART1); // reset uart 1 to default	233	UART_DeInit(UART1); // reset uart 1 to default
234	UART_Init(UART1, &UART_InitStructure); // initialize uart 1	234	UART_Init(UART1, &UART_InitStructure); // initialize uart 1
235	// enable uart 1 interrupts selective	235	// enable uart 1 interrupts selective
236	UART_ITConfig(UART1, UART_IT_Receive \| UART_IT_ReceiveTimeOut, ENABLE);	236	UART_ITConfig(UART1, UART_IT_Receive \| UART_IT_ReceiveTimeOut, ENABLE);
237	UART_Cmd(UART1, ENABLE); // enable uart 1	237	UART_Cmd(UART1, ENABLE); // enable uart 1
238	// configure the uart 1 interupt line as an IRQ with priority 4 (0 is highest)	238	// configure the uart 1 interupt line as an IRQ with priority 4 (0 is highest)
239	VIC_Config(UART1_ITLine, VIC_IRQ, 4);	239	VIC_Config(UART1_ITLine, VIC_IRQ, 4);
240	// enable the uart 1 IRQ	240	// enable the uart 1 IRQ
241	VIC_ITCmd(UART1_ITLine, ENABLE);	241	VIC_ITCmd(UART1_ITLine, ENABLE);
242	// initialize the debug timer	242	// initialize the debug timer
243	DebugData_Timer = SetDelay(DebugData_Interval);	243	DebugData_Timer = SetDelay(DebugData_Interval);
244	NaviData_Timer = SetDelay(NaviData_Interval)+500;	244	NaviData_Timer = SetDelay(NaviData_Interval)+500;
245	// unlock rxd_buffer	245	// unlock rxd_buffer
246	rxd_buffer_locked = FALSE;	246	rxd_buffer_locked = FALSE;
247	pRxData = NULL;	247	pRxData = NULL;
248	RxDataLen = 0;	248	RxDataLen = 0;
249	// no bytes to send	249	// no bytes to send
250	txd_complete = TRUE;	250	txd_complete = TRUE;
251	// Fill Version Info Structure	251	// Fill Version Info Structure
252	UART_VersionInfo.SWMajor = VERSION_MAJOR;	252	UART_VersionInfo.SWMajor = VERSION_MAJOR;
253	UART_VersionInfo.SWMinor = VERSION_MINOR;	253	UART_VersionInfo.SWMinor = VERSION_MINOR;
254	UART_VersionInfo.SWPatch = VERSION_PATCH;	254	UART_VersionInfo.SWPatch = VERSION_PATCH;
255	UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;	255	UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;
256	UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;	256	UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;
257		257
258	NaviData.Version = NAVIDATA_VERSION;	258	NaviData.Version = NAVIDATA_VERSION;
259		259
260	SerialPutString("\r\nUART1 init...ok");	260	SerialPutString("\r\nUART1 init...ok");
261	}	261	}
262		262
263		263
264	/****************************************************************/	264	/****************************************************************/
265	/* USART1 receiver ISR */	265	/* USART1 receiver ISR */
266	/****************************************************************/	266	/****************************************************************/
267	void UART1_IRQHandler(void)	267	void UART1_IRQHandler(void)
268	{	268	{
269	static u16 crc;	269	static u16 crc;
270	static u8 ptr_rxd_buffer = 0;	270	static u8 ptr_rxd_buffer = 0;
271	static u8 crc1, crc2;	271	static u8 crc1, crc2;
272	static u8 abortState = 0;	272	static u8 abortState = 0;
273	u8 c;	273	u8 c;
274		274
275	if((UART_GetITStatus(UART1, UART_IT_Receive) != RESET) \|\| (UART_GetITStatus(UART1, UART_IT_ReceiveTimeOut) != RESET) )	275	if((UART_GetITStatus(UART1, UART_IT_Receive) != RESET) \|\| (UART_GetITStatus(UART1, UART_IT_ReceiveTimeOut) != RESET) )
276	{	276	{
277	// clear the pending bits	277	// clear the pending bits
278	UART_ClearITPendingBit(UART1, UART_IT_Receive);	278	UART_ClearITPendingBit(UART1, UART_IT_Receive);
279	UART_ClearITPendingBit(UART1, UART_IT_ReceiveTimeOut);	279	UART_ClearITPendingBit(UART1, UART_IT_ReceiveTimeOut);
280	// if debug UART is not UART1	280	// if debug UART is not UART1
281	if (DebugUART != UART1)	281	if (DebugUART != UART1)
282	{ // forward received data to the debug UART tx buffer	282	{ // forward received data to the debug UART tx buffer
283	while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)	283	while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)
284	{	284	{
285	// wait for space in the tx buffer of the DebugUART	285	// wait for space in the tx buffer of the DebugUART
286	while(UART_GetFlagStatus(DebugUART, UART_FLAG_TxFIFOFull) == SET) {};	286	while(UART_GetFlagStatus(DebugUART, UART_FLAG_TxFIFOFull) == SET) {};
287	// move the byte from the rx buffer of UART1 to the tx buffer of DebugUART	287	// move the byte from the rx buffer of UART1 to the tx buffer of DebugUART
288	c = UART_ReceiveData(UART1);	288	c = UART_ReceiveData(UART1);
289		289
290	// check for abort condition (ESC ESC 0x55 0xAA 0x00)	290	// check for abort condition (ESC ESC 0x55 0xAA 0x00)
291	switch (abortState)	291	switch (abortState)
292	{	292	{
293	case 0: if (c == 27) abortState++;	293	case 0: if (c == 27) abortState++;
294	break;	294	break;
295	case 1: if (c == 27) abortState++; else abortState = 0;	295	case 1: if (c == 27) abortState++; else abortState = 0;
296	break;	296	break;
297	case 2: if (c == 0x55) abortState++; else abortState = 0;	297	case 2: if (c == 0x55) abortState++; else abortState = 0;
298	break;	298	break;
299	case 3: if (c == 0xAA) abortState++; else abortState = 0;	299	case 3: if (c == 0xAA) abortState++; else abortState = 0;
300	break;	300	break;
301	case 4: if (c == 0x00)	301	case 4: if (c == 0x00)
302	{	302	{
303	DebugUART = UART1;	303	DebugUART = UART1;
304	UART0_Connect_to_MKGPS();	304	UART0_Connect_to_MKGPS();
305	}	305	}
306	abortState = 0;	306	abortState = 0;
307	break;	307	break;
308	}	308	}
309		309
310	if (DebugUART != UART1) UART_SendData(DebugUART, c);	310	if (DebugUART != UART1) UART_SendData(DebugUART, c);
311	}	311	}
312	}	312	}
313	else // DebugUART == UART1 (normal operation)	313	else // DebugUART == UART1 (normal operation)
314	{	314	{
315	while ((UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET) && (!rxd_buffer_locked))	315	while ((UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET) && (!rxd_buffer_locked))
316	{ // some byes in the fifo and rxd buffer not locked	316	{ // some byes in the fifo and rxd buffer not locked
317	// get byte from fifo	317	// get byte from fifo
318	c = UART_ReceiveData(UART1);	318	c = UART_ReceiveData(UART1);
319	if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received	319	if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received
320	{	320	{
321	rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer	321	rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
322	crc = c; // init crc	322	crc = c; // init crc
323	}	323	}
324	#if 0	324	#if 0
325	else if (ptr_rxd_buffer == 1) // handle address	325	else if (ptr_rxd_buffer == 1) // handle address
326	{	326	{
327	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer	327	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
328	crc += c; // update crc	328	crc += c; // update crc
329	}	329	}
330	#endif	330	#endif
331	else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // rxd buffer not full	331	else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // rxd buffer not full
332	{	332	{
333	if (c != '\r') // no termination character received	333	if (c != '\r') // no termination character received
334	{	334	{
335	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer	335	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
336	crc += c; // update crc	336	crc += c; // update crc
337	}	337	}
338	else // termination character received	338	else // termination character received
339	{	339	{
340	// the last 2 bytes are no subject for checksum calculation	340	// the last 2 bytes are no subject for checksum calculation
341	// they are the checksum itself	341	// they are the checksum itself
342	crc -= rxd_buffer[ptr_rxd_buffer-2];	342	crc -= rxd_buffer[ptr_rxd_buffer-2];
343	crc -= rxd_buffer[ptr_rxd_buffer-1];	343	crc -= rxd_buffer[ptr_rxd_buffer-1];
344	// calculate checksum from transmitted data	344	// calculate checksum from transmitted data
345	crc %= 4096;	345	crc %= 4096;
346	crc1 = '=' + crc / 64;	346	crc1 = '=' + crc / 64;
347	crc2 = '=' + crc % 64;	347	crc2 = '=' + crc % 64;
348	// compare checksum to transmitted checksum bytes	348	// compare checksum to transmitted checksum bytes
349	if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))	349	if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))
350	{ // checksum valid	350	{ // checksum valid
351	rxd_buffer_locked = TRUE; // lock the rxd buffer	351	rxd_buffer_locked = TRUE; // lock the rxd buffer
352	ReceivedBytes = ptr_rxd_buffer + 1; // store number of received bytes	352	ReceivedBytes = ptr_rxd_buffer + 1; // store number of received bytes
353	rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character	353	rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
354	// if 2nd byte is an 'R' start bootloader	354	// if 2nd byte is an 'R' start bootloader
355	if(rxd_buffer[2] == 'R')	355	if(rxd_buffer[2] == 'R')
356	{	356	{
357	PowerOff();	357	PowerOff();
358	VIC_DeInit();	358	VIC_DeInit();
359	Execute_Bootloader(); // Reset-Commando - Bootloader starten	359	Execute_Bootloader(); // Reset-Commando - Bootloader starten
360	}	360	}
361	} // eof checksum valid	361	} // eof checksum valid
362	else	362	else
363	{ // checksum invalid	363	{ // checksum invalid
364	rxd_buffer_locked = FALSE; // unlock rxd buffer	364	rxd_buffer_locked = FALSE; // unlock rxd buffer
365	} // eof checksum invalid	365	} // eof checksum invalid
366	ptr_rxd_buffer = 0; // reset rxd buffer pointer	366	ptr_rxd_buffer = 0; // reset rxd buffer pointer
367	} // eof termination character received	367	} // eof termination character received
368	} // rxd buffer not full	368	} // rxd buffer not full
369	else // rxd buffer overrun	369	else // rxd buffer overrun
370	{	370	{
371	ptr_rxd_buffer = 0; // reset rxd buffer pointer	371	ptr_rxd_buffer = 0; // reset rxd buffer pointer
372	rxd_buffer_locked = FALSE; // unlock rxd buffer	372	rxd_buffer_locked = FALSE; // unlock rxd buffer
373	} // eof rxd buffer overrrun	373	} // eof rxd buffer overrrun
374	} // some byes in the fifo and rxd buffer not locked	374	} // some byes in the fifo and rxd buffer not locked
375	} // eof DebugUart = UART1	375	} // eof DebugUart = UART1
376	}	376	}
377	}	377	}
378		378
379	/**************************************************************/	379	/**************************************************************/
380	/* Transmit tx buffer via debug uart */	380	/* Transmit tx buffer via debug uart */
381	/**************************************************************/	381	/**************************************************************/
382	void UART1_Transmit(void)	382	void UART1_Transmit(void)
383	{	383	{
384	u8 tmp_tx;	384	u8 tmp_tx;
385	// if something has to be send and the txd fifo is not full	385	// if something has to be send and the txd fifo is not full
386	if((!txd_complete) && (UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) == RESET))	386	if((!txd_complete) && (UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) == RESET))
387	{	387	{
388	tmp_tx = txd_buffer[ptr_txd_buffer]; // read byte from txd buffer	388	tmp_tx = txd_buffer[ptr_txd_buffer]; // read byte from txd buffer
389	// if terminating character or end of txd buffer reached	389	// if terminating character or end of txd buffer reached
390	if((tmp_tx == '\r') \|\| (ptr_txd_buffer == TXD_BUFFER_LEN))	390	if((tmp_tx == '\r') \|\| (ptr_txd_buffer == TXD_BUFFER_LEN))
391	{	391	{
392	ptr_txd_buffer = 0; // reset txd buffer pointer	392	ptr_txd_buffer = 0; // reset txd buffer pointer
393	txd_complete = TRUE;// set complete flag	393	txd_complete = TRUE;// set complete flag
394	}	394	}
395	UART_SendData(UART1, tmp_tx); // put character to txd fifo	395	UART_SendData(UART1, tmp_tx); // put character to txd fifo
396	// set pointer to next byte	396	// set pointer to next byte
397	ptr_txd_buffer++;	397	ptr_txd_buffer++;
398	}	398	}
399	}	399	}
400		400
401	/**************************************************************/	401	/**************************************************************/
402	/* Add CRC and initiate transmission via debug uart */	402	/* Add CRC and initiate transmission via debug uart */
403	/**************************************************************/	403	/**************************************************************/
404	void AddCRC(u16 datalen)	404	void AddCRC(u16 datalen)
405	{	405	{
406	u16 tmpCRC = 0, i;	406	u16 tmpCRC = 0, i;
407	for(i = 0; i < datalen; i++)	407	for(i = 0; i < datalen; i++)
408	{	408	{
409	tmpCRC += txd_buffer[i];	409	tmpCRC += txd_buffer[i];
410	}	410	}
411	tmpCRC %= 4096;	411	tmpCRC %= 4096;
412	txd_buffer[i++] = '=' + tmpCRC / 64;	412	txd_buffer[i++] = '=' + tmpCRC / 64;
413	txd_buffer[i++] = '=' + tmpCRC % 64;	413	txd_buffer[i++] = '=' + tmpCRC % 64;
414	txd_buffer[i++] = '\r';	414	txd_buffer[i++] = '\r';
415		415
416	ptr_txd_buffer = 0;	416	ptr_txd_buffer = 0;
417	txd_complete = FALSE;	417	txd_complete = FALSE;
418	UART_SendData(UART1, txd_buffer[ptr_txd_buffer++]); // send first byte, to be continued in the txd irq	418	UART_SendData(UART1, txd_buffer[ptr_txd_buffer++]); // send first byte, to be continued in the txd irq
419	}	419	}
420		420
421		421
422		422
423	/**************************************************************/	423	/**************************************************************/
424	/* Code output data */	424	/* Code output data */
425	/**************************************************************/	425	/**************************************************************/
426	void SendOutData(u8 cmd, u8 Address, u8 numofbuffers , ...) //u8 *data, u8 len, ....	426	void SendOutData(u8 cmd, u8 Address, u8 numofbuffers , ...) //u8 *data, u8 len, ....
427	{	427	{
428	va_list ap;	428	va_list ap;
429		429
430	u16 pt = 0;	430	u16 pt = 0;
431	u8 a,b,c;	431	u8 a,b,c;
432	u8 ptr = 0;	432	u8 ptr = 0;
433		433
434	u8* pdata = NULL;	434	u8* pdata = NULL;
435	int len = 0;	435	int len = 0;
436		436
437	txd_buffer[pt++] = '#'; // Start character	437	txd_buffer[pt++] = '#'; // Start character
438	txd_buffer[pt++] = 'a' + Address; // Address (a=0; b=1,...)	438	txd_buffer[pt++] = 'a' + Address; // Address (a=0; b=1,...)
439	txd_buffer[pt++] = cmd; // Command	439	txd_buffer[pt++] = cmd; // Command
440		440
441	va_start(ap, numofbuffers);	441	va_start(ap, numofbuffers);
442	if(numofbuffers)	442	if(numofbuffers)
443	{	443	{
444	pdata = va_arg(ap, u8*);	444	pdata = va_arg(ap, u8*);
445	len = va_arg(ap, int);	445	len = va_arg(ap, int);
446	ptr = 0;	446	ptr = 0;
447	numofbuffers--;	447	numofbuffers--;
448	}	448	}
449	while(len)	449	while(len)
450	{	450	{
451	if(len)	451	if(len)
452	{	452	{
453	a = pdata[ptr++];	453	a = pdata[ptr++];
454	len--;	454	len--;
455	if((!len) && numofbuffers) // try to jump to next buffer	455	if((!len) && numofbuffers) // try to jump to next buffer
456	{	456	{
457	pdata = va_arg(ap, u8*);	457	pdata = va_arg(ap, u8*);
458	len = va_arg(ap, int);	458	len = va_arg(ap, int);
459	ptr = 0;	459	ptr = 0;
460	numofbuffers--;	460	numofbuffers--;
461	}	461	}
462	}	462	}
463	else a = 0;	463	else a = 0;
464	if(len)	464	if(len)
465	{	465	{
466	b = pdata[ptr++];	466	b = pdata[ptr++];
467	len--;	467	len--;
468	if((!len) && numofbuffers) // try to jump to next buffer	468	if((!len) && numofbuffers) // try to jump to next buffer
469	{	469	{
470	pdata = va_arg(ap, u8*);	470	pdata = va_arg(ap, u8*);
471	len = va_arg(ap, int);	471	len = va_arg(ap, int);
472	ptr = 0;	472	ptr = 0;
473	numofbuffers--;	473	numofbuffers--;
474	}	474	}
475	}	475	}
476	else b = 0;	476	else b = 0;
477	if(len)	477	if(len)
478	{	478	{
479	c = pdata[ptr++];	479	c = pdata[ptr++];
480	len--;	480	len--;
481	if((!len) && numofbuffers) // try to jump to next buffer	481	if((!len) && numofbuffers) // try to jump to next buffer
482	{	482	{
483	pdata = va_arg(ap, u8*);	483	pdata = va_arg(ap, u8*);
484	len = va_arg(ap, int);	484	len = va_arg(ap, int);
485	ptr = 0;	485	ptr = 0;
486	numofbuffers--;	486	numofbuffers--;
487	}	487	}
488	}	488	}
489	else c = 0;	489	else c = 0;
490	txd_buffer[pt++] = '=' + (a >> 2);	490	txd_buffer[pt++] = '=' + (a >> 2);
491	txd_buffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));	491	txd_buffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));
492	txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));	492	txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));
493	txd_buffer[pt++] = '=' + ( c & 0x3f);	493	txd_buffer[pt++] = '=' + ( c & 0x3f);
494	}	494	}
495	va_end(ap);	495	va_end(ap);
496	AddCRC(pt); // add checksum after data block and initates the transmission	496	AddCRC(pt); // add checksum after data block and initates the transmission
497	}	497	}
498		498
499		499
500	/**************************************************************/	500	/**************************************************************/

Subversion Repositories NaviCtrl

(root)/tags/V2.10e/uart1.c @ 757 - Rev 95 → 96