WebSVN - NaviCtrl - Diff - Rev 163 and 165


/*#######################################################################################*/
/* !!! 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 "config.h"
#include "menu.h"
#include "printf_P.h"
#include "GPS.h"
#include "i2c.h"
#include "uart0.h"
#include "uart1.h"
#include "uart2.h"
#include "timer1.h"
#include "timer2.h"
#include "analog.h"
#include "main.h"
#include "waypoints.h"
#include "mkprotocol.h"
#include "params.h"
#include "fifo.h"
 
#define FALSE   0
#define TRUE    1
 
 
u8 UART1_Request_VersionInfo    = FALSE;
u8 UART1_Request_SendFollowMe   = FALSE;
u8 UART1_Request_ExternalControl= FALSE;
u8 UART1_Request_Display                = FALSE;
u8 UART1_Request_Display1               = FALSE;
u8 UART1_Request_DebugData              = FALSE;
u8 UART1_Request_DebugLabel             = 255;
u8 UART1_Request_NaviData               = FALSE;
u8 UART1_Request_ErrorMessage   = FALSE;
u8 UART1_Request_NewWaypoint    = FALSE;
u8 UART1_Request_ReadWaypoint   = 255;
u8 UART1_Request_Data3D             = FALSE;
u8 UART1_Request_Echo               = FALSE;
u8 UART1_Request_ParameterId    = 0;
u8 UART1_Request_Parameter              = FALSE;
u8 UART1_DisplayLine                    = 0;
u8 UART1_ConfirmFrame                   = 0;
 
UART_TypeDef *DebugUART = UART1;
 
// the primary rx fifo
#define UART1_RX_FIFO_LEN 1024
u8 UART1_rxfifobuffer[UART1_RX_FIFO_LEN];
fifo_t UART1_rx_fifo;
 
// the rx buffer
#define UART1_RX_BUFFER_LEN  150
u8 UART1_rbuffer[UART1_RX_BUFFER_LEN];
Buffer_t UART1_rx_buffer;
 
// the tx buffer
#define UART1_TX_BUFFER_LEN  150
u8 UART1_tbuffer[UART1_TX_BUFFER_LEN];
Buffer_t UART1_tx_buffer;
 
 
 
volatile u8 SerialLinkOkay = 0;
 
u8 text[200];
 
const u8 ANALOG_LABEL[32][16] =
{
   //1234567890123456
        "AngleNick       ", //0
        "AngleRoll       ",
        "AccNick         ",
        "AccRoll         ",
        "                ",
        "MK-Flags        ", //5
        "NC-Flags        ",
        "NickServo       ",
        "RollServo       ",
        "GPS Data        ",
        "CompassHeading  ", //10
        "GyroHeading     ",
        "SPI Error       ",
        "SPI Okay        ",
        "I2C Error       ",
        "I2C Okay        ", //15
        "                ",//    "Kalman_K        ",
        "ACC_Speed_N     ",
        "ACC_Speed_E     ",
        "Speed_z         ",//    "GPS ACC         ",
        "                ",//    "MAXDrift        ", //20
        "N_Speed         ",
        "E_Speed         ",
        "P-Part          ",
        "I-Part          ",
        "D-Part          ",//25
        "PID-Part        ",
        "Distance N      ",
        "Distance E      ",
        "GPS_Nick        ",
        "GPS_Roll        ", //30
        "Used_Sats       "
};
 
DebugOut_t DebugOut;
ExternControl_t ExternControl;
UART_VersionInfo_t UART_VersionInfo;
NaviData_t NaviData;
Waypoint_t FollowMe;
Data3D_t Data3D;
u16 Echo; // 2 bytes recieved will be sent back as echo
 
u32 UART1_DebugData_Timer;
u32 UART1_DebugData_Interval = 5000;    // in ms
u32 UART1_NaviData_Timer;
u32 UART1_NaviData_Interval = 5000;     // in ms
u32 UART1_Data3D_Timer = 0;                     // in ms
u32 UART1_Data3D_Interval = 0;
 
/********************************************************/
/*            Initialization the UART1                  */
/********************************************************/
void UART1_Init (void)
{
        GPIO_InitTypeDef GPIO_InitStructure;
        UART_InitTypeDef UART_InitStructure;
 
        // initialize txd buffer
        Buffer_Init(&UART1_tx_buffer, UART1_tbuffer, UART1_TX_BUFFER_LEN);
 
        // initialize rxd buffer
        Buffer_Init(&UART1_rx_buffer, UART1_rbuffer, UART1_RX_BUFFER_LEN);
 
        // initialize the rx fifo
        fifo_init(&UART1_rx_fifo, UART1_rxfifobuffer, UART1_RX_FIFO_LEN);
 
        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 =                              UART1_BAUD_RATE;
        UART_InitStructure. UART_HardwareFlowControl =  UART_HardwareFlowControl_None;
        UART_InitStructure.UART_Mode =                                  UART_Mode_Tx_Rx;
        UART_InitStructure.UART_FIFO =                                  UART_FIFO_Enable;
        UART_InitStructure.UART_TxFIFOLevel =                   UART_FIFOLevel_1_2;
        UART_InitStructure.UART_RxFIFOLevel =                   UART_FIFOLevel_1_2;
 
        UART_DeInit(UART1); // reset uart 1     to default
        UART_Init(UART1, &UART_InitStructure); // initialize uart 1
        // enable uart 1 interrupts selective
        UART_ITConfig(UART1, UART_IT_Receive | UART_IT_ReceiveTimeOut, ENABLE);
        UART_Cmd(UART1, ENABLE); // enable uart 1
        // configure the uart 1 interupt line
        VIC_Config(UART1_ITLine, VIC_IRQ, PRIORITY_UART1);
                // enable the uart 1 IRQ
        VIC_ITCmd(UART1_ITLine, ENABLE);
 
        // initialize the debug timer
        UART1_DebugData_Timer = SetDelay(UART1_DebugData_Interval);
        UART1_NaviData_Timer = SetDelay(UART1_NaviData_Interval)+500;
 
        // 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;
 
        UART1_PutString("\r\nUART1 init...ok");
}
 
 
/****************************************************************/
/*               USART1 receiver ISR                            */
/****************************************************************/
void UART1_IRQHandler(void)
{
        static u8 abortState = 0;
        u8 c;
 
        if((UART_GetITStatus(UART1, UART_IT_Receive) != RESET) || (UART_GetITStatus(UART1, UART_IT_ReceiveTimeOut) != RESET) )
        {
                // clear the pending bits!
                UART_ClearITPendingBit(UART1, UART_IT_Receive);
                UART_ClearITPendingBit(UART1, UART_IT_ReceiveTimeOut);
                // if debug UART is not UART1
                if (DebugUART != UART1)
                {       // forward received data to the debug UART tx buffer
                        while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)
                        {
                                // move the byte from the rx buffer of UART1 to the tx buffer of DebugUART
                                c = UART_ReceiveData(UART1);
 
                                // check for abort condition (ESC ESC 0x55 0xAA 0x00)
                                switch (abortState)
                                {
                                        case 0:
                                                if (c == 27) abortState++;
                                                break;
                                        case 1:
                                                if (c == 27) abortState++;
                                                else abortState = 0;
                                        break;
                                        case 2:
                                                if (c == 0x55) abortState++;
                                                else abortState = 0;
                                                break;
                                        case 3:
                                                if (c == 0xAA) abortState++;
                                                else abortState = 0;
                                                break;
                                        case 4:
                                                if (c == 0x00)
                                                {
                                                        if(DebugUART == UART0)
                                                        {
                                                                UART0_Connect_to_MKGPS();
                                                                TIMER2_Init();                          // enbable servo outputs
                                                                fifo_purge(&UART1_rx_fifo); // flush the whole fifo init buffer
                                                        }
                                                        DebugUART = UART1;
                                                }
                                                abortState = 0;
                                                break;
                                } // end switch abort state
                                // if the Debug uart is not UART1, redirect input to the Debug UART
                                if (DebugUART != UART1)
                                {
                                        // wait for space in the tx buffer of the DebugUART
                                        while(UART_GetFlagStatus(DebugUART, UART_FLAG_TxFIFOFull) == SET) {};
                                        // move byte to the tx fifo of the debug uart
                                        UART_SendData(DebugUART, c);
                                }
                        }
                }
                else  // DebugUART == UART1 (normal operation)
                {
                        while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)
                        { // some byes in the hardware fifo
                            // get byte from hardware fifo
                        c = UART_ReceiveData(UART1);
                                // put into the software fifo
                                if(!fifo_put(&UART1_rx_fifo, c))
                                {       // fifo overflow
                                        //fifo_purge(&UART1_rx_fifo); // flush the whole buffer
                                }
                        } // EOF while some byes in the hardware fifo
                } // eof DebugUart = UART1
        }
}
 
