WebSVN - NaviCtrl - Diff - Rev 330 and 338


/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 2008 Ingo Busker, Holger Buss
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
// + 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 oder Nutzung 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 the sources to other systems or using the software on other systems (except 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 "main.h"
#include "config.h"
#include "menu.h"
#include "GPS.h"
#include "i2c.h"
#include "uart0.h"
#include "uart1.h"
#include "uart2.h"
#include "timer1.h"
#include "timer2.h"
#include "analog.h"
#include "compass.h"
#include "waypoints.h"
#include "mkprotocol.h"
#include "params.h"
#include "fifo.h"
#include "debug.h"
 
#define FALSE   0
#define TRUE    1
 
#define ABO_TIMEOUT 4000 // disable abo after 4 seconds
u32 UART1_AboTimeOut = 0;
 
u8 UART1_Request_VersionInfo    = FALSE;
u8 UART1_Request_ExternalControl= FALSE;
u8 UART1_Request_Display                = FALSE;
u8 UART1_Request_Display1               = FALSE;
u8 UART1_Request_DebugData              = FALSE;
u8 UART1_Request_DebugLabel             = 255;
u8 UART1_Request_NaviData               = FALSE;
u8 UART1_Request_ErrorMessage   = FALSE;
u8 UART1_Request_WritePoint             = 0xFF;
u8 UART1_Request_ReadPoint              = 0;
u8 UART1_Request_Data3D             = FALSE;
u8 UART1_Request_Echo               = FALSE;
u8 UART1_Request_ParameterId    = 0;
u8 UART1_Request_Parameter              = FALSE;
u8 UART1_DisplayKeys                    = 0;
u8 UART1_DisplayLine                    = 0;
u8 UART1_ConfirmFrame                   = 0;
 
UART_TypeDef *DebugUART = UART1;
 
#ifdef FOLLOW_ME
#define FOLLOW_ME_INTERVAL 200 // 5 Hz
u32 UART1_FollowMe_Timer        = 0;
Point_t FollowMe;
#endif
 
// the primary rx fifo
#define UART1_RX_FIFO_LEN 512
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         ",
        "OperatingRadius ",
        "FC-Flags        ", //5
        "NC-Flags        ",
        "NickServo       ",
        "RollServo       ",
        "GPS Data        ",
        "CompassHeading  ", //10
        "GyroHeading     ",
        "SPI Error       ",
        "SPI Okay        ",
        "I2C Error       ",
        "I2C Okay        ", //15
        "16 KompassKalm  ",//    "Kalman_K        ",
        "17 Inclination  ",
        "18 Horizontal   ",
        "19 Normal       ",
        "EarthMagnet [%] ", //20
 
 
 
        "Z_Speed         ",
        "N_Speed         ",
        "E_Speed         ",
        "Magnet X        ",
        "Magnet Y        ", //25
        "Magnet Z        ",
        "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;
Data3D_t Data3D;
u16 Echo; // 2 bytes recieved will be sent back as echo
 
u32 UART1_DebugData_Timer = 0;
u32 UART1_DebugData_Interval = 0;       // in ms
u32 UART1_NaviData_Timer = 0;
u32 UART1_NaviData_Interval = 0;        // in ms
u32 UART1_Data3D_Timer = 0;
u32 UART1_Data3D_Interval = 0;          // in ms
u32 UART1_Display_Timer = 0;
u32 UART1_Display_Interval = 0;         // in ms
 
