Diff: demodulator.cpp

Revision:

3:204e23521e39

Parent:

2:124a066878cc

Child:

4:af325c921e79

--- a/demodulator.cpp	Tue Feb 11 04:23:35 2014 +0000
+++ b/demodulator.cpp	Tue Feb 11 06:37:34 2014 +0000
@@ -1,24 +1,73 @@
 #include "main.h"
 
 extern Serial pc;
-extern Queue<int, 16> ReaderQueue;
+extern Queue<string, 16> WriterQueue;
 extern Mail<message_t, 16> mailBox;
+message_t gTrame;
+string dataTrame;
+
 
-void Demodulator_init()
+bool verifyCRC()
 {
+    bool result = false;
     
+    dataTrame = "";
+    string crcTrame = "";
+    dataTrame = gTrame.trame.substr(32, gTrame.size);
+    crcTrame = gTrame.trame.substr(32+gTrame.size, 16);
+    
+    char *a = (char*)dataTrame.c_str();
+    uint16_t crc = calculate_crc16(a, sizeof(a));
+    string crc_s = bitset<16>(crc).to_string<char,string::traits_type,string::allocator_type>();
+    
+    if(crc_s == crcTrame)
+    {
+        result = true;    
+    }
+    
+    return result;
+     //pc.printf("\n\r%s",dataTrame);
+     //pc.printf("\n\r%s",crcTrame);
+}
+
+string rebuildMessage()
+{
+    string message;
+    int tab[8] = {128, 64, 32, 16, 8, 4, 2, 1};
+    int asciiValue = 0;
+
+    for(int i=0; i<gTrame.size; i+=OCTET)
+    {
+        for(int y=0; y<OCTET; y++)
+        {
+            asciiValue += (dataTrame.c_str()[i+y]-48)* tab[y];
+        }
+        message += asciiValue;
+        asciiValue = 0;
+    }  
+    
+     return message;
 }
 
 void Demodulator_thread(void const *args)
-{
-    Demodulator_init();
-    
+{  
     while(1)
     {
         osEvent evt = mailBox.get();
-        if (evt.status == osEventMail) {
+        if (evt.status == osEventMail) 
+        {
             message_t *mbTrame = (message_t*)evt.value.p;
-            pc.printf("\n\r%s", mbTrame->trame);
-            mailBox.free(mail);
+            gTrame.trame = mbTrame->trame;
+            gTrame.size = mbTrame->size;
+            mailBox.free(mbTrame);
+            
+            //pc.printf("\n\r HAHAHA %s", gTrame.trame.c_str());
+            
+            //pc.printf("\n\r%s", mbTrame->trame);
+            if(verifyCRC())
+            {
+                WriterQueue.put(new string(rebuildMessage())); 
+            }          
+        }
     }
 }
\ No newline at end of file