/**************************************************************/
/* Process incomming data from debug uart                     */
/**************************************************************/
void UART1_ProcessRxData(void)
{
        // return on forwarding uart  or unlocked rx buffer
        if(DebugUART != UART1) return;
 
        u8 c;
        // if rx buffer is not locked
        if(UART1_rx_buffer.Locked == FALSE)
        {   //collect data from primary rx fifo
                while(fifo_get(&UART1_rx_fifo, &c))
                {       // break if complete frame is collected
                        if(MKProtocol_CollectSerialFrame(&UART1_rx_buffer, c)) break;
                }
        }
        if(UART1_rx_buffer.Locked == FALSE) return;
 
        Waypoint_t * pWaypoint = NULL;
        SerialMsg_t SerialMsg;
 
        MKProtocol_DecodeSerialFrame(&UART1_rx_buffer, &SerialMsg); // decode serial frame in rxd buffer
    if(*(SerialMsg.pCmdID) != 'z') SerialLinkOkay = 250;          // reset SerialTimeout, but not in case of the "ping"
        switch(*(SerialMsg.pAddress)) // check for Slave Address
        {
                case NC_ADDRESS:  // own Slave Address
                switch(*(SerialMsg.pCmdID))
                {
                        case 'z': // connection checker
                                memcpy(&Echo, SerialMsg.pData, sizeof(Echo)); // copy echo pattern
                                UART1_Request_Echo = TRUE;
                                break;
 
                        case 'e': // request for the text of the error status
                                UART1_Request_ErrorMessage = TRUE;
                                break;
 
                        case 's'://  new target position
                                pWaypoint = (Waypoint_t*)SerialMsg.pData;
                                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(SerialMsg.pData[0])
                                {
                                        case UART_FLIGHTCTRL:
                                                UART2_Init();                           // initialize UART2 to FC pins
                                                fifo_purge(&UART1_rx_fifo);
                                                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;
                                                fifo_purge(&UART1_rx_fifo);
                                                DebugUART = UART0;
                                                break;
                                        case UART_MKGPS:
                                                if(FC.MKFlags & MKFLAG_MOTOR_RUN) break; // not if the motors are running
                                                TIMER2_Deinit();
                                                UART0_Connect_to_MKGPS();       // connect UART0 to MKGPS pins
                                                GPSData.Status = INVALID;
                                                fifo_purge(&UART1_rx_fifo);
                                                DebugUART = UART0;
                                                break;
                                        default:
                                                break;
                                }
                                break;
 
                        case 'w'://  Append Waypoint to List
                                {
                                        static u8 oldIndex = 0x00;
 
                                        pWaypoint = (Waypoint_t*)SerialMsg.pData;
                                        if(pWaypoint->Position.Status == INVALID)
                                        {  // clear WP List
                                                WPList_Clear();
                                                oldIndex = 0x00;
                                                GPS_pWaypoint = WPList_Begin();
 
                                                UART1_Request_NewWaypoint = TRUE;
                                        }
                                        else if (pWaypoint->Position.Status == NEWDATA)
                                        {  // app current WP to the list
 
 
 
                                                 if (pWaypoint->Index == oldIndex + 1)
                                                {
                                                        WPList_Append(pWaypoint);
                                                        BeepTime = 500;
                                                        oldIndex = pWaypoint->Index;
                                                        UART1_Request_NewWaypoint = TRUE;
                                                }
 
                                        }
                                }
 
                                break;
 
                        case 'x'://  Read Waypoint from List
                                UART1_Request_ReadWaypoint = SerialMsg.pData[0];
                                break;
 
                        case 'j':// Set/Get NC-Parameter
                                switch(SerialMsg.pData[0])
                                {
                                        case 0: // get
                                        break;
 
                                        case 1: // set
                                        {
                                                s16 value;
                                                value = SerialMsg.pData[2] + (s16)SerialMsg.pData[3] * 0x0100;
                                                NCParams_SetValue(SerialMsg.pData[1], &value);
                                        }
                                        break;
 
                                        default:
                                        break;
                                }
                                UART1_Request_ParameterId = SerialMsg.pData[1];
                                UART1_Request_Parameter = TRUE;
                                break;
                        default:
                                // unsupported command recieved
                                break;
                } // case NC_ADDRESS
                // "break;" is missing here to fall thru to the common commands
 
                default:  // and any other Slave Address
 
                switch(*(SerialMsg.pCmdID)) // check CmdID
                {
                        case 'a':// request for the labels of the analog debug outputs
                                UART1_Request_DebugLabel = SerialMsg.pData[0];
                                if(UART1_Request_DebugLabel > 31) UART1_Request_DebugLabel = 31;
                                break;
 
                        case 'b': // submit extern control
                                memcpy(&ExternControl, SerialMsg.pData, sizeof(ExternControl));
                                UART1_ConfirmFrame = ExternControl.Frame;
                                break;
 
                        case 'd': // request for debug data;
                                UART1_DebugData_Interval = (u32) SerialMsg.pData[0] * 10;
                                if(UART1_DebugData_Interval > 0) UART1_Request_DebugData = TRUE;
                                break;
 
                        case 'c': // request for 3D data;
                                UART1_Data3D_Interval = (u32) SerialMsg.pData[0] * 10;
                                if(UART1_Data3D_Interval > 0) UART1_Request_Data3D = TRUE;
                                break;
 
                        case 'g':// request for external control data
                                UART1_Request_ExternalControl = TRUE;
                                break;
 
                        case 'h':// reqest for display line
                                RemoteKeys |= SerialMsg.pData[0];
                                if(RemoteKeys != 0) UART1_DisplayLine = 0;
                                UART1_Request_Display = TRUE;
                                break;
 
                        case 'l':// reqest for display columns
                                MenuItem = SerialMsg.pData[0];
                                UART1_Request_Display1 = TRUE;
                                break;
 
                        case 'o': // request for navigation information
                                UART1_NaviData_Interval = (u32) SerialMsg.pData[0] * 10;
                                if(UART1_NaviData_Interval > 0) UART1_Request_NaviData = TRUE;
                                break;
 
                        case 'v': // request for version info
                                UART1_Request_VersionInfo = TRUE;
                                break;
                        default:
                                // unsupported command recieved
                                break;
                }
                break; // default:
        }
        Buffer_Clear(&UART1_rx_buffer); // free rc buffer for next frame
}
 
 
/*****************************************************/
/*                   Send a character                */
/*****************************************************/
s16 UART1_Putchar(char c)
{
        if (c == '\n') UART1_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 UART1_PutString(u8 *s)
{
        if(s == NULL) return;
        while (*s != '\0' && DebugUART == UART1)
        {
                UART1_Putchar(*s);
                s ++;
        }
}
 
 
/**************************************************************/
/*         Transmit tx buffer via debug uart                  */
/**************************************************************/
void UART1_Transmit(void)
{
        u8 tmp_tx;
        if(DebugUART != UART1) return;
        // if something has to be send and the txd fifo is not full
        if(UART1_tx_buffer.Locked == TRUE)
        {
                // while there is some space in the tx fifo
                while(UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != SET)
                {
                        tmp_tx = UART1_tx_buffer.pData[UART1_tx_buffer.Position++]; // read next byte from txd buffer
                        UART_SendData(UART1, tmp_tx); // put character to txd fifo
                        // if terminating character or end of txd buffer reached
                        if((tmp_tx == '\r') || (UART1_tx_buffer.Position == UART1_tx_buffer.DataBytes))
                        {
                                Buffer_Clear(&UART1_tx_buffer); // clear txd buffer
                                break; // end while loop
                        }
                }
        }
}
 
/**************************************************************/
/* Send the answers to incomming commands at the debug uart   */
/**************************************************************/
void UART1_TransmitTxData(void)
{
        if(DebugUART != UART1) return;
        UART1_Transmit(); // output pending bytes in tx buffer
        if((UART1_tx_buffer.Locked == TRUE)) return;
 
        if(UART1_Request_Parameter && (UART1_tx_buffer.Locked == FALSE))
        {
                s16 ParamValue;
                NCParams_GetValue(UART1_Request_ParameterId, &ParamValue);
                //sprintf(text, "\r\nId=%d, value = %d\r\n", UART1_Request_ParameterId, ParamValue);
                //UART1_PutString(text);
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'J', NC_ADDRESS, 2, &UART1_Request_ParameterId, sizeof(UART1_Request_ParameterId), &ParamValue, sizeof(ParamValue)); // answer the param request
                UART1_Request_Parameter = FALSE;
        }
        else if(UART1_Request_Echo && (UART1_tx_buffer.Locked == FALSE))
        {
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'Z', NC_ADDRESS, 1, &Echo, sizeof(Echo)); // answer the echo request
                Echo = 0; // reset echo value
                UART1_Request_Echo = FALSE;
        }
        else if(UART1_Request_NewWaypoint && (UART1_tx_buffer.Locked == FALSE))
        {
                u8 WPNumber = WPList_GetCount();
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'W', NC_ADDRESS, 1, &WPNumber, sizeof(WPNumber));
                UART1_Request_NewWaypoint = FALSE;
        }
        else if((UART1_Request_ReadWaypoint != 0xFF) && (UART1_tx_buffer.Locked == FALSE))
        {
                u8 WPNumber = WPList_GetCount();
                if (UART1_Request_ReadWaypoint < WPNumber)
                {
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'X', NC_ADDRESS, 3, &WPNumber, 1, &UART1_Request_ReadWaypoint, 1, WPList_GetAt(UART1_Request_ReadWaypoint), sizeof(Waypoint_t));
                }
                else
                {
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer,'X', NC_ADDRESS, 1, &WPNumber, sizeof(WPNumber));
                }
                UART1_Request_ReadWaypoint = 0xFF;
        }
        else if((UART1_Request_DebugLabel != 0xFF) && (UART1_tx_buffer.Locked == FALSE))
        {
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'A', NC_ADDRESS, 2, &UART1_Request_DebugLabel, sizeof(UART1_Request_DebugLabel), (u8 *) ANALOG_LABEL[UART1_Request_DebugLabel], 16);
                UART1_Request_DebugLabel = 0xFF;
        }
        else if(( ((UART1_NaviData_Interval > 0) && CheckDelay(UART1_NaviData_Timer) ) || UART1_Request_NaviData) && (UART1_tx_buffer.Locked == FALSE))
        {
                NaviData.Errorcode = ErrorCode;
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'O', NC_ADDRESS,1, (u8 *)&NaviData, sizeof(NaviData));
                UART1_NaviData_Timer = SetDelay(UART1_NaviData_Interval);
                UART1_Request_NaviData = FALSE;
        }
        else if( (( (UART1_DebugData_Interval > 0) && CheckDelay(UART1_DebugData_Timer)) || UART1_Request_DebugData) && (UART1_tx_buffer.Locked == FALSE))
        {
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'D', NC_ADDRESS, 1,(u8 *)&DebugOut, sizeof(DebugOut));
                UART1_DebugData_Timer = SetDelay(UART1_DebugData_Interval);
                UART1_Request_DebugData = FALSE;
        }
        else if((( (UART1_Data3D_Interval > 0) && CheckDelay(UART1_Data3D_Timer) ) || UART1_Request_Data3D) && (UART1_tx_buffer.Locked == FALSE))
        {
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'C', NC_ADDRESS, 1,(u8 *)&Data3D, sizeof(Data3D));
                UART1_Data3D_Timer = SetDelay(UART1_Data3D_Interval);
                UART1_Request_Data3D = FALSE;
        }
        else if(UART1_ConfirmFrame && (UART1_tx_buffer.Locked == FALSE))
        {
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'B', NC_ADDRESS, 1, &UART1_ConfirmFrame, sizeof(UART1_ConfirmFrame));
                UART1_ConfirmFrame = 0;
        }
        else if(UART1_Request_ExternalControl && (UART1_tx_buffer.Locked == FALSE))
        {
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'G', NC_ADDRESS, 1, (u8 *)&ExternControl, sizeof(ExternControl));
                UART1_Request_ExternalControl = FALSE;
        }
        else if(UART1_Request_Display && (UART1_tx_buffer.Locked == FALSE))
        {
                LCD_PrintMenu();
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'H', NC_ADDRESS, 2, &UART1_DisplayLine, sizeof(UART1_DisplayLine), (u8*)&DisplayBuff[UART1_DisplayLine * 20], 20);
                UART1_DisplayLine++;
                if(UART1_DisplayLine >= 4) UART1_DisplayLine = 0;
                UART1_Request_Display = FALSE;
        }
        else if(UART1_Request_Display1 && (UART1_tx_buffer.Locked == FALSE))
        {
                LCD_PrintMenu();
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'L', NC_ADDRESS, 3, (u8*)&MenuItem, sizeof(MenuItem), (u8*)&MaxMenuItem, sizeof(MaxMenuItem),(u8*)DisplayBuff, sizeof(DisplayBuff));
                UART1_Request_Display1 = FALSE;
        }
        else if(UART1_Request_VersionInfo && (UART1_tx_buffer.Locked == FALSE))
        {
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'V', NC_ADDRESS,1, (u8 *)&UART_VersionInfo, sizeof(UART_VersionInfo));
                UART1_Request_VersionInfo = FALSE;
        }
        else if(UART1_Request_ErrorMessage && (UART1_tx_buffer.Locked == FALSE))
        {
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'E', NC_ADDRESS, 1, (u8 *)&ErrorMSG, sizeof(ErrorMSG));
                UART1_Request_ErrorMessage = FALSE;
        }
        else if(UART1_Request_SendFollowMe && (UART1_tx_buffer.Locked == FALSE) && (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
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 's', NC_ADDRESS, 1, (u8 *)&FollowMe, sizeof(FollowMe));
                UART1_Request_SendFollowMe = FALSE;
        }
        UART1_Transmit(); // output pending bytes in tx buffer
}
 
 

