WebSVN - MK3Mag - Diff - Rev 39 and 41

 // +  POSSIBILITY OF SUCH DAMAGE.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <avr/wdt.h>
+#include <stdarg.h>
+#include <string.h>
 #include "main.h"
 #include "uart.h"
 #include "timer0.h"
 #include "twislave.h"
 volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
 volatile uint8_t rxd_buffer_locked = FALSE;
 volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
 volatile uint8_t txd_complete = TRUE;
 volatile uint8_t ReceivedBytes = 0;
+volatile uint8_t *pRxData = 0;
+volatile uint8_t RxDataLen = 0;
 // send flags
 #define RQST_VERSION_INFO               0x01
 #define RQST_DEBUG_DATA                 0x02
 #define RQST_EXTERN_CTRL                0x10
+uint8_t RequestFlags = 0x00;
-uint8_t RequestFlags = 0x00;
+uint8_t RequestDebugLabel = 0;
-uint8_t RequestDebugLabel = 0;
+uint8_t ConfirmFrame = 0;
 uint8_t PC_Connected = 0;
 uint8_t FC_Connected = 0;
 uint8_t MySlaveAddr = MK3MAG_ADDRESS;
 DebugOut_t              DebugOut;
-DebugOut_t              DebugOut;
+ExternData_t    ExternData;
-ExternData_t    ExternData;
+ExternControl_t ExternControl;
-ExternControl_t ExternControl;
+UART_VersionInfo_t      UART_VersionInfo;
 /****************************************************************/
 /*              Initialization of the USART0                    */
 /****************************************************************/
 void USART0_Init (void)