/********************************************************/
/*            Initialization the UART1                  */
/********************************************************/
void UART1_Init (void)
{
        GPIO_InitTypeDef GPIO_InitStructure;
        UART_InitTypeDef UART_InitStructure;
 
        // initialize txd buffer
        Buffer_Init(&UART1_tx_buffer, UART1_tbuffer, UART1_TX_BUFFER_LEN);
 
        // initialize rxd buffer
        Buffer_Init(&UART1_rx_buffer, UART1_rbuffer, UART1_RX_BUFFER_LEN);
 
        // initialize the rx fifo, block UART IRQ geting a byte from fifo
        fifo_init(&UART1_rx_fifo, UART1_rxfifobuffer, UART1_RX_FIFO_LEN, NO_ITLine, UART1_ITLine);
 
        SCU_APBPeriphClockConfig(__UART1, ENABLE);  // Enable the UART1 Clock
        SCU_APBPeriphClockConfig(__GPIO3, ENABLE);  // Enable the GPIO3 Clock
 
        /*Configure UART1_Rx pin GPIO3.2*/
        GPIO_StructInit(&GPIO_InitStructure);
        GPIO_InitStructure.GPIO_Direction =     GPIO_PinInput;
        GPIO_InitStructure.GPIO_Pin =                   GPIO_Pin_2;
        GPIO_InitStructure.GPIO_Type =                  GPIO_Type_PushPull;
        GPIO_InitStructure.GPIO_IPInputConnected =      GPIO_IPInputConnected_Enable;
        GPIO_InitStructure.GPIO_Alternate =     GPIO_InputAlt1; // UART1_RxD
        GPIO_Init(GPIO3, &GPIO_InitStructure);
 
        /*Configure UART1_Tx pin GPIO3.3*/
        GPIO_StructInit(&GPIO_InitStructure);
        GPIO_InitStructure.GPIO_Direction =     GPIO_PinOutput;
        GPIO_InitStructure.GPIO_Pin =                   GPIO_Pin_3;
        GPIO_InitStructure.GPIO_Type =                  GPIO_Type_PushPull;
        GPIO_InitStructure.GPIO_Alternate =     GPIO_OutputAlt2; // UART1_TX
        GPIO_Init(GPIO3, &GPIO_InitStructure);
 
        /* UART1 configured as follow:
        - Word Length = 8 Bits
        - One Stop Bit
        - No parity
        - BaudRate = 57600 baud
        - Hardware flow control Disabled
        - Receive and transmit enabled
        - Receive and transmit FIFOs are Disabled
        */
        UART_StructInit(&UART_InitStructure);
        UART_InitStructure.UART_WordLength =                    UART_WordLength_8D;
        UART_InitStructure.UART_StopBits =                              UART_StopBits_1;
        UART_InitStructure.UART_Parity =                                UART_Parity_No ;
        UART_InitStructure.UART_BaudRate =                              UART1_BAUD_RATE;
        UART_InitStructure. UART_HardwareFlowControl =  UART_HardwareFlowControl_None;
        UART_InitStructure.UART_Mode =                                  UART_Mode_Tx_Rx;
        UART_InitStructure.UART_FIFO =                                  UART_FIFO_Enable;
        UART_InitStructure.UART_TxFIFOLevel =                   UART_FIFOLevel_1_2;
        UART_InitStructure.UART_RxFIFOLevel =                   UART_FIFOLevel_1_2;
 
        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\n UART1 init...ok");
}
 
 
/****************************************************************/
/*               USART1 receiver ISR                            */
/****************************************************************/
void UART1_IRQHandler(void)
{
        static u8 abortState = 0;
        u8 c;
 
        IENABLE;
 
        if((UART_GetITStatus(UART1, UART_IT_Receive) != RESET) || (UART_GetITStatus(UART1, UART_IT_ReceiveTimeOut) != RESET) )
        {
                // clear the pending bits!
                UART_ClearITPendingBit(UART1, UART_IT_Receive);
                UART_ClearITPendingBit(UART1, UART_IT_ReceiveTimeOut);
                // if debug UART is not UART1
                if (DebugUART != UART1)
                {       // forward received data to the debug UART tx buffer
                        while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)
                        {
                                // move the byte from the rx buffer of UART1 to the tx buffer of DebugUART
                                c = UART_ReceiveData(UART1);
 
                                // check for abort condition (ESC ESC 0x55 0xAA 0x00)
                                switch (abortState)
                                {
                                        case 0:
                                                if (c == 27) abortState++;
                                                break;
                                        case 1:
                                                if (c == 27) abortState++;
                                                else abortState = 0;
                                        break;
                                        case 2:
                                                if (c == 0x55) abortState++;
                                                else abortState = 0;
                                                break;
                                        case 3:
                                                if (c == 0xAA) abortState++;
                                                else abortState = 0;
                                                break;
                                        case 4:
                                                if (c == 0x00)
                                                {
                                                        if(DebugUART == UART0)
                                                        {
                                                                UART0_Connect_to_MKGPS(UART0_BAUD_RATE);
                                                                TIMER2_Init(); // enbable servo outputs
                                                                fifo_purge(&UART1_rx_fifo); // flush the whole fifo init buffer
                                                        }
                                                        DebugUART = UART1;
                                                }
                                                abortState = 0;
                                                break;
                                } // end switch abort state
                                // if the Debug uart is not UART1, redirect input to the Debug UART
                                if (DebugUART != UART1)
                                {
                                        // wait for space in the tx buffer of the DebugUART
                                        while(UART_GetFlagStatus(DebugUART, UART_FLAG_TxFIFOFull) == SET) {};
                                        // move byte to the tx fifo of the debug uart
                                        UART_SendData(DebugUART, c);
                                }
                        }
                }
                else  // DebugUART == UART1 (normal operation)
                {
                        while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)
                        { // some byes in the hardware fifo
                            // get byte from hardware fifo
                        c = UART_ReceiveData(UART1);
                                // put into the software fifo
                                if(!fifo_put(&UART1_rx_fifo, c))
                                {       // fifo overflow
                                        //fifo_purge(&UART1_rx_fifo); // flush the whole buffer
                                }
                        } // EOF while some byes in the hardware fifo
                } // eof DebugUart = UART1
        }
 
        IDISABLE;
}
 