Rev 163	Rev 165
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 "config.h"	62	#include "config.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 "timer1.h"	70	#include "timer1.h"
71	#include "timer2.h"	71	#include "timer2.h"
72	#include "analog.h"	72	#include "analog.h"
73	#include "main.h"	73	#include "main.h"
74	#include "waypoints.h"	74	#include "waypoints.h"
75	#include "mkprotocol.h"	75	#include "mkprotocol.h"
76	#include "params.h"	76	#include "params.h"
77	#include "fifo.h"	77	#include "fifo.h"
78		78
79	#define FALSE 0	79	#define FALSE 0
80	#define TRUE 1	80	#define TRUE 1
81		81
82		82
83	u8 UART1_Request_VersionInfo = FALSE;	83	u8 UART1_Request_VersionInfo = FALSE;
84	u8 UART1_Request_SendFollowMe = FALSE;	84	u8 UART1_Request_SendFollowMe = FALSE;
85	u8 UART1_Request_ExternalControl= FALSE;	85	u8 UART1_Request_ExternalControl= FALSE;
86	u8 UART1_Request_Display = FALSE;	86	u8 UART1_Request_Display = FALSE;
87	u8 UART1_Request_Display1 = FALSE;	87	u8 UART1_Request_Display1 = FALSE;
88	u8 UART1_Request_DebugData = FALSE;	88	u8 UART1_Request_DebugData = FALSE;
89	u8 UART1_Request_DebugLabel = 255;	89	u8 UART1_Request_DebugLabel = 255;
90	u8 UART1_Request_NaviData = FALSE;	90	u8 UART1_Request_NaviData = FALSE;
91	u8 UART1_Request_ErrorMessage = FALSE;	91	u8 UART1_Request_ErrorMessage = FALSE;
92	u8 UART1_Request_NewWaypoint = FALSE;	92	u8 UART1_Request_NewWaypoint = FALSE;
93	u8 UART1_Request_ReadWaypoint = 255;	93	u8 UART1_Request_ReadWaypoint = 255;
94	u8 UART1_Request_Data3D = FALSE;	94	u8 UART1_Request_Data3D = FALSE;
95	u8 UART1_Request_Echo = FALSE;	95	u8 UART1_Request_Echo = FALSE;
96	u8 UART1_Request_ParameterId = 0;	96	u8 UART1_Request_ParameterId = 0;
97	u8 UART1_Request_Parameter = FALSE;	97	u8 UART1_Request_Parameter = FALSE;
98	u8 UART1_DisplayLine = 0;	98	u8 UART1_DisplayLine = 0;
99	u8 UART1_ConfirmFrame = 0;	99	u8 UART1_ConfirmFrame = 0;
100		100
101	UART_TypeDef *DebugUART = UART1;	101	UART_TypeDef *DebugUART = UART1;
102		102
103	// the primary rx fifo	103	// the primary rx fifo
104	#define UART1_RX_FIFO_LEN 1024	104	#define UART1_RX_FIFO_LEN 1024
105	u8 UART1_rxfifobuffer[UART1_RX_FIFO_LEN];	105	u8 UART1_rxfifobuffer[UART1_RX_FIFO_LEN];
106	fifo_t UART1_rx_fifo;	106	fifo_t UART1_rx_fifo;
107		107
108	// the rx buffer	108	// the rx buffer
109	#define UART1_RX_BUFFER_LEN 150	109	#define UART1_RX_BUFFER_LEN 150
110	u8 UART1_rbuffer[UART1_RX_BUFFER_LEN];	110	u8 UART1_rbuffer[UART1_RX_BUFFER_LEN];
111	Buffer_t UART1_rx_buffer;	111	Buffer_t UART1_rx_buffer;
112		112
113	// the tx buffer	113	// the tx buffer
114	#define UART1_TX_BUFFER_LEN 150	114	#define UART1_TX_BUFFER_LEN 150
115	u8 UART1_tbuffer[UART1_TX_BUFFER_LEN];	115	u8 UART1_tbuffer[UART1_TX_BUFFER_LEN];
116	Buffer_t UART1_tx_buffer;	116	Buffer_t UART1_tx_buffer;
117		117
118		118
119		119
120	volatile u8 SerialLinkOkay = 0;	120	volatile u8 SerialLinkOkay = 0;
121		121
122	u8 text[200];	122	u8 text[200];
123		123
124	const u8 ANALOG_LABEL[32][16] =	124	const u8 ANALOG_LABEL[32][16] =
125	{	125	{
126	//1234567890123456	126	//1234567890123456
127	"AngleNick ", //0	127	"AngleNick ", //0
128	"AngleRoll ",	128	"AngleRoll ",
129	"AccNick ",	129	"AccNick ",
130	"AccRoll ",	130	"AccRoll ",
131	" ",	131	" ",
132	"MK-Flags ", //5	132	"MK-Flags ", //5
133	"NC-Flags ",	133	"NC-Flags ",
134	"NickServo ",	134	"NickServo ",
135	"RollServo ",	135	"RollServo ",
136	"GPS Data ",	136	"GPS Data ",
137	"CompassHeading ", //10	137	"CompassHeading ", //10
138	"GyroHeading ",	138	"GyroHeading ",
139	"SPI Error ",	139	"SPI Error ",
140	"SPI Okay ",	140	"SPI Okay ",
141	"I2C Error ",	141	"I2C Error ",
142	"I2C Okay ", //15	142	"I2C Okay ", //15
143	" ",// "Kalman_K ",	143	" ",// "Kalman_K ",
144	"ACC_Speed_N ",	144	"ACC_Speed_N ",
145	"ACC_Speed_E ",	145	"ACC_Speed_E ",
146	"Speed_z ",// "GPS ACC ",	146	"Speed_z ",// "GPS ACC ",
147	" ",// "MAXDrift ", //20	147	" ",// "MAXDrift ", //20
148	"N_Speed ",	148	"N_Speed ",
149	"E_Speed ",	149	"E_Speed ",
150	"P-Part ",	150	"P-Part ",
151	"I-Part ",	151	"I-Part ",
152	"D-Part ",//25	152	"D-Part ",//25
153	"PID-Part ",	153	"PID-Part ",
154	"Distance N ",	154	"Distance N ",
155	"Distance E ",	155	"Distance E ",
156	"GPS_Nick ",	156	"GPS_Nick ",
157	"GPS_Roll ", //30	157	"GPS_Roll ", //30
158	"Used_Sats "	158	"Used_Sats "
159	};	159	};
160		160
161	DebugOut_t DebugOut;	161	DebugOut_t DebugOut;
162	ExternControl_t ExternControl;	162	ExternControl_t ExternControl;
163	UART_VersionInfo_t UART_VersionInfo;	163	UART_VersionInfo_t UART_VersionInfo;
164	NaviData_t NaviData;	164	NaviData_t NaviData;
165	Waypoint_t FollowMe;	165	Waypoint_t FollowMe;
166	Data3D_t Data3D;	166	Data3D_t Data3D;
167	u16 Echo; // 2 bytes recieved will be sent back as echo	167	u16 Echo; // 2 bytes recieved will be sent back as echo
168		168
169	u32 UART1_DebugData_Timer;	169	u32 UART1_DebugData_Timer;
170	u32 UART1_DebugData_Interval = 5000; // in ms	170	u32 UART1_DebugData_Interval = 5000; // in ms
171	u32 UART1_NaviData_Timer;	171	u32 UART1_NaviData_Timer;
172	u32 UART1_NaviData_Interval = 5000; // in ms	172	u32 UART1_NaviData_Interval = 5000; // in ms
173	u32 UART1_Data3D_Timer = 0; // in ms	173	u32 UART1_Data3D_Timer = 0; // in ms
174	u32 UART1_Data3D_Interval = 0;	174	u32 UART1_Data3D_Interval = 0;
175		175
176	/********************************************************/	176	/********************************************************/
177	/* Initialization the UART1 */	177	/* Initialization the UART1 */
178	/********************************************************/	178	/********************************************************/
179	void UART1_Init (void)	179	void UART1_Init (void)
180	{	180	{
181	GPIO_InitTypeDef GPIO_InitStructure;	181	GPIO_InitTypeDef GPIO_InitStructure;
182	UART_InitTypeDef UART_InitStructure;	182	UART_InitTypeDef UART_InitStructure;
183		183
184	// initialize txd buffer	184	// initialize txd buffer
185	Buffer_Init(&UART1_tx_buffer, UART1_tbuffer, UART1_TX_BUFFER_LEN);	185	Buffer_Init(&UART1_tx_buffer, UART1_tbuffer, UART1_TX_BUFFER_LEN);
186		186
187	// initialize rxd buffer	187	// initialize rxd buffer
188	Buffer_Init(&UART1_rx_buffer, UART1_rbuffer, UART1_RX_BUFFER_LEN);	188	Buffer_Init(&UART1_rx_buffer, UART1_rbuffer, UART1_RX_BUFFER_LEN);
189		189
190	// initialize the rx fifo	190	// initialize the rx fifo
191	fifo_init(&UART1_rx_fifo, UART1_rxfifobuffer, UART1_RX_FIFO_LEN);	191	fifo_init(&UART1_rx_fifo, UART1_rxfifobuffer, UART1_RX_FIFO_LEN);
192		192
193	SCU_APBPeriphClockConfig(__UART1, ENABLE); // Enable the UART1 Clock	193	SCU_APBPeriphClockConfig(__UART1, ENABLE); // Enable the UART1 Clock
194	SCU_APBPeriphClockConfig(__GPIO3, ENABLE); // Enable the GPIO3 Clock	194	SCU_APBPeriphClockConfig(__GPIO3, ENABLE); // Enable the GPIO3 Clock
195		195
196	/Configure UART1_Rx pin GPIO3.2/	196	/Configure UART1_Rx pin GPIO3.2/
197	GPIO_StructInit(&GPIO_InitStructure);	197	GPIO_StructInit(&GPIO_InitStructure);
198	GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;	198	GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
199	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;	199	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
200	GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull;	200	GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull;
201	GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;	201	GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;
202	GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1; // UART1_RxD	202	GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1; // UART1_RxD
203	GPIO_Init(GPIO3, &GPIO_InitStructure);	203	GPIO_Init(GPIO3, &GPIO_InitStructure);
204		204
205	/Configure UART1_Tx pin GPIO3.3/	205	/Configure UART1_Tx pin GPIO3.3/
206	GPIO_StructInit(&GPIO_InitStructure);	206	GPIO_StructInit(&GPIO_InitStructure);
207	GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;	207	GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
208	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;	208	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
209	GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull;	209	GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull;
210	GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2; // UART1_TX	210	GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2; // UART1_TX
211	GPIO_Init(GPIO3, &GPIO_InitStructure);	211	GPIO_Init(GPIO3, &GPIO_InitStructure);
212		212
213	/* UART1 configured as follow:	213	/* UART1 configured as follow:
214	- Word Length = 8 Bits	214	- Word Length = 8 Bits
215	- One Stop Bit	215	- One Stop Bit
216	- No parity	216	- No parity
217	- BaudRate = 57600 baud	217	- BaudRate = 57600 baud
218	- Hardware flow control Disabled	218	- Hardware flow control Disabled
219	- Receive and transmit enabled	219	- Receive and transmit enabled
220	- Receive and transmit FIFOs are Disabled	220	- Receive and transmit FIFOs are Disabled
221	*/	221	*/
222	UART_StructInit(&UART_InitStructure);	222	UART_StructInit(&UART_InitStructure);
223	UART_InitStructure.UART_WordLength = UART_WordLength_8D;	223	UART_InitStructure.UART_WordLength = UART_WordLength_8D;
224	UART_InitStructure.UART_StopBits = UART_StopBits_1;	224	UART_InitStructure.UART_StopBits = UART_StopBits_1;
225	UART_InitStructure.UART_Parity = UART_Parity_No ;	225	UART_InitStructure.UART_Parity = UART_Parity_No ;
226	UART_InitStructure.UART_BaudRate = UART1_BAUD_RATE;	226	UART_InitStructure.UART_BaudRate = UART1_BAUD_RATE;
227	UART_InitStructure. UART_HardwareFlowControl = UART_HardwareFlowControl_None;	227	UART_InitStructure. UART_HardwareFlowControl = UART_HardwareFlowControl_None;
228	UART_InitStructure.UART_Mode = UART_Mode_Tx_Rx;	228	UART_InitStructure.UART_Mode = UART_Mode_Tx_Rx;
229	UART_InitStructure.UART_FIFO = UART_FIFO_Enable;	229	UART_InitStructure.UART_FIFO = UART_FIFO_Enable;
230	UART_InitStructure.UART_TxFIFOLevel = UART_FIFOLevel_1_2;	230	UART_InitStructure.UART_TxFIFOLevel = UART_FIFOLevel_1_2;
231	UART_InitStructure.UART_RxFIFOLevel = UART_FIFOLevel_1_2;	231	UART_InitStructure.UART_RxFIFOLevel = UART_FIFOLevel_1_2;
232		232
233	UART_DeInit(UART1); // reset uart 1 to default	233	UART_DeInit(UART1); // reset uart 1 to default
234	UART_Init(UART1, &UART_InitStructure); // initialize uart 1	234	UART_Init(UART1, &UART_InitStructure); // initialize uart 1
235	// enable uart 1 interrupts selective	235	// enable uart 1 interrupts selective
236	UART_ITConfig(UART1, UART_IT_Receive \| UART_IT_ReceiveTimeOut, ENABLE);	236	UART_ITConfig(UART1, UART_IT_Receive \| UART_IT_ReceiveTimeOut, ENABLE);
237	UART_Cmd(UART1, ENABLE); // enable uart 1	237	UART_Cmd(UART1, ENABLE); // enable uart 1
238	// configure the uart 1 interupt line	238	// configure the uart 1 interupt line
239	VIC_Config(UART1_ITLine, VIC_IRQ, PRIORITY_UART1);	239	VIC_Config(UART1_ITLine, VIC_IRQ, PRIORITY_UART1);
240	// enable the uart 1 IRQ	240	// enable the uart 1 IRQ
241	VIC_ITCmd(UART1_ITLine, ENABLE);	241	VIC_ITCmd(UART1_ITLine, ENABLE);
242		242
243	// initialize the debug timer	243	// initialize the debug timer
244	UART1_DebugData_Timer = SetDelay(UART1_DebugData_Interval);	244	UART1_DebugData_Timer = SetDelay(UART1_DebugData_Interval);
245	UART1_NaviData_Timer = SetDelay(UART1_NaviData_Interval)+500;	245	UART1_NaviData_Timer = SetDelay(UART1_NaviData_Interval)+500;
246		246
247	// Fill Version Info Structure	247	// Fill Version Info Structure
248	UART_VersionInfo.SWMajor = VERSION_MAJOR;	248	UART_VersionInfo.SWMajor = VERSION_MAJOR;
249	UART_VersionInfo.SWMinor = VERSION_MINOR;	249	UART_VersionInfo.SWMinor = VERSION_MINOR;
250	UART_VersionInfo.SWPatch = VERSION_PATCH;	250	UART_VersionInfo.SWPatch = VERSION_PATCH;
251	UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;	251	UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;
252	UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;	252	UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;
253		253
254	NaviData.Version = NAVIDATA_VERSION;	254	NaviData.Version = NAVIDATA_VERSION;
255		255
256	UART1_PutString("\r\nUART1 init...ok");	256	UART1_PutString("\r\nUART1 init...ok");
257	}	257	}
258		258
259		259
260	/****************************************************************/	260	/****************************************************************/
261	/* USART1 receiver ISR */	261	/* USART1 receiver ISR */
262	/****************************************************************/	262	/****************************************************************/
263	void UART1_IRQHandler(void)	263	void UART1_IRQHandler(void)
264	{	264	{
265	static u8 abortState = 0;	265	static u8 abortState = 0;
266	u8 c;	266	u8 c;
267		267
268	if((UART_GetITStatus(UART1, UART_IT_Receive) != RESET) \|\| (UART_GetITStatus(UART1, UART_IT_ReceiveTimeOut) != RESET) )	268	if((UART_GetITStatus(UART1, UART_IT_Receive) != RESET) \|\| (UART_GetITStatus(UART1, UART_IT_ReceiveTimeOut) != RESET) )
269	{	269	{
270	// clear the pending bits!	270	// clear the pending bits!
271	UART_ClearITPendingBit(UART1, UART_IT_Receive);	271	UART_ClearITPendingBit(UART1, UART_IT_Receive);
272	UART_ClearITPendingBit(UART1, UART_IT_ReceiveTimeOut);	272	UART_ClearITPendingBit(UART1, UART_IT_ReceiveTimeOut);
273	// if debug UART is not UART1	273	// if debug UART is not UART1
274	if (DebugUART != UART1)	274	if (DebugUART != UART1)
275	{ // forward received data to the debug UART tx buffer	275	{ // forward received data to the debug UART tx buffer
276	while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)	276	while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)
277	{	277	{
278	// move the byte from the rx buffer of UART1 to the tx buffer of DebugUART	278	// move the byte from the rx buffer of UART1 to the tx buffer of DebugUART
279	c = UART_ReceiveData(UART1);	279	c = UART_ReceiveData(UART1);
280		280
281	// check for abort condition (ESC ESC 0x55 0xAA 0x00)	281	// check for abort condition (ESC ESC 0x55 0xAA 0x00)
282	switch (abortState)	282	switch (abortState)
283	{	283	{
284	case 0:	284	case 0:
285	if (c == 27) abortState++;	285	if (c == 27) abortState++;
286	break;	286	break;
287	case 1:	287	case 1:
288	if (c == 27) abortState++;	288	if (c == 27) abortState++;
289	else abortState = 0;	289	else abortState = 0;
290	break;	290	break;
291	case 2:	291	case 2:
292	if (c == 0x55) abortState++;	292	if (c == 0x55) abortState++;
293	else abortState = 0;	293	else abortState = 0;
294	break;	294	break;
295	case 3:	295	case 3:
296	if (c == 0xAA) abortState++;	296	if (c == 0xAA) abortState++;
297	else abortState = 0;	297	else abortState = 0;
298	break;	298	break;
299	case 4:	299	case 4:
300	if (c == 0x00)	300	if (c == 0x00)
301	{	301	{
302	if(DebugUART == UART0)	302	if(DebugUART == UART0)
303	{	303	{
304	UART0_Connect_to_MKGPS();	304	UART0_Connect_to_MKGPS();
305	TIMER2_Init(); // enbable servo outputs	305	TIMER2_Init(); // enbable servo outputs
306	fifo_purge(&UART1_rx_fifo); // flush the whole fifo init buffer	306	fifo_purge(&UART1_rx_fifo); // flush the whole fifo init buffer
307	}	307	}
308	DebugUART = UART1;	308	DebugUART = UART1;
309	}	309	}
310	abortState = 0;	310	abortState = 0;
311	break;	311	break;
312	} // end switch abort state	312	} // end switch abort state
313	// if the Debug uart is not UART1, redirect input to the Debug UART	313	// if the Debug uart is not UART1, redirect input to the Debug UART
314	if (DebugUART != UART1)	314	if (DebugUART != UART1)
315	{	315	{
316	// wait for space in the tx buffer of the DebugUART	316	// wait for space in the tx buffer of the DebugUART
317	while(UART_GetFlagStatus(DebugUART, UART_FLAG_TxFIFOFull) == SET) {};	317	while(UART_GetFlagStatus(DebugUART, UART_FLAG_TxFIFOFull) == SET) {};
318	// move byte to the tx fifo of the debug uart	318	// move byte to the tx fifo of the debug uart
319	UART_SendData(DebugUART, c);	319	UART_SendData(DebugUART, c);
320	}	320	}
321	}	321	}
322	}	322	}
323	else // DebugUART == UART1 (normal operation)	323	else // DebugUART == UART1 (normal operation)
324	{	324	{
325	while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)	325	while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)
326	{ // some byes in the hardware fifo	326	{ // some byes in the hardware fifo
327	// get byte from hardware fifo	327	// get byte from hardware fifo
328	c = UART_ReceiveData(UART1);	328	c = UART_ReceiveData(UART1);
329	// put into the software fifo	329	// put into the software fifo
330	if(!fifo_put(&UART1_rx_fifo, c))	330	if(!fifo_put(&UART1_rx_fifo, c))
331	{ // fifo overflow	331	{ // fifo overflow
332	//fifo_purge(&UART1_rx_fifo); // flush the whole buffer	332	//fifo_purge(&UART1_rx_fifo); // flush the whole buffer
333	}	333	}
334	} // EOF while some byes in the hardware fifo	334	} // EOF while some byes in the hardware fifo
335	} // eof DebugUart = UART1	335	} // eof DebugUart = UART1
336	}	336	}
337	}	337	}
338		338
339	/**************************************************************/	339	/**************************************************************/
340	/* Process incomming data from debug uart */	340	/* Process incomming data from debug uart */
341	/**************************************************************/	341	/**************************************************************/
342	void UART1_ProcessRxData(void)	342	void UART1_ProcessRxData(void)
343	{	343	{
344	// return on forwarding uart or unlocked rx buffer	344	// return on forwarding uart or unlocked rx buffer
345	if(DebugUART != UART1) return;	345	if(DebugUART != UART1) return;
346		346
347	u8 c;	347	u8 c;
348	// if rx buffer is not locked	348	// if rx buffer is not locked
349	if(UART1_rx_buffer.Locked == FALSE)	349	if(UART1_rx_buffer.Locked == FALSE)
350	{ //collect data from primary rx fifo	350	{ //collect data from primary rx fifo
351	while(fifo_get(&UART1_rx_fifo, &c))	351	while(fifo_get(&UART1_rx_fifo, &c))
352	{ // break if complete frame is collected	352	{ // break if complete frame is collected
353	if(MKProtocol_CollectSerialFrame(&UART1_rx_buffer, c)) break;	353	if(MKProtocol_CollectSerialFrame(&UART1_rx_buffer, c)) break;
354	}	354	}
355	}	355	}
356	if(UART1_rx_buffer.Locked == FALSE) return;	356	if(UART1_rx_buffer.Locked == FALSE) return;
357		357
358	Waypoint_t * pWaypoint = NULL;	358	Waypoint_t * pWaypoint = NULL;
359	SerialMsg_t SerialMsg;	359	SerialMsg_t SerialMsg;
360		360
361	MKProtocol_DecodeSerialFrame(&UART1_rx_buffer, &SerialMsg); // decode serial frame in rxd buffer	361	MKProtocol_DecodeSerialFrame(&UART1_rx_buffer, &SerialMsg); // decode serial frame in rxd buffer
362	if(*(SerialMsg.pCmdID) != 'z') SerialLinkOkay = 250; // reset SerialTimeout, but not in case of the "ping"	362	if(*(SerialMsg.pCmdID) != 'z') SerialLinkOkay = 250; // reset SerialTimeout, but not in case of the "ping"
363	switch(*(SerialMsg.pAddress)) // check for Slave Address	363	switch(*(SerialMsg.pAddress)) // check for Slave Address
364	{	364	{
365	case NC_ADDRESS: // own Slave Address	365	case NC_ADDRESS: // own Slave Address
366	switch(*(SerialMsg.pCmdID))	366	switch(*(SerialMsg.pCmdID))
367	{	367	{
368	case 'z': // connection checker	368	case 'z': // connection checker
369	memcpy(&Echo, SerialMsg.pData, sizeof(Echo)); // copy echo pattern	369	memcpy(&Echo, SerialMsg.pData, sizeof(Echo)); // copy echo pattern
370	UART1_Request_Echo = TRUE;	370	UART1_Request_Echo = TRUE;
371	break;	371	break;
372		372
373	case 'e': // request for the text of the error status	373	case 'e': // request for the text of the error status
374	UART1_Request_ErrorMessage = TRUE;	374	UART1_Request_ErrorMessage = TRUE;
375	break;	375	break;
376		376
377	case 's':// new target position	377	case 's':// new target position
378	pWaypoint = (Waypoint_t*)SerialMsg.pData;	378	pWaypoint = (Waypoint_t*)SerialMsg.pData;
379	BeepTime = 300;	379	BeepTime = 300;
380	if(pWaypoint->Position.Status == NEWDATA)	380	if(pWaypoint->Position.Status == NEWDATA)
381	{	381	{
382	WPList_Clear(); // empty WPList	382	WPList_Clear(); // empty WPList
383	WPList_Append(pWaypoint);	383	WPList_Append(pWaypoint);
384	GPS_pWaypoint = WPList_Begin();	384	GPS_pWaypoint = WPList_Begin();
385	}	385	}
386	break;	386	break;
387		387
388	case 'u': // redirect debug uart	388	case 'u': // redirect debug uart
389	switch(SerialMsg.pData[0])	389	switch(SerialMsg.pData[0])
390	{	390	{
391	case UART_FLIGHTCTRL:	391	case UART_FLIGHTCTRL:
392	UART2_Init(); // initialize UART2 to FC pins	392	UART2_Init(); // initialize UART2 to FC pins
393	fifo_purge(&UART1_rx_fifo);	393	fifo_purge(&UART1_rx_fifo);
394	DebugUART = UART2;	394	DebugUART = UART2;
395	break;	395	break;
396	case UART_MK3MAG:	396	case UART_MK3MAG:
397	if(FC.MKFlags & MKFLAG_MOTOR_RUN) break; // not if the motors are running	397	if(FC.MKFlags & MKFLAG_MOTOR_RUN) break; // not if the motors are running
398	UART0_Connect_to_MK3MAG(); // mux UART0 to MK3MAG pins	398	UART0_Connect_to_MK3MAG(); // mux UART0 to MK3MAG pins
399	GPSData.Status = INVALID;	399	GPSData.Status = INVALID;
400	fifo_purge(&UART1_rx_fifo);	400	fifo_purge(&UART1_rx_fifo);
401	DebugUART = UART0;	401	DebugUART = UART0;
402	break;	402	break;
403	case UART_MKGPS:	403	case UART_MKGPS:
404	if(FC.MKFlags & MKFLAG_MOTOR_RUN) break; // not if the motors are running	404	if(FC.MKFlags & MKFLAG_MOTOR_RUN) break; // not if the motors are running
405	TIMER2_Deinit();	405	TIMER2_Deinit();
406	UART0_Connect_to_MKGPS(); // connect UART0 to MKGPS pins	406	UART0_Connect_to_MKGPS(); // connect UART0 to MKGPS pins
407	GPSData.Status = INVALID;	407	GPSData.Status = INVALID;
408	fifo_purge(&UART1_rx_fifo);	408	fifo_purge(&UART1_rx_fifo);
409	DebugUART = UART0;	409	DebugUART = UART0;
410	break;	410	break;
411	default:	411	default:
412	break;	412	break;
413	}	413	}
414	break;	414	break;
415		415
416	case 'w':// Append Waypoint to List	416	case 'w':// Append Waypoint to List
417	{	417	{
418	static u8 oldIndex = 0x00;	418	static u8 oldIndex = 0x00;
419		419
420	pWaypoint = (Waypoint_t*)SerialMsg.pData;	420	pWaypoint = (Waypoint_t*)SerialMsg.pData;
421	if(pWaypoint->Position.Status == INVALID)	421	if(pWaypoint->Position.Status == INVALID)
422	{ // clear WP List	422	{ // clear WP List
423	WPList_Clear();	423	WPList_Clear();
424	oldIndex = 0x00;	424	oldIndex = 0x00;
425	GPS_pWaypoint = WPList_Begin();	425	GPS_pWaypoint = WPList_Begin();
426	//UART1_PutString("\r\nClear WP List\r\n");	-
427	UART1_Request_NewWaypoint = TRUE;	426	UART1_Request_NewWaypoint = TRUE;
428	}	427	}
429	else if (pWaypoint->Position.Status == NEWDATA)	428	else if (pWaypoint->Position.Status == NEWDATA)
430	{ // app current WP to the list	429	{ // app current WP to the list
431	sprintf(text, "\r\nAdd WP: Index= %d, oldIndex = %d\r\n", pWaypoint->Index, oldIndex);	-
432	UART1_PutString(text);	-
433		-
434	if (pWaypoint->Index == oldIndex + 1)	430	if (pWaypoint->Index == oldIndex + 1)
435	{	431	{
436	WPList_Append(pWaypoint);	432	WPList_Append(pWaypoint);
437	BeepTime = 500;	433	BeepTime = 500;
438	oldIndex = pWaypoint->Index;	434	oldIndex = pWaypoint->Index;
439	UART1_Request_NewWaypoint = TRUE;	435	UART1_Request_NewWaypoint = TRUE;
440	}	436	}
441		-
442		437	}
443	}	438	}
444	}	-
445	break;	439	break;
446		440
447	case 'x':// Read Waypoint from List	441	case 'x':// Read Waypoint from List
448	UART1_Request_ReadWaypoint = SerialMsg.pData[0];	442	UART1_Request_ReadWaypoint = SerialMsg.pData[0];
449	break;	443	break;
450		444
451	case 'j':// Set/Get NC-Parameter	445	case 'j':// Set/Get NC-Parameter
452	switch(SerialMsg.pData[0])	446	switch(SerialMsg.pData[0])
453	{	447	{
454	case 0: // get	448	case 0: // get
455	break;	449	break;
456		450
457	case 1: // set	451	case 1: // set
458	{	452	{
459	s16 value;	453	s16 value;
460	value = SerialMsg.pData[2] + (s16)SerialMsg.pData[3] * 0x0100;	454	value = SerialMsg.pData[2] + (s16)SerialMsg.pData[3] * 0x0100;
461	NCParams_SetValue(SerialMsg.pData[1], &value);	455	NCParams_SetValue(SerialMsg.pData[1], &value);
462	}	456	}
463	break;	457	break;
464		458
465	default:	459	default:
466	break;	460	break;
467	}	461	}
468	UART1_Request_ParameterId = SerialMsg.pData[1];	462	UART1_Request_ParameterId = SerialMsg.pData[1];
469	UART1_Request_Parameter = TRUE;	463	UART1_Request_Parameter = TRUE;
470	break;	464	break;
471	default:	465	default:
472	// unsupported command recieved	466	// unsupported command recieved
473	break;	467	break;
474	} // case NC_ADDRESS	468	} // case NC_ADDRESS
475	// "break;" is missing here to fall thru to the common commands	469	// "break;" is missing here to fall thru to the common commands
476		470
477	default: // and any other Slave Address	471	default: // and any other Slave Address
478		472
479	switch(*(SerialMsg.pCmdID)) // check CmdID	473	switch(*(SerialMsg.pCmdID)) // check CmdID
480	{	474	{
481	case 'a':// request for the labels of the analog debug outputs	475	case 'a':// request for the labels of the analog debug outputs
482	UART1_Request_DebugLabel = SerialMsg.pData[0];	476	UART1_Request_DebugLabel = SerialMsg.pData[0];
483	if(UART1_Request_DebugLabel > 31) UART1_Request_DebugLabel = 31;	477	if(UART1_Request_DebugLabel > 31) UART1_Request_DebugLabel = 31;
484	break;	478	break;
485		479
486	case 'b': // submit extern control	480	case 'b': // submit extern control
487	memcpy(&ExternControl, SerialMsg.pData, sizeof(ExternControl));	481	memcpy(&ExternControl, SerialMsg.pData, sizeof(ExternControl));
488	UART1_ConfirmFrame = ExternControl.Frame;	482	UART1_ConfirmFrame = ExternControl.Frame;
489	break;	483	break;
490		484
491	case 'd': // request for debug data;	485	case 'd': // request for debug data;
492	UART1_DebugData_Interval = (u32) SerialMsg.pData[0] * 10;	486	UART1_DebugData_Interval = (u32) SerialMsg.pData[0] * 10;
493	if(UART1_DebugData_Interval > 0) UART1_Request_DebugData = TRUE;	487	if(UART1_DebugData_Interval > 0) UART1_Request_DebugData = TRUE;
494	break;	488	break;
495		489
496	case 'c': // request for 3D data;	490	case 'c': // request for 3D data;
497	UART1_Data3D_Interval = (u32) SerialMsg.pData[0] * 10;	491	UART1_Data3D_Interval = (u32) SerialMsg.pData[0] * 10;
498	if(UART1_Data3D_Interval > 0) UART1_Request_Data3D = TRUE;	492	if(UART1_Data3D_Interval > 0) UART1_Request_Data3D = TRUE;
499	break;	493	break;
500		494

Subversion Repositories NaviCtrl

(root)/tags/V2.04a/uart1.c @ 563 - Rev 163 → 165