WebSVN - NaviCtrl - Diff - Rev 89 and 90


/*#######################################################################################*/
/* !!! 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 Request_Data3D                    = 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;
Data3D_t Data3D;
 
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
        "                ",
        "                ",
        "                ",
        "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
        "SD-Logs         ",
        "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 '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'://  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
                // "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_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;
        }
 
}
 
 

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

Subversion Repositories NaviCtrl

(root)/tags/V0.24e/uart1.c @ 577 - Rev 89 → 90