/**************************************************************/
/* 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;
 
        Point_t * pPoint = NULL;
        SerialMsg_t SerialMsg;
 
        // analyze header first
        MKProtocol_DecodeSerialFrameHeader(&UART1_rx_buffer, &SerialMsg);
        if( SerialMsg.Address == FC_ADDRESS )
        {
                switch(SerialMsg.CmdID)
                {
//                      case 'v': // version
                        case 'y': // serial poti values
                        case 'b': // extern control
                                Buffer_Copy(&UART1_rx_buffer, &UART2_tx_buffer); //forward to FC
                                Buffer_Clear(&UART1_rx_buffer); // free rc buffer for next frame
                                return; //end process rx data
                        break;
                }
        }
 
        MKProtocol_DecodeSerialFrameData(&UART1_rx_buffer, &SerialMsg); // decode serial frame in rxd buffer
    if(SerialMsg.CmdID != 'z') SerialLinkOkay = 250;      // reset SerialTimeout, but not in case of the "ping"
        switch(SerialMsg.Address) // check for Slave Address
        {
                case NC_ADDRESS:  // own Slave Address
                switch(SerialMsg.CmdID)
                {
                        case 'z': // connection checker
                                memcpy(&Echo, SerialMsg.pData, sizeof(Echo)); // copy echo pattern
                                UART1_Request_Echo = TRUE;
                                break;
 
                        case 'e': // request for the text of the error status
                                UART1_Request_ErrorMessage = TRUE;
                                break;
 
                        case 's'://  new target position
                                pPoint = (Point_t*)SerialMsg.pData;
                                if(pPoint->Position.Status == NEWDATA)
                                {
                                        //if(!(FC.StatusFlags & FC_STATUS_FLY)) PointList_Clear(); // flush the list
                                        //pPoint->Index = 1; // must be one after empty list
                                        PointList_SetAt(pPoint);
                                        if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
                                        GPS_pWaypoint = PointList_WPBegin(); // updates POI index
                                        if(GPS_pWaypoint != NULL) // if new WP exist
                                        {   // update WP hold time stamp immediately!
/*                                              if(GPS_pWaypoint->Heading > 0 && GPS_pWaypoint->Heading <= 360)
                                                {
                                                 CAM_Orientation.Azimuth = GPS_pWaypoint->Heading;
                                                 CAM_Orientation.UpdateMask |= CAM_UPDATE_AZIMUTH;
                                                }
*/
                                        }
                                        BeepTime = 50;
                                }
                                break;
 
                        case 'u': // redirect debug uart
                                switch(SerialMsg.pData[0])
                                {
                                        case UART_FLIGHTCTRL:
                                                UART2_Init();                           // initialize UART2 to FC pins
                                                fifo_purge(&UART1_rx_fifo);
                                                TIMER2_Deinit();                        // reduce irq load
                                                DebugUART = UART2;
                                                break;
                                        case UART_MK3MAG:
                                                if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) break; // not if the motors are running
                                                UART0_Connect_to_MK3MAG();      // mux UART0 to MK3MAG pins
                                                GPSData.Status = INVALID;
                                                fifo_purge(&UART1_rx_fifo);
                                                DebugUART = UART0;
                                                break;
                                        case UART_MKGPS:
                                                if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) break; // not if the motors are running
                                                TIMER2_Deinit();                        // disable servo outputs to reduce irq load
                                                UART0_Connect_to_MKGPS(UART0_BAUD_RATE);        // connect UART0 to MKGPS pins
                                                GPSData.Status = INVALID;
                                                fifo_purge(&UART1_rx_fifo);
                                                DebugUART = UART0;
                                                break;
                                        default:
                                                break;
                                }
                                break;
 
                        case 'w'://  Set point in list at index
                                {
                                        pPoint = (Point_t*)SerialMsg.pData;
 
                                        if((pPoint->Position.Status == INVALID) && (pPoint->Index == 0))
                                        {
                                                PointList_Clear();
                                                GPS_pWaypoint = PointList_WPBegin();
                                                UART1_Request_WritePoint = 0; // return new point count
                                        }
                                        else
                                        {  // update WP in list at index
                                                UART1_Request_WritePoint = PointList_SetAt(pPoint);
                                                if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
                                                if(UART1_Request_WritePoint == pPoint->Index)
                                                {
                                                        BeepTime = 500;
                                                }
                                        }
                                }
                                break;
 
                        case 'x'://  Read Waypoint from List
                                UART1_Request_ReadPoint = 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.CmdID) // check CmdID
                {
                        case 'a':// request for the labels of the analog debug outputs
                                UART1_Request_DebugLabel = SerialMsg.pData[0];
                                if(UART1_Request_DebugLabel > 31) UART1_Request_DebugLabel = 31;
                                break;
                        /*
                        case 'b': // submit extern control
                                memcpy(&ExternControl, SerialMsg.pData, sizeof(ExternControl));
                                UART1_ConfirmFrame = ExternControl.Frame;
                                break;
                        */
                        case 'd': // request for debug data;
                                UART1_DebugData_Interval = (u32) SerialMsg.pData[0] * 10;
                                if(UART1_DebugData_Interval > 0) UART1_Request_DebugData = TRUE;
                                UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);
                                break;
 
                        case 'c': // request for 3D data;
                                UART1_Data3D_Interval = (u32) SerialMsg.pData[0] * 10;
                                if(UART1_Data3D_Interval > 0) UART1_Request_Data3D = TRUE;
                                UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);
                                break;
 
                        case 'h':// reqest for display line
                                if((SerialMsg.pData[0]& 0x80) == 0x00)// old format
                                {
                                        UART1_DisplayLine = 2;
                                        UART1_Display_Interval = 0;
                                }
                                else
                                {
                                        UART1_DisplayKeys |= ~SerialMsg.pData[0];
                                        UART1_Display_Interval = (u32) SerialMsg.pData[1] * 10;
                                        UART1_DisplayLine = 4;
                                        UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);
                                }
                                UART1_Request_Display = TRUE;
                                break;
 
                        case 'l':// reqest for display columns
                                MenuItem = SerialMsg.pData[0];
                                UART1_Request_Display1 = TRUE;
                                break;
 
                        case 'o': // request for navigation information
                                UART1_NaviData_Interval = (u32) SerialMsg.pData[0] * 10;
                                if(UART1_NaviData_Interval > 0) UART1_Request_NaviData = TRUE;
                                UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);
                                break;
 
                        case 'v': // request for version info
                                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);
        #ifdef FOLLOW_ME
        if(TransmitAlsoToFC) UART_SendData(UART2, c);
        #endif
        return (0);
}
 
