WebSVN - NaviCtrl - Diff - Rev 357 and 358


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

Subversion Repositories NaviCtrl

(root)/tags/V2.14d/uart1.c @ 935 - Rev 357 → 358