+{
+        uint8_t sreg = SREG;
         uint16_t ubrr = (uint16_t) ((uint32_t) F_CPU/(8 * BAUD_RATE) - 1);
         // disable all interrupts before configuration
         // enable RX-Interrupt
         UCSR0B |= (1 << RXCIE0);
         // enable TX-Interrupt
         UCSR0B |= (1 << TXCIE0);
+        // initialize the debug timer
+        DebugData_Timer = SetDelay(DebugData_Interval);
         // unlock rxd_buffer
+        rxd_buffer_locked = FALSE;
+        pRxData = 0;
+        RxDataLen = 0;
-        rxd_buffer_locked = FALSE;
+        // no bytes to send
         txd_complete = TRUE;
+        UART_VersionInfo.Major = VERSION_MAJOR;
-        VersionInfo.Major = VERSION_MAJOR;
+        UART_VersionInfo.Minor = VERSION_MINOR;
-        VersionInfo.Minor = VERSION_MINOR;
+        UART_VersionInfo.Patch = VERSION_PATCH;
-        VersionInfo.PCCompatible = VERSION_PCCOMPATIBLE;
+        UART_VersionInfo.Compatible = VERSION_SERIAL_COMPATIBLE;
         USART0_putchar ('V');
         USART0_putchar (0x30 + VERSION_MAJOR);
         USART0_putchar ('.');
         USART0_putchar (0x30 + VERSION_MINOR/10);
         USART0_putchar (0x30 + VERSION_MINOR%10);
+        USART0_putchar ('a' + VERSION_PATCH);
     USART0_putchar ('\n');
+    // restore global interrupt flags
+    SREG = sreg;
+}
 // ---------------------------------------------------------------------------------
 void USART0_EnableTXD(void)
                         crc1 = '=' + crc / 64;
                         crc2 = '=' + crc % 64;
                         // compare checksum to transmitted checksum bytes
                         if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))
                         {   // checksum valid
-                                rxd_buffer_locked = TRUE;          // lock the rxd buffer
-                                ReceivedBytes = ptr_rxd_buffer;    // store number of received bytes
                                 rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
+                                ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes
+                                rxd_buffer_locked = TRUE;          // lock the rxd buffer
                                 // if 2nd byte is an 'R' enable watchdog that will result in an reset
                                 if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando
+                        }
                         else
                         {       // checksum invalid
+}
+// --------------------------------------------------------------------------
-// --------------------------------------------------------------------------
+void SendOutData(uint8_t cmd,uint8_t module, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ...
 void SendOutData(uint8_t cmd,uint8_t module, uint8_t *snd, uint8_t len)
         va_list ap;
+        uint16_t pt = 0;
+        uint8_t a,b,c;
+        uint8_t ptr = 0;
         uint16_t pt = 0;
-        uint8_t a,b,c;
+        uint8_t *pdata = 0;
-        uint8_t ptr = 0;
+        int len = 0;
+        txd_buffer[pt++] = '#';         // Start character
+        txd_buffer[pt++] = module;      // Address (a=0; b=1,...)
+        txd_buffer[pt++] = cmd;         // Command
+        va_start(ap, numofbuffers);
+        if(numofbuffers--)
+        {
                 pdata = va_arg(ap, uint8_t*);
-        txd_buffer[pt++] = '#';         // Start character
+                len = va_arg(ap, int);
+                ptr = 0;
+        }
+        while(len)
-        txd_buffer[pt++] = module;      // Address (a=0; b=1,...)
+        {
+                if(len)
+                {
+                        a = pdata[ptr++];
+                        len--;
+                        if((!len) && numofbuffers)
+                        {
+                                pdata = va_arg(ap, uint8_t*);
+                                len = va_arg(ap, int);
+                                ptr = 0;
+                                numofbuffers--;
+                        }
+                }
-        txd_buffer[pt++] = cmd;         // Command
+                else a = 0;
+                if(len)
+                {
+                        b = pdata[ptr++];
+                        len--;
+                        if((!len) && numofbuffers)
+                        {
+                                pdata = va_arg(ap, uint8_t*);
+                                len = va_arg(ap, int);
+                                ptr = 0;
+                                numofbuffers--;
+                        }
+                }
+                else b = 0;
+                if(len)
+                {
+                        c = pdata[ptr++];
+                        len--;
+                        if((!len) && numofbuffers)
+                        {
+                                pdata = va_arg(ap, uint8_t*);
+                                len = va_arg(ap, int);
-        while(len)
+                                ptr = 0;
                                 numofbuffers--;
                 if(len) { a = snd[ptr++]; len--;} else a = 0;
                 if(len) { b = snd[ptr++]; len--;} else b = 0;
-                if(len) { c = snd[ptr++]; len--;} else c = 0;
+                else c = 0;
+                txd_buffer[pt++] = '=' + (a >> 2);
-                txd_buffer[pt++] = '=' + (a >> 2);
+                txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
-                txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
+                txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
-                txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
+                txd_buffer[pt++] = '=' + ( c & 0x3f);
-                txd_buffer[pt++] = '=' + ( c & 0x3f);
+        }
         va_end(ap);
         AddCRC(pt); // add checksum after data block and initates the transmission
-}
+}
+// --------------------------------------------------------------------------
+void Decode64(void)
+{
-// --------------------------------------------------------------------------
+        uint8_t a,b,c,d;
-void Decode64(uint8_t *ptrOut, uint8_t len, uint8_t ptrIn, uint8_t max)
+        uint8_t x,y,z;
         uint8_t ptrIn = 3;
-        uint8_t a,b,c,d;
+        uint8_t ptrOut = 3;
-        uint8_t ptr = 0;
+        uint8_t len = ReceivedBytes - 6;
-        uint8_t x,y,z;
         while(len)
+        {
                 a = rxd_buffer[ptrIn++] - '=';
                 b = rxd_buffer[ptrIn++] - '=';
                 c = rxd_buffer[ptrIn++] - '=';
                 d = rxd_buffer[ptrIn++] - '=';
-                if(ptrIn > max - 2) break;
+                //if(ptrIn > ReceivedBytes - 3) break;
+                x = (a << 2) | (b >> 4);
                 y = ((b & 0x0f) << 4) | (c >> 2);
+                z = ((c & 0x03) << 6) | d;
                 x = (a << 2) | (b >> 4);
-                y = ((b & 0x0f) << 4) | (c >> 2);
+                if(len--) rxd_buffer[ptrOut++] = x; else break;
-                z = ((c & 0x03) << 6) | d;
+                if(len--) rxd_buffer[ptrOut++] = y; else break;
                 if(len--) rxd_buffer[ptrOut++] = z; else break;
                 if(len--) ptrOut[ptr++] = x; else break;
 void USART0_ProcessRxData(void)
+{
         // if data in the rxd buffer are not locked immediately return
         if(!rxd_buffer_locked) return;
-        uint8_t tmp_char_arr2[2]; // local char buffer
+        Decode64(); // decode data block in rxd_buffer
         switch(rxd_buffer[2])
         {
-           case 'w':// Attitude info from FC
+           case 'k':// Attitude info from FC
-                        Decode64((uint8_t *) &ExternData, sizeof(ExternData), 3, ReceivedBytes);
+                        memcpy(&ExternData, (uint8_t*)pRxData, sizeof(ExternData));
                         RequestFlags |= RQST_COMPASS_HEADING;
                         AttitudeSource = ATTITUDE_SOURCE_UART;
                         Orientation = ExternData.Orientation;
                         FC_Connected = 255;
                         break;
-        // used only for debug proposes at serial port
-                case 'b': // extern control
+        // used only for debug proposes at serial port
-                case 'c': // extern control with debug request
+                case 'b': // extern control
-                        Decode64((uint8_t *) &ExternControl,sizeof(ExternControl), 3,ReceivedBytes);
+                        memcpy(&ExternControl, (uint8_t*)pRxData, sizeof(ExternControl));
                         #define KEY1    0x01
                         #define KEY2    0x02
                         if(ExternControl.RemoteButtons & KEY2)
+                        {
                                 ExternData.CalState++;
                                 if(ExternData.CalState == 6) ExternData.CalState = 0;
+                        }
+                        ConfirmFrame = ExternControl.Frame;
                         PC_Connected = 255;
                         break;
-                // get debug values
-                case 'f':
+                case 'd': // request for the debug data
-                        RequestFlags |= RQST_DEBUG_DATA;
-                        PC_Connected = 255;
-                        break;
+                        DebugData_Interval = (uint16_t) pRxData[0] * 10;
-                // no reaction for display line request
-                case 'h':// x-1 display columns
-                        PC_Connected = 255;
+                        if(DebugData_Interval) RequestFlags |= RQST_DEBUG_DATA;
                         break;
-                case 'v': // get version and board release
+                case 'v': // request version and board release
                         RequestFlags |= RQST_VERSION_INFO;
                         PC_Connected = 255;
-                        break;
+                        break;
                 case 'a':// Labels of the Analog Debug outputs
-                        Decode64((uint8_t *) &tmp_char_arr2[0], sizeof(tmp_char_arr2), 3, ReceivedBytes);
+                case 'a':// Labels of the Analog Debug outputs
-                        RequestDebugLabel = tmp_char_arr2[0];
+                        RequestDebugLabel = pRxData[0];
                         RequestFlags |= RQST_DEBUG_LABEL;
                         PC_Connected = 255;
                         break;
                 case 'g':// get extern control data
                         RequestFlags |= RQST_EXTERN_CTRL;
+                        PC_Connected = 255;
+                        break;
                         PC_Connected = 255;
-                        break;
+        // unlock the rxd buffer after processing
+{
         if(!(UCSR0B & (1 << TXEN0))) return;
         if(!txd_complete) return;
-        if(CheckDelay(Debug_Timer) || (RequestFlags & RQST_DEBUG_DATA))
-        {
+        if((RequestFlags & RQST_DEBUG_LABEL) && txd_complete)
-                SetDebugValues();
+        {
-                SendOutData('D',MySlaveAddr,(uint8_t *) &DebugOut,sizeof(DebugOut));
+                SendOutData('A', MySlaveAddr, 2, (uint8_t *)&RequestDebugLabel, sizeof(RequestDebugLabel), (uint8_t *) ANALOG_LABEL[RequestDebugLabel], 16);
-                Debug_Timer = SetDelay(250);
+                RequestDebugLabel = 0xFF;
-                RequestFlags &= ~RQST_DEBUG_DATA;
+                RequestFlags &= ~RQST_DEBUG_LABEL;
+        }
-        if(RequestFlags & RQST_DEBUG_LABEL)
+        if(ConfirmFrame && txd_complete)
-        {
-                SendOutData('A',RequestDebugLabel + '0',(uint8_t *) ANALOG_LABEL[RequestDebugLabel],16);
                 RequestDebugLabel = 255;
-                RequestFlags &= ~RQST_DEBUG_LABEL;
+                SendOutData('B',MySlaveAddr, 1, (uint8_t *) &ConfirmFrame, sizeof(ConfirmFrame));
-                Debug_Timer = SetDelay(500);
+                ConfirmFrame = 0;
+        }
-        }
+        if(( (DebugData_Interval && CheckDelay(DebugData_Timer)) || (RequestFlags & RQST_DEBUG_DATA)) && txd_complete)
+        {
-        if(RequestFlags & RQST_VERSION_INFO)
+                SetDebugValues();
+                SendOutData('D', MySlaveAddr, 1, (uint8_t *) &DebugOut, sizeof(DebugOut));
-        {
+                DebugData_Timer = SetDelay(DebugData_Interval);
-                SendOutData('V',MySlaveAddr,(uint8_t *) &VersionInfo, sizeof(VersionInfo));
+                RequestFlags &= ~RQST_DEBUG_DATA;
-                RequestFlags &= ~RQST_VERSION_INFO;
         }
         if(RequestFlags & RQST_EXTERN_CTRL)
-        {
+        if((RequestFlags & RQST_EXTERN_CTRL) && txd_complete)
                 SetDebugValues();
-                SendOutData('G',MySlaveAddr,(uint8_t *) &ExternControl,sizeof(ExternControl));
+                SendOutData('G',MySlaveAddr, 1, (uint8_t *) &ExternControl,sizeof(ExternControl));
                 RequestFlags &= ~RQST_EXTERN_CTRL;
+        }
+        if((RequestFlags & RQST_COMPASS_HEADING) && txd_complete)
+        {
+                SendOutData('K',MySlaveAddr, 1, (uint8_t *) &I2C_Heading, sizeof(I2C_Heading));
+                RequestFlags &= ~RQST_COMPASS_HEADING;
+        }
-        if(RequestFlags & RQST_COMPASS_HEADING)
+        if((RequestFlags & RQST_VERSION_INFO) && txd_complete)
+        {

Subversion Repositories MK3Mag

(root)/tags/V0.22c/uart.c @ 76 - Rev 39 → 41