Diff: modules/CommModule/CommModule.cpp

Revision:

6:4a3dbfbc30f1

Parent:

5:146523a0d1f4

--- a/modules/CommModule/CommModule.cpp	Wed Jan 14 17:46:44 2015 +0000
+++ b/modules/CommModule/CommModule.cpp	Thu Jan 15 07:15:33 2015 +0000
@@ -83,17 +83,11 @@
         evt = osMailGet(inst->_rxQueue, osWaitForever);
 
         if (evt.status == osEventMail) {
+            DigitalOut led(LED4, 1);
 
             // get a pointer to where the data is stored
             p = (RTP_t*)evt.value.p;
 
-            /*
-            if (inst->p.port) {
-                // write the received packet to it's respective port's queue
-                inst->rxQueue[p->port]
-            }
-            */
-
             // If there is an open socket for the port, call it.
             if (std::binary_search(inst->_open_ports->begin(), inst->_open_ports->end(), p->port)) {
                 inst->_rx_handles[p->port].call();
@@ -102,7 +96,7 @@
             EVENT("Reception: \r\n  Port: %u\r\n  Subclass: %u\r\n", p->port, p->subclass);
 
             osMailFree(inst->_rxQueue, p);  // free memory allocated for mail
-            
+
         }
     }
 }
@@ -180,14 +174,16 @@
 {
     static bool isReady = false;
 
-    if (!isReady) {
-        isReady = true;
+    if (isReady) {
+        return;
+    }
 
-        _open_ports = new std::vector<uint8_t>;
+    isReady = true;
 
-        osSignalSet(_txID, COMM_MODULE_SIGNAL_START_THREAD);
-        osSignalSet(_rxID, COMM_MODULE_SIGNAL_START_THREAD);
-    }
+    _open_ports = new std::vector<uint8_t>;
+
+    osSignalSet(_txID, COMM_MODULE_SIGNAL_START_THREAD);
+    osSignalSet(_rxID, COMM_MODULE_SIGNAL_START_THREAD);
 }
 
 
@@ -204,6 +200,7 @@
         // =================
         p->port = packet.port;
         p->subclass = packet.subclass;
+        p->data_size = packet.data_size;
 
         for (int i=0; i<(sizeof(packet)/sizeof(uint8_t)); i++)
             p->data[i] = packet.data[i];
@@ -231,7 +228,7 @@
         p->port = packet.port;
         p->subclass = packet.subclass;
 
-        for (int i=0; i<(sizeof(packet)/sizeof(uint8_t)); i++)
+        for (int i=0; i<packet.data_size; i++)
             p->data[i] = packet.data[i];
 
         // [X] - 1.3 - Place the passed packet into the txQueue.