WebSVN - NaviCtrl - Diff - Rev 41 and 49


/*#######################################################################################*/
/* !!! 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
 
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 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;
 
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;
 
u32 DebugData_Timer;
u32 DebugData_Interval = 1000; // in 1ms
u32 NaviData_Timer;
u32 NaviData_Interval = 100; // in 1ms
 
 
 
static u16 ptr_txd_buffer = 0;
 
const u8 ANALOG_LABEL[32][16] =
{
        //1234567890123456
        "AngleNick       ", //0
        "AngleRoll       ",
        "AccNick         ",
        "AccRoll         ",
        "                ",
        "                ", //5
        "                ",
        "                ",
        "                ",
        "                ",
        "                ", //10
        "GPS Data        ",
        "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      ",
        "GyroHeading     ",//25
        "CompassHeading  ",
        "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;
 
        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 'e': // request for the text of the error status
                                Request_ErrorMessage = TRUE;
                                break;
 
                        case 'o': // request for navigation information
                                NaviData_Interval = (u32) pRxData[0] * 10;
                                if(NaviData_Interval > 0) Request_NaviData = 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'://  new PCPosition for GPSTargetList
                                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;
 
                        default:
                                // unsupported command recieved
                                break;
                } // case NC_ADDRESS
 
 
                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 '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 '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_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(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));
#ifdef SEND_MKOSD_DATA          
                if (DebugUART == UART1) SendOutData0('O', NC_ADDRESS,1, (u8 *)&NaviData, sizeof(NaviData));      
#endif
                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;
        }
 
}
 
 

Rev 41	Rev 49
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	u8 Request_SendFollowMe = FALSE;	84	u8 Request_SendFollowMe = FALSE;
85	u8 Request_VerInfo = FALSE;	85	u8 Request_VerInfo = FALSE;
86	u8 Request_ExternalControl = FALSE;	86	u8 Request_ExternalControl = FALSE;
87	u8 Request_Display = FALSE;	87	u8 Request_Display = FALSE;
88	u8 Request_Display1 = FALSE;	88	u8 Request_Display1 = FALSE;
89	u8 Request_DebugData = FALSE;	89	u8 Request_DebugData = FALSE;
90	u8 Request_DebugLabel = 255;	90	u8 Request_DebugLabel = 255;
91	u8 Request_ChannelOnly = FALSE;	91	u8 Request_ChannelOnly = FALSE;
92	u8 Request_NaviData = FALSE;	92	u8 Request_NaviData = FALSE;
93	u8 Request_ErrorMessage = FALSE;	93	u8 Request_ErrorMessage = FALSE;
94	u8 Request_NewWaypoint = FALSE;	94	u8 Request_NewWaypoint = FALSE;
95		95
96	u8 DisplayLine = 0;	96	u8 DisplayLine = 0;
97		97
98	UART_TypeDef *DebugUART = UART1;	98	UART_TypeDef *DebugUART = UART1;
99		99
100	volatile u8 txd_buffer[TXD_BUFFER_LEN];	100	volatile u8 txd_buffer[TXD_BUFFER_LEN];
101	volatile u8 rxd_buffer_locked = FALSE;	101	volatile u8 rxd_buffer_locked = FALSE;
102	volatile u8 rxd_buffer[RXD_BUFFER_LEN];	102	volatile u8 rxd_buffer[RXD_BUFFER_LEN];
103	volatile u8 txd_complete = TRUE;	103	volatile u8 txd_complete = TRUE;
104	volatile u8 ReceivedBytes = 0;	104	volatile u8 ReceivedBytes = 0;
105	volatile u8 CntCrcError = 0;	105	volatile u8 CntCrcError = 0;
106	volatile u8 *pRxData = NULL;	106	volatile u8 *pRxData = NULL;
107	volatile u8 RxDataLen = 0;	107	volatile u8 RxDataLen = 0;
108		108
109	u8 text[100];	109	u8 text[100];
110		110
111	u8 PcAccess = 100;	111	u8 PcAccess = 100;
112	u8 MotorTest[4] = {0,0,0,0};	112	u8 MotorTest[4] = {0,0,0,0};
113	u8 ConfirmFrame = 0;	113	u8 ConfirmFrame = 0;
114		114
115	DebugOut_t DebugOut;	115	DebugOut_t DebugOut;
116	ExternControl_t ExternControl;	116	ExternControl_t ExternControl;
117	UART_VersionInfo_t UART_VersionInfo;	117	UART_VersionInfo_t UART_VersionInfo;
118	NaviData_t NaviData;	118	NaviData_t NaviData;
119	Waypoint_t FollowMe;	119	Waypoint_t FollowMe;
120		120
121	u32 DebugData_Timer;	121	u32 DebugData_Timer;
122	u32 DebugData_Interval = 1000; // in 1ms	122	u32 DebugData_Interval = 1000; // in 1ms
123	u32 NaviData_Timer;	123	u32 NaviData_Timer;
124	u32 NaviData_Interval = 1000; // in 1ms	124	u32 NaviData_Interval = 100; // in 1ms
125		125
126		126
127		127
128	static u16 ptr_txd_buffer = 0;	128	static u16 ptr_txd_buffer = 0;
129		129
130	const u8 ANALOG_LABEL[32][16] =	130	const u8 ANALOG_LABEL[32][16] =
131	{	131	{
132	//1234567890123456	132	//1234567890123456
133	"AngleNick ", //0	133	"AngleNick ", //0
134	"AngleRoll ",	134	"AngleRoll ",
135	"AccNick ",	135	"AccNick ",
136	"AccRoll ",	136	"AccRoll ",
137	" ",	137	" ",
138	" ", //5	138	" ", //5
139	" ",	139	" ",
140	" ",	140	" ",
141	" ",	141	" ",
142	" ",	142	" ",
143	" ", //10	143	" ", //10
144	"GPS Data ",	144	"GPS Data ",
145	"SPI Error ",	145	"SPI Error ",
146	"SPI Okay ",	146	"SPI Okay ",
147	"I2C Error ",	147	"I2C Error ",
148	"I2C Okay ", //15	148	"I2C Okay ", //15
149	" ",// "FC_Kalman_K ",	149	" ",// "FC_Kalman_K ",
150	"ACC_Speed_N ",	150	"ACC_Speed_N ",
151	"ACC_Speed_E ",	151	"ACC_Speed_E ",
152	" ",// "GPS ACC ",	152	" ",// "GPS ACC ",
153	" ",// "MAXDrift ", //20	153	" ",// "MAXDrift ", //20
154	"N_Speed ",	154	"N_Speed ",
155	"E_Speed ",	155	"E_Speed ",
156	" ",// "KalmDist_N ",	156	" ",// "KalmDist_N ",
157	" ",// "KalmDist_E ",	157	" ",// "KalmDist_E ",
158	"GyroHeading ",//25	158	"GyroHeading ",//25
159	"CompassHeading ",	159	"CompassHeading ",
160	"Distance N ",	160	"Distance N ",
161	"Distance E ",	161	"Distance E ",
162	"GPS_Nick ",	162	"GPS_Nick ",
163	"GPS_Roll ", //30	163	"GPS_Roll ", //30
164	"Used_Sats "	164	"Used_Sats "
165	};	165	};
166		166
167		167
168	/********************************************************/	168	/********************************************************/
169	/* Initialization the UART1 */	169	/* Initialization the UART1 */
170	/********************************************************/	170	/********************************************************/
171	void UART1_Init (void)	171	void UART1_Init (void)
172	{	172	{
173	GPIO_InitTypeDef GPIO_InitStructure;	173	GPIO_InitTypeDef GPIO_InitStructure;
174	UART_InitTypeDef UART_InitStructure;	174	UART_InitTypeDef UART_InitStructure;
175		175
176	SCU_APBPeriphClockConfig(__UART1, ENABLE); // Enable the UART1 Clock	176	SCU_APBPeriphClockConfig(__UART1, ENABLE); // Enable the UART1 Clock
177	SCU_APBPeriphClockConfig(__GPIO3, ENABLE); // Enable the GPIO3 Clock	177	SCU_APBPeriphClockConfig(__GPIO3, ENABLE); // Enable the GPIO3 Clock
178		178
179	/Configure UART1_Rx pin GPIO3.2/	179	/Configure UART1_Rx pin GPIO3.2/
180	GPIO_StructInit(&GPIO_InitStructure);	180	GPIO_StructInit(&GPIO_InitStructure);
181	GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;	181	GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
182	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;	182	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
183	GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull;	183	GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull;
184	GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;	184	GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;
185	GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1; // UART1_RxD	185	GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1; // UART1_RxD
186	GPIO_Init(GPIO3, &GPIO_InitStructure);	186	GPIO_Init(GPIO3, &GPIO_InitStructure);
187		187
188	/Configure UART1_Tx pin GPIO3.3/	188	/Configure UART1_Tx pin GPIO3.3/
189	GPIO_StructInit(&GPIO_InitStructure);	189	GPIO_StructInit(&GPIO_InitStructure);
190	GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;	190	GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
191	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;	191	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
192	GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull;	192	GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull;
193	GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2; // UART1_TX	193	GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2; // UART1_TX
194	GPIO_Init(GPIO3, &GPIO_InitStructure);	194	GPIO_Init(GPIO3, &GPIO_InitStructure);
195		195
196	/* UART1 configured as follow:	196	/* UART1 configured as follow:
197	- Word Length = 8 Bits	197	- Word Length = 8 Bits
198	- One Stop Bit	198	- One Stop Bit
199	- No parity	199	- No parity
200	- BaudRate = 57600 baud	200	- BaudRate = 57600 baud
201	- Hardware flow control Disabled	201	- Hardware flow control Disabled
202	- Receive and transmit enabled	202	- Receive and transmit enabled
203	- Receive and transmit FIFOs are Disabled	203	- Receive and transmit FIFOs are Disabled
204	*/	204	*/
205	UART_StructInit(&UART_InitStructure);	205	UART_StructInit(&UART_InitStructure);
206	UART_InitStructure.UART_WordLength = UART_WordLength_8D;	206	UART_InitStructure.UART_WordLength = UART_WordLength_8D;
207	UART_InitStructure.UART_StopBits = UART_StopBits_1;	207	UART_InitStructure.UART_StopBits = UART_StopBits_1;
208	UART_InitStructure.UART_Parity = UART_Parity_No ;	208	UART_InitStructure.UART_Parity = UART_Parity_No ;
209	UART_InitStructure.UART_BaudRate = BAUD_RATE;	209	UART_InitStructure.UART_BaudRate = BAUD_RATE;
210	UART_InitStructure. UART_HardwareFlowControl = UART_HardwareFlowControl_None;	210	UART_InitStructure. UART_HardwareFlowControl = UART_HardwareFlowControl_None;
211	UART_InitStructure.UART_Mode = UART_Mode_Tx_Rx;	211	UART_InitStructure.UART_Mode = UART_Mode_Tx_Rx;
212	UART_InitStructure.UART_FIFO = UART_FIFO_Enable;	212	UART_InitStructure.UART_FIFO = UART_FIFO_Enable;
213	UART_InitStructure.UART_TxFIFOLevel = UART_FIFOLevel_1_2;	213	UART_InitStructure.UART_TxFIFOLevel = UART_FIFOLevel_1_2;
214	UART_InitStructure.UART_RxFIFOLevel = UART_FIFOLevel_1_2;	214	UART_InitStructure.UART_RxFIFOLevel = UART_FIFOLevel_1_2;
215		215
216	UART_DeInit(UART1); // reset uart 1 to default	216	UART_DeInit(UART1); // reset uart 1 to default
217	UART_Init(UART1, &UART_InitStructure); // initialize uart 1	217	UART_Init(UART1, &UART_InitStructure); // initialize uart 1
218	// enable uart 1 interrupts selective	218	// enable uart 1 interrupts selective
219	UART_ITConfig(UART1, UART_IT_Receive \| UART_IT_ReceiveTimeOut, ENABLE);	219	UART_ITConfig(UART1, UART_IT_Receive \| UART_IT_ReceiveTimeOut, ENABLE);
220	UART_Cmd(UART1, ENABLE); // enable uart 1	220	UART_Cmd(UART1, ENABLE); // enable uart 1
221	// configure the uart 1 interupt line as an IRQ with priority 4 (0 is highest)	221	// configure the uart 1 interupt line as an IRQ with priority 4 (0 is highest)
222	VIC_Config(UART1_ITLine, VIC_IRQ, 4);	222	VIC_Config(UART1_ITLine, VIC_IRQ, 4);
223	// enable the uart 1 IRQ	223	// enable the uart 1 IRQ
224	VIC_ITCmd(UART1_ITLine, ENABLE);	224	VIC_ITCmd(UART1_ITLine, ENABLE);
225	// initialize the debug timer	225	// initialize the debug timer
226	DebugData_Timer = SetDelay(DebugData_Interval);	226	DebugData_Timer = SetDelay(DebugData_Interval);
227	NaviData_Timer = SetDelay(NaviData_Interval+500);	227	NaviData_Timer = SetDelay(NaviData_Interval+500);
228	// unlock rxd_buffer	228	// unlock rxd_buffer
229	rxd_buffer_locked = FALSE;	229	rxd_buffer_locked = FALSE;
230	pRxData = NULL;	230	pRxData = NULL;
231	RxDataLen = 0;	231	RxDataLen = 0;
232	// no bytes to send	232	// no bytes to send
233	txd_complete = TRUE;	233	txd_complete = TRUE;
234	// Fill Version Info Structure	234	// Fill Version Info Structure
235	UART_VersionInfo.SWMajor = VERSION_MAJOR;	235	UART_VersionInfo.SWMajor = VERSION_MAJOR;
236	UART_VersionInfo.SWMinor = VERSION_MINOR;	236	UART_VersionInfo.SWMinor = VERSION_MINOR;
237	UART_VersionInfo.SWPatch = VERSION_PATCH;	237	UART_VersionInfo.SWPatch = VERSION_PATCH;
238	UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;	238	UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;
239	UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;	239	UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;
240		240
241	SerialPutString("\r\nUART1 init...ok");	241	SerialPutString("\r\nUART1 init...ok");
242	}	242	}
243		243
244		244
245	/****************************************************************/	245	/****************************************************************/
246	/* USART1 receiver ISR */	246	/* USART1 receiver ISR */
247	/****************************************************************/	247	/****************************************************************/
248	void UART1_IRQHandler(void)	248	void UART1_IRQHandler(void)
249	{	249	{
250	static u16 crc;	250	static u16 crc;
251	static u8 ptr_rxd_buffer = 0;	251	static u8 ptr_rxd_buffer = 0;
252	static u8 crc1, crc2;	252	static u8 crc1, crc2;
253	static u8 abortState = 0;	253	static u8 abortState = 0;
254	u8 c;	254	u8 c;
255		255
256	if((UART_GetITStatus(UART1, UART_IT_Receive) != RESET) \|\| (UART_GetITStatus(UART1, UART_IT_ReceiveTimeOut) != RESET) )	256	if((UART_GetITStatus(UART1, UART_IT_Receive) != RESET) \|\| (UART_GetITStatus(UART1, UART_IT_ReceiveTimeOut) != RESET) )
257	{	257	{
258	// clear the pending bits	258	// clear the pending bits
259	UART_ClearITPendingBit(UART1, UART_IT_Receive);	259	UART_ClearITPendingBit(UART1, UART_IT_Receive);
260	UART_ClearITPendingBit(UART1, UART_IT_ReceiveTimeOut);	260	UART_ClearITPendingBit(UART1, UART_IT_ReceiveTimeOut);
261	// if debug UART is not UART1	261	// if debug UART is not UART1
262	if (DebugUART != UART1)	262	if (DebugUART != UART1)
263	{ // forward received data to the debug UART tx buffer	263	{ // forward received data to the debug UART tx buffer
264	while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)	264	while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)
265	{	265	{
266	// wait for space in the tx buffer of the DebugUART	266	// wait for space in the tx buffer of the DebugUART
267	while(UART_GetFlagStatus(DebugUART, UART_FLAG_TxFIFOFull) == SET) {};	267	while(UART_GetFlagStatus(DebugUART, UART_FLAG_TxFIFOFull) == SET) {};
268	// move the byte from the rx buffer of UART1 to the tx buffer of DebugUART	268	// move the byte from the rx buffer of UART1 to the tx buffer of DebugUART
269	c = UART_ReceiveData(UART1);	269	c = UART_ReceiveData(UART1);
270		270
271	// check for abort condition (ESC ESC 0x55 0xAA 0x00)	271	// check for abort condition (ESC ESC 0x55 0xAA 0x00)
272	switch (abortState)	272	switch (abortState)
273	{	273	{
274	case 0: if (c == 27) abortState++;	274	case 0: if (c == 27) abortState++;
275	break;	275	break;
276	case 1: if (c == 27) abortState++; else abortState = 0;	276	case 1: if (c == 27) abortState++; else abortState = 0;
277	break;	277	break;
278	case 2: if (c == 0x55) abortState++; else abortState = 0;	278	case 2: if (c == 0x55) abortState++; else abortState = 0;
279	break;	279	break;
280	case 3: if (c == 0xAA) abortState++; else abortState = 0;	280	case 3: if (c == 0xAA) abortState++; else abortState = 0;
281	break;	281	break;
282	case 4: if (c == 0x00)	282	case 4: if (c == 0x00)
283	{	283	{
284	DebugUART = UART1;	284	DebugUART = UART1;
285	UART0_Connect_to_MKGPS();	285	UART0_Connect_to_MKGPS();
286	}	286	}
287	abortState = 0;	287	abortState = 0;
288	break;	288	break;
289	}	289	}
290		290
291	if (DebugUART != UART1) UART_SendData(DebugUART, c);	291	if (DebugUART != UART1) UART_SendData(DebugUART, c);
292	}	292	}
293	}	293	}
294	else // DebugUART == UART1 (normal operation)	294	else // DebugUART == UART1 (normal operation)
295	{	295	{
296	while ((UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET) && (!rxd_buffer_locked))	296	while ((UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET) && (!rxd_buffer_locked))
297	{ // some byes in the fifo and rxd buffer not locked	297	{ // some byes in the fifo and rxd buffer not locked
298	// get byte from fifo	298	// get byte from fifo
299	c = UART_ReceiveData(UART1);	299	c = UART_ReceiveData(UART1);
300	if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received	300	if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received
301	{	301	{
302	rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer	302	rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
303	crc = c; // init crc	303	crc = c; // init crc
304	}	304	}
305	#if 0	305	#if 0
306	else if (ptr_rxd_buffer == 1) // handle address	306	else if (ptr_rxd_buffer == 1) // handle address
307	{	307	{
308	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer	308	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
309	crc += c; // update crc	309	crc += c; // update crc
310	}	310	}
311	#endif	311	#endif
312	else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // rxd buffer not full	312	else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // rxd buffer not full
313	{	313	{
314	if (c != '\r') // no termination character received	314	if (c != '\r') // no termination character received
315	{	315	{
316	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer	316	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
317	crc += c; // update crc	317	crc += c; // update crc
318	}	318	}
319	else // termination character received	319	else // termination character received
320	{	320	{
321	// the last 2 bytes are no subject for checksum calculation	321	// the last 2 bytes are no subject for checksum calculation
322	// they are the checksum itself	322	// they are the checksum itself
323	crc -= rxd_buffer[ptr_rxd_buffer-2];	323	crc -= rxd_buffer[ptr_rxd_buffer-2];
324	crc -= rxd_buffer[ptr_rxd_buffer-1];	324	crc -= rxd_buffer[ptr_rxd_buffer-1];
325	// calculate checksum from transmitted data	325	// calculate checksum from transmitted data
326	crc %= 4096;	326	crc %= 4096;
327	crc1 = '=' + crc / 64;	327	crc1 = '=' + crc / 64;
328	crc2 = '=' + crc % 64;	328	crc2 = '=' + crc % 64;
329	// compare checksum to transmitted checksum bytes	329	// compare checksum to transmitted checksum bytes
330	if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))	330	if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))
331	{ // checksum valid	331	{ // checksum valid
332	rxd_buffer_locked = TRUE; // lock the rxd buffer	332	rxd_buffer_locked = TRUE; // lock the rxd buffer
333	ReceivedBytes = ptr_rxd_buffer + 1; // store number of received bytes	333	ReceivedBytes = ptr_rxd_buffer + 1; // store number of received bytes
334	rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character	334	rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
335	// if 2nd byte is an 'R' start bootloader	335	// if 2nd byte is an 'R' start bootloader
336	if(rxd_buffer[2] == 'R')	336	if(rxd_buffer[2] == 'R')
337	{	337	{
338	PowerOff();	338	PowerOff();
339	VIC_DeInit();	339	VIC_DeInit();
340	Execute_Bootloader(); // Reset-Commando - Bootloader starten	340	Execute_Bootloader(); // Reset-Commando - Bootloader starten
341	}	341	}
342	} // eof checksum valid	342	} // eof checksum valid
343	else	343	else
344	{ // checksum invalid	344	{ // checksum invalid
345	rxd_buffer_locked = FALSE; // unlock rxd buffer	345	rxd_buffer_locked = FALSE; // unlock rxd buffer
346	} // eof checksum invalid	346	} // eof checksum invalid
347	ptr_rxd_buffer = 0; // reset rxd buffer pointer	347	ptr_rxd_buffer = 0; // reset rxd buffer pointer
348	} // eof termination character received	348	} // eof termination character received
349	} // rxd buffer not full	349	} // rxd buffer not full
350	else // rxd buffer overrun	350	else // rxd buffer overrun
351	{	351	{
352	ptr_rxd_buffer = 0; // reset rxd buffer pointer	352	ptr_rxd_buffer = 0; // reset rxd buffer pointer
353	rxd_buffer_locked = FALSE; // unlock rxd buffer	353	rxd_buffer_locked = FALSE; // unlock rxd buffer
354	} // eof rxd buffer overrrun	354	} // eof rxd buffer overrrun
355	} // some byes in the fifo and rxd buffer not locked	355	} // some byes in the fifo and rxd buffer not locked
356	} // eof DebugUart = UART1	356	} // eof DebugUart = UART1
357	}	357	}
358	}	358	}
359		359
360	/**************************************************************/	360	/**************************************************************/
361	/* Transmit tx buffer via debug uart */	361	/* Transmit tx buffer via debug uart */
362	/**************************************************************/	362	/**************************************************************/
363	void UART1_Transmit(void)	363	void UART1_Transmit(void)
364	{	364	{
365	u8 tmp_tx;	365	u8 tmp_tx;
366	// if something has to be send and the txd fifo is not full	366	// if something has to be send and the txd fifo is not full
367	if((!txd_complete) && (UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) == RESET))	367	if((!txd_complete) && (UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) == RESET))
368	{	368	{
369	tmp_tx = txd_buffer[ptr_txd_buffer]; // read byte from txd buffer	369	tmp_tx = txd_buffer[ptr_txd_buffer]; // read byte from txd buffer
370	// if terminating character or end of txd buffer reached	370	// if terminating character or end of txd buffer reached
371	if((tmp_tx == '\r') \|\| (ptr_txd_buffer == TXD_BUFFER_LEN))	371	if((tmp_tx == '\r') \|\| (ptr_txd_buffer == TXD_BUFFER_LEN))
372	{	372	{
373	ptr_txd_buffer = 0; // reset txd buffer pointer	373	ptr_txd_buffer = 0; // reset txd buffer pointer
374	txd_complete = TRUE;// set complete flag	374	txd_complete = TRUE;// set complete flag
375	}	375	}
376	UART_SendData(UART1, tmp_tx); // put character to txd fifo	376	UART_SendData(UART1, tmp_tx); // put character to txd fifo
377	// set pointer to next byte	377	// set pointer to next byte
378	ptr_txd_buffer++;	378	ptr_txd_buffer++;
379	}	379	}
380	}	380	}
381		381
382	/**************************************************************/	382	/**************************************************************/
383	/* Add CRC and initiate transmission via debug uart */	383	/* Add CRC and initiate transmission via debug uart */
384	/**************************************************************/	384	/**************************************************************/
385	void AddCRC(u16 datalen)	385	void AddCRC(u16 datalen)
386	{	386	{
387	u16 tmpCRC = 0, i;	387	u16 tmpCRC = 0, i;
388	for(i = 0; i < datalen; i++)	388	for(i = 0; i < datalen; i++)
389	{	389	{
390	tmpCRC += txd_buffer[i];	390	tmpCRC += txd_buffer[i];
391	}	391	}
392	tmpCRC %= 4096;	392	tmpCRC %= 4096;
393	txd_buffer[i++] = '=' + tmpCRC / 64;	393	txd_buffer[i++] = '=' + tmpCRC / 64;
394	txd_buffer[i++] = '=' + tmpCRC % 64;	394	txd_buffer[i++] = '=' + tmpCRC % 64;
395	txd_buffer[i++] = '\r';	395	txd_buffer[i++] = '\r';
396		396
397	ptr_txd_buffer = 0;	397	ptr_txd_buffer = 0;
398	txd_complete = FALSE;	398	txd_complete = FALSE;
399	UART_SendData(UART1,txd_buffer[ptr_txd_buffer++]); // send first byte, to be continued in the txd irq	399	UART_SendData(UART1, txd_buffer[ptr_txd_buffer++]); // send first byte, to be continued in the txd irq
400	}	400	}
401		401
402		402
403		403
404	/**************************************************************/	404	/**************************************************************/
405	/* Code output data */	405	/* Code output data */
406	/**************************************************************/	406	/**************************************************************/
407	void SendOutData(u8 cmd, u8 Address, u8 numofbuffers , ...) //u8 *data, u8 len, ....	407	void SendOutData(u8 cmd, u8 Address, u8 numofbuffers , ...) //u8 *data, u8 len, ....
408	{	408	{
409	va_list ap;	409	va_list ap;
410		410
411	u16 pt = 0;	411	u16 pt = 0;
412	u8 a,b,c;	412	u8 a,b,c;
413	u8 ptr = 0;	413	u8 ptr = 0;
414		414
415	u8* pdata = NULL;	415	u8* pdata = NULL;
416	int len = 0;	416	int len = 0;
417		417
418	txd_buffer[pt++] = '#'; // Start character	418	txd_buffer[pt++] = '#'; // Start character
419	txd_buffer[pt++] = 'a' + Address; // Address (a=0; b=1,...)	419	txd_buffer[pt++] = 'a' + Address; // Address (a=0; b=1,...)
420	txd_buffer[pt++] = cmd; // Command	420	txd_buffer[pt++] = cmd; // Command
421		421
422	va_start(ap, numofbuffers);	422	va_start(ap, numofbuffers);
423	if(numofbuffers)	423	if(numofbuffers)
424	{	424	{
425	pdata = va_arg(ap, u8*);	425	pdata = va_arg(ap, u8*);
426	len = va_arg(ap, int);	426	len = va_arg(ap, int);
427	ptr = 0;	427	ptr = 0;
428	numofbuffers--;	428	numofbuffers--;
429	}	429	}
430	while(len)	430	while(len)
431	{	431	{
432	if(len)	432	if(len)
433	{	433	{
434	a = pdata[ptr++];	434	a = pdata[ptr++];
435	len--;	435	len--;
436	if((!len) && numofbuffers) // try to jump to next buffer	436	if((!len) && numofbuffers) // try to jump to next buffer
437	{	437	{
438	pdata = va_arg(ap, u8*);	438	pdata = va_arg(ap, u8*);
439	len = va_arg(ap, int);	439	len = va_arg(ap, int);
440	ptr = 0;	440	ptr = 0;
441	numofbuffers--;	441	numofbuffers--;
442	}	442	}
443	}	443	}
444	else a = 0;	444	else a = 0;
445	if(len)	445	if(len)
446	{	446	{
447	b = pdata[ptr++];	447	b = pdata[ptr++];
448	len--;	448	len--;
449	if((!len) && numofbuffers) // try to jump to next buffer	449	if((!len) && numofbuffers) // try to jump to next buffer
450	{	450	{
451	pdata = va_arg(ap, u8*);	451	pdata = va_arg(ap, u8*);
452	len = va_arg(ap, int);	452	len = va_arg(ap, int);
453	ptr = 0;	453	ptr = 0;
454	numofbuffers--;	454	numofbuffers--;
455	}	455	}
456	}	456	}
457	else b = 0;	457	else b = 0;
458	if(len)	458	if(len)
459	{	459	{
460	c = pdata[ptr++];	460	c = pdata[ptr++];
461	len--;	461	len--;
462	if((!len) && numofbuffers) // try to jump to next buffer	462	if((!len) && numofbuffers) // try to jump to next buffer
463	{	463	{
464	pdata = va_arg(ap, u8*);	464	pdata = va_arg(ap, u8*);
465	len = va_arg(ap, int);	465	len = va_arg(ap, int);
466	ptr = 0;	466	ptr = 0;
467	numofbuffers--;	467	numofbuffers--;
468	}	468	}
469	}	469	}
470	else c = 0;	470	else c = 0;
471	txd_buffer[pt++] = '=' + (a >> 2);	471	txd_buffer[pt++] = '=' + (a >> 2);
472	txd_buffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));	472	txd_buffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));
473	txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));	473	txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));
474	txd_buffer[pt++] = '=' + ( c & 0x3f);	474	txd_buffer[pt++] = '=' + ( c & 0x3f);
475	}	475	}
476	va_end(ap);	476	va_end(ap);
477	AddCRC(pt); // add checksum after data block and initates the transmission	477	AddCRC(pt); // add checksum after data block and initates the transmission
478	}	478	}
479		479
480		480
481	/**************************************************************/	481	/**************************************************************/
482	/* Decode data */	482	/* Decode data */
483	/**************************************************************/	483	/**************************************************************/
484	void Decode64(void)	484	void Decode64(void)
485	{	485	{
486	u8 a,b,c,d;	486	u8 a,b,c,d;
487	u8 x,y,z;	487	u8 x,y,z;
488	u8 ptrIn = 3; // start with first data byte in rx buffer	488	u8 ptrIn = 3; // start with first data byte in rx buffer
489	u8 ptrOut = 3;	489	u8 ptrOut = 3;
490	u8 len = ReceivedBytes - 6; // must be a multiple of 4 (3 bytes at begin and 3 bytes at end are no payload )	490	u8 len = ReceivedBytes - 6; // must be a multiple of 4 (3 bytes at begin and 3 bytes at end are no payload )
491	while(len)	491	while(len)
492	{	492	{
493	a = rxd_buffer[ptrIn++] - '=';	493	a = rxd_buffer[ptrIn++] - '=';
494	b = rxd_buffer[ptrIn++] - '=';	494	b = rxd_buffer[ptrIn++] - '=';
495	c = rxd_buffer[ptrIn++] - '=';	495	c = rxd_buffer[ptrIn++] - '=';
496	d = rxd_buffer[ptrIn++] - '=';	496	d = rxd_buffer[ptrIn++] - '=';
497	//if(ptrIn > ReceivedBytes - 3) break;	497	//if(ptrIn > ReceivedBytes - 3) break;
498		498
499	x = (a << 2) \| (b >> 4);	499	x = (a << 2) \| (b >> 4);
500	y = ((b & 0x0f) << 4) \| (c >> 2);	500	y = ((b & 0x0f) << 4) \| (c >> 2);

Subversion Repositories NaviCtrl

(root)/tags/V2.10a/uart1.c @ 870 - Rev 41 → 49