WebSVN - Projects - Diff - Rev 440 and 449 - /QMK-Groundstation/branches/libMK/libMK/Parser.cpp

 #include <Parser.h>
+/**
+ * create a frame that can be send to the MK using the
+ * connection class.
+ * see http://www.mikrokopter.com/ucwiki/en/SerialProtocol
+ * how the protocol is encoded and
+ * see http://www.mikrokopter.com/ucwiki/en/SerialCommands
+ * to look at the possible commands that are already coded
+ * in data
+ */
+void Parser::create_frame(char cmd, int address, char * data, unsigned int length) {
+        //if # is cmd we do not touch anything, because
+        // the command is already encoded in data
+        if (cmd != '#') {
+            /*
+            //calculate buffer length
+            //(4 Bytes for 1-byte '#', 1-byte address, and 2-byte crc)
+            //(the rest for the data length - see encode how this is calculated)
+            int buff_len = 4+(length/3 + (length%3==0?0:1) )*4;
+            //allociate memory for the data we want to send
+            char * send_data = (char *)malloc(buf_len);
+            */
+            char send_data[150];
+            send_data[0]='#';
+            send_data[1]=(char)address;
+            send_data[2]=cmd;
+            for (int i = 0; i < length; i++)
+                send_data[i+3] = data[i];
+            //TODO: abgleich mit MKCommunication::send_command
+            Parser::encode64(send_data, length);
+            address = 'a' + address;
+        }
+}
+/**
-// Base64 Decoder
+ * Base64 Decoder
+ * see Parser.h for details about sRxData
+ * data = data that will be decoded
+ * len = length of data
-// see Parser.h for details about sRxData
+ * ptrOut = pointer to decoded data
+ * offset = offset in data
+ */
-bool Parser::decode64(sRxData &rx)
+int Parser::decode64(char * data, int len, unsigned char *ptrOut, int offset)
-{
     int length = rx.str.size();
     unsigned char a,b,c,d;
     unsigned char ptr = 0;
     unsigned char x,y,z;
-    int ptrOut[150];
-    int ptrIn = 3;
-    int max = length;
     int len = length;
     int decLen = 0;
+/*
+    //FIXME: dies wieder einklammern!
-    if (rx.input[ptrIn] == 0) {
+    if (data[ptrIn] == 0) {
-        return false;
+        return -1;
         //TODO: catch error to show that something went wrong during the decode process
         //throw "Nothing received";
+    }
+*/
+    //decode data
+    while(len) {
+        a = data[offset++] - '=';
+        b = data[offset++] - '=';
+        c = data[offset++] - '=';
+        d = data[offset++] - '=';
-    while(len != 0) {
-        a = rx.input[ptrIn++] - '=';
-        b = rx.input[ptrIn++] - '=';
-        c = rx.input[ptrIn++] - '=';
-        d = rx.input[ptrIn++] - '=';
-        if(ptrIn > max - 2) break;
+        //if(offset > max - 2) break;
         x = (a << 2) | (b >> 4);
         y = ((b & 0x0f) << 4) | (c >> 2);
         z = ((c & 0x03) << 6) | d;
         if(len--) ptrOut[ptr++] = x; else break;
         if(len--) ptrOut[ptr++] = y; else break;
         if(len--) ptrOut[ptr++] = z; else break;
+    }
+    //decoded data
+    unsigned char * decData;
     for (int a=0; a<ptr; a++) {
-        if (length == false) {
+        if (len) {
+            decData[decLen] = ptrOut[a];
+            decLen++;
+        } else {
             int b1, b2, b3;
             b1 = ptrOut[a++];
             b2 = ptrOut[a];
             b3 = (b2 << 8) | b1;
             if (b3 > 32767)
                 b3 = b3 - 65536;
-            rx.decode[decLen] = b3;
-            decLen++;
-        } else {
-            rx.decode[decLen] = ptrOut[a];
+            decData[decLen] = b3;
             decLen++;
-        }
         rx.decLen = decLen;
+    }
-    }
+    ptrOut = decData;
-    return true;
+    return decLen;
+}
-}
+/**
+ * base64 encoder
-// base64 encoder
+ */
-string Parser::encode64(char data[150],unsigned int length)
+void Parser::encode64(char data[150],unsigned int length)
+{
     unsigned int pt = 0;
     unsigned char a,b,c;
     unsigned char ptr = 0;
     char tx_buff[150];
     while(length > 0)
+    {
         if(length) { a = data[ptr++]; length--;} else a = 0;
         if(length) { b = data[ptr++]; length--;} else b = 0;
         if(length) { c = data[ptr++]; length--;} else c = 0;
         tx_buff[pt++] = '=' + (a >> 2);
         tx_buff[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
         tx_buff[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
         tx_buff[pt++] = '=' + ( c & 0x3f);
+    }
     tx_buff[pt] = 0;
+    //move pointer of tx_buff to data
-    return string(tx_buff);
+    data = tx_buff;
+}
 // Datensatz nach 8bit Integer
 int Parser::dataToChar(int *data , int start, bool is_signed) {
     int out = (data[start]);
     if ((out > 128) && (is_signed))
       out = out - 256;
     return out;
+}
+/**
+ * received char to 8 bit integer
-// received char to 8 bit integer
+ */
 int Parser::charToData(int data) {
     if (data < 0)
         return data + 256;
     return data;
+}
+/**
+ * convert data to 16bit Integer
-// convert data to 16bit Integer
+ */
 int Parser::dataToInt(int *Data , int Start, bool is_signed)
+{
     int Out = (Data[Start+1]<<8) | (Data[Start+0]);
     if ((Out > 32767) && (is_signed))
       Out = Out - 65536;
     return Out;
+}
+/**
+ * convert data to 32bit Long
-// convert data to 32bit Long
+ */
 long Parser::dataToLong(int *Data , int Start, bool is_signed)
+{
     long Out = (Data[Start+3]<<24) | (Data[Start+2]<<16) | (Data[Start+1]<<8) | (Data[Start+0]);
     if ((Out > 32767) && (is_signed))
       Out = Out;
     return Out;
+}
 float Parser::getFloat(long value, int count)
+{
     long num = pow(10, count);
     float temp = value;
     return temp / num;
+}
-string Parser::dataToString(int Data[150], int Start, int End)
-{
-    char String[150];
-    for (int a = Start; a < End; a++)
+/**
-    {
-        String[a - Start] = Data[a];
+ * check CRC
-    }
-    String[End - Start] = '\0';
-    return string(String);
-}
-// check CRC
+ */
-bool Parser::check_CRC(string RXstr)
+bool Parser::check_CRC(char * rx, int length)
-{
     int length = RXstr.size();
-    int CRC = 0;
-    char *RX = (char *)RXstr.c_str();
+    int CRC = 0;
-    if (RX[1] == 127)
+    if (rx[1] == 127)
+    {
-        RX[1] = 0;
+        rx[1] = 0;
+    }
     for(int i=0; i < length-2; i++)
+    {
-            CRC+=RX[i];
+            CRC+=rx[i];
+    }
     CRC = CRC % 4096;
-    if(RX[length - 2] != ('=' + (CRC / 64)))
+    if(rx[length - 2] != ('=' + (CRC / 64)))
+    {
         return false;
+    }
-    if(RX[length - 1] != ('=' + CRC % 64))
+    if(rx[length - 1] != ('=' + CRC % 64))
+    {
         return false;
+    }
     return true;
+}
+/**
+ * create CRC and add it to tx
-// add CRC
+ */
-string Parser::add_CRC(string TX)
+void Parser::add_CRC(char * tx, int length)
-{
     int length = TX.size();
-    unsigned int tmpCRC = 0;
-    char CRC[2];
+    unsigned int tmpCRC = 0;
     for(int i = 0; i < length; i++)
+    {
-        tmpCRC += TX[i];
+        tmpCRC += tx[i];
+    }
     tmpCRC %= 4096;
-    CRC[0] = '=' + tmpCRC / 64;
+    tx[length-2] = '=' + tmpCRC / 64;
-    CRC[1] = '=' + tmpCRC % 64;
+    tx[length-1] = '=' + tmpCRC % 64;
-    CRC[2] = '\0';
-    return string(TX) + string(CRC);
+    tx[length] = '\0';
+}

Subversion Repositories Projects

(root)/QMK-Groundstation/branches/libMK/libMK/Parser.cpp @ 450 - Rev 440 → 449