/*****************************************************/
/*       Send a string to the debug uart              */
/*****************************************************/
void UART1_PutString(u8 *s)
{
        if(s == NULL) return;
        while (*s != '\0' && DebugUART == UART1)
        {
                UART1_Putchar(*s);
                s ++;
        }
}
 
 
/**************************************************************/
/*         Transmit tx buffer via debug uart                  */
/**************************************************************/
void UART1_Transmit(void)
{
        u8 tmp_tx;
        if(DebugUART != UART1) return;
        // if something has to be send and the txd fifo is not full
        if(UART1_tx_buffer.Locked == TRUE)
        {
                // while there is some space in the tx fifo
                while(UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != SET)
                {
                        tmp_tx = UART1_tx_buffer.pData[UART1_tx_buffer.Position++]; // read next byte from txd buffer
                        UART_SendData(UART1, tmp_tx); // put character to txd fifo
                        #ifdef FOLLOW_ME
                        if(TransmitAlsoToFC)
                        {
                                UART_SendData(UART2, tmp_tx); // put character to txd fifo
                        }
                        #endif
                        // if terminating character or end of txd buffer reached
                        if((tmp_tx == '\r') || (UART1_tx_buffer.Position == UART1_tx_buffer.DataBytes))
                        {
                                Buffer_Clear(&UART1_tx_buffer); // clear txd buffer
                                #ifdef FOLLOW_ME
                                TransmitAlsoToFC = 0;
                                #endif
                                break; // end while loop
                        }
                }
        }
}
 
/**************************************************************/
/* Send the answers to incomming commands at the debug uart   */
/**************************************************************/
void UART1_TransmitTxData(void)
{
        if(DebugUART != UART1) return;
 
        if(CheckDelay(UART1_AboTimeOut))
        {
                UART1_DebugData_Interval = 0;
                UART1_NaviData_Interval = 0;
                UART1_Data3D_Interval = 0;
                UART1_Display_Interval = 0;
        }
 
        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);
                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_WritePoint!= 0xFF) && (UART1_tx_buffer.Locked == FALSE))
        {
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'W', NC_ADDRESS, 1, &UART1_Request_WritePoint, sizeof(UART1_Request_WritePoint));
                UART1_Request_WritePoint = 0xFF;
        }
        else if((UART1_Request_ReadPoint) && (UART1_tx_buffer.Locked == FALSE))
        {
                u8 PointCount = PointList_GetCount();
                if (UART1_Request_ReadPoint <= PointCount)
                {
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'X', NC_ADDRESS, 3, &PointCount, 1, &UART1_Request_ReadPoint, 1, PointList_GetAt(UART1_Request_ReadPoint), sizeof(Point_t));
                }
                else
                {
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer,'X', NC_ADDRESS, 1, &PointCount, sizeof(PointCount));
                }
                UART1_Request_ReadPoint = 0;
        }
        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_Display_Interval > 0) && CheckDelay(UART1_Display_Timer)) || UART1_Request_Display) && (UART1_tx_buffer.Locked == FALSE))
        {
                if(UART1_DisplayLine > 3)
                {
                        Menu_Update(UART1_DisplayKeys);
                        UART1_DisplayKeys = 0;
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'H', NC_ADDRESS, 1, (u8*)DisplayBuff, sizeof(DisplayBuff));
                }
                else
                {
                        UART1_DisplayLine = 2;
                        sprintf(text,"!!! incompatible !!!");
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'H', NC_ADDRESS, 2, &UART1_DisplayLine, sizeof(UART1_DisplayLine), (u8*)&text, 20);
                        if(UART1_DisplayLine++ > 3) UART1_DisplayLine = 0;
                }
                UART1_Display_Timer = SetDelay(UART1_Display_Interval);
                UART1_Request_Display = FALSE;
        }
        else if(UART1_Request_Display1 && (UART1_tx_buffer.Locked == FALSE))
        {
                Menu_Update(0);
                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;
        }
#ifdef FOLLOW_ME
        else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE))
        {
                if((GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.NumOfSats >= 4))
                {
                        TransmitAlsoToFC = 1;
                        // update FollowMe content
                        FollowMe.Position.Longitude     = GPSData.Position.Longitude;
                        FollowMe.Position.Latitude      = GPSData.Position.Latitude;
                        FollowMe.Position.Status = NEWDATA;
                        FollowMe.Position.Altitude = 1;
                        // 0  -> no Orientation
                        // 1-360 -> CompassCourse Setpoint
                        // -1 -> points to  WP1 -> itself
                FollowMe.Heading = -1;
                        FollowMe.ToleranceRadius = 1;
                        FollowMe.HoldTime = 60;
                        FollowMe.Event_Flag = 1;
                        FollowMe.Index = 1; // 0 = Delete List, 1 place at first entry in the list
                        FollowMe.Type = POINT_TYPE_WP;
                        FollowMe.WP_EventChannelValue = 100;  // set servo value
                        FollowMe.AltitudeRate = 0;                        // do not change height
                        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_FollowMe_Timer = SetDelay(FOLLOW_ME_INTERVAL); // set new update time
        }
#endif
#ifdef DEBUG                                                                                                                    // only include functions if DEBUG is defined
        else if(SendDebugOutput && (UART1_tx_buffer.Locked == FALSE))
        {
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer,'0', NC_ADDRESS, 1, (u8 *) &tDebug, sizeof(tDebug));
                SendDebugOutput = 0;
        }
#endif
        UART1_Transmit(); // output pending bytes in tx buffer
}
 
 

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

Subversion Repositories NaviCtrl

(root)/tags/V0.28h/uart1.c @ 882 - Rev 330 → 338