Diff: ssWi.cpp

Revision:

17:8ba1b278b407

Parent:

16:b50c3b3b241c

Child:

18:133e42dc82b0

--- a/ssWi.cpp	Thu Mar 07 15:44:05 2013 +0000
+++ b/ssWi.cpp	Wed Jan 18 21:01:25 2017 +0000
@@ -13,7 +13,6 @@
 #include <map>
 
 
-
 /** \brief first byte of the header
  *
  * The header is composed of 3 bytes
@@ -44,13 +43,13 @@
  * This object represent the communication channel that can be
  * different means
  */
-ssWiChannel* channel = NULL;
+static ssWiChannel* channel = NULL;
 
 /** \brief serialize read and write operations
  *
  * simultaneous multiple accesses to the mean are avoided
  */
-Mutex mutexChannel;
+static Mutex mutexChannel;
 
 /** \brief set of registered communication ports
  *
@@ -60,202 +59,109 @@
 /** \brief transmission rate
  *
  */
-int TXRate;
+static int TXRate;
 
 /** \brief reading rate
  *
  */
-int RXRate;
+static int RXRate;
 
+void functionSender ();
+void functionReceiver ();
 
-/*
-Thread* sender;
-Thread* receiver;
+// threads
+static Thread thread_sender(osPriorityRealtime);
+static Thread thread_receiver(osPriorityRealtime);
 
-void threadSender (void const* arg);
-void threadReceiver (void const* arg);
-
+    
 bool ssWi_init (ssWiChannel* c, int rateTX, int rateRX)
 {
-    if (channel!=NULL)
+    // protocol cannot be restarted once started
+    if (channel != NULL || rateTX <= 0 || rateRX <= 0)
         return false;
-    TXRate = 1000.0/rateTX;
-    RXRate = 1000.0/rateRX;
-    mutexChannel.lock();
+
     channel = c;
-    mutexChannel.unlock();
-    sender = new Thread(threadSender);
-    receiver = new Thread(threadReceiver);
+
+    TXRate = 1000 / rateTX;
+    RXRate = 1000 / rateRX;
+
+    thread_sender.start(&functionSender);
+    thread_receiver.start(&functionReceiver);
+
     return true;
 }
 
-void threadSender (void const* arg)
+void functionSender ()
 {
-    static char buffer[INTERNAL_BUFFER_SIZE];
-    while(true) {
+    while (1) {
+        static char buffer[INTERNAL_BUFFER_SIZE];
         int n = 3;
         int numFrames = 0;
-        for (std::map<int, ssWiPort>::iterator it = ports.begin(); it!=ports.end(); it++) {
+        for (std::map<int, ssWiPort>::iterator it = ports.begin();
+                                                      it!=ports.end(); it++) {
             if ((*it).second.isModified()) {
                 buffer[n++] = (*it).first;
                 PortValue tmp = (*it).second.getTXValue();
                 memcpy(&buffer[n], &tmp, sizeof(PortValue));
                 n += sizeof(PortValue);
-                //printf("Write[%d] = %d\n\r", (*it).first, tmp);
                 numFrames++;
             }
         }
-        if (numFrames>0) {
+        if (numFrames > 0) {
             buffer[0] = START_0;
             buffer[1] = START_1;
             buffer[2] = START_2;
             mutexChannel.lock();
             channel->write(buffer, n);
             mutexChannel.unlock();
-//            printf("W(%d): ", n);
-//            for(int k=0; k<n; k++)
-//                printf("%d ", buffer[k]);
-//            printf("\n\r");
         }
         Thread::wait(TXRate);
     }
 }
 
-void threadReceiver (void const* arg)
+void functionReceiver ()
 {
-    static char buffer[INTERNAL_BUFFER_SIZE];
-    while(true) {
-        printf("...read...\n\r");
+    while (1) {
+        static char buffer[INTERNAL_BUFFER_SIZE];
         mutexChannel.lock();
         int n = channel->read(buffer);
         mutexChannel.unlock();
-//        if (n>0) {
-//            printf("R(%d): ", n);
-//              for(int k=0; k<n; k++)
-//                   printf("%d ", buffer[k]);
-//              printf("\n\r");
-//        }
         bool alreadyIn = false;
-        for (int i=0; i<(n-2);) {
+        for (int i=0; i < (n - 2); ) {
             bool found = false;
-            if (buffer[i]==START_0 && buffer[i+1]==START_1 && buffer[i+2]==START_2) {
+            if (buffer[i] == START_0 && buffer[i+1] == START_1 && 
+                                                      buffer[i+2] == START_2) {
                 found = true;
                 alreadyIn = true;
                 i += 3;
             }
-            if ((found || alreadyIn) && (n-i)>=(sizeof(PortID)+sizeof(PortValue))) {
+            if ((found || alreadyIn) && (n-i) >= (sizeof(PortID) + 
+                                                          sizeof(PortValue))) {
                 PortID port = buffer[i++];
                 PortValue value = 0;
                 memcpy(&value, &buffer[i], sizeof(PortValue));
-//                printf("Read[%d] = %d\n\r", port, value);
                 i += sizeof(PortValue);
-                if (ports.find(port)!=ports.end())
+                if (ports.find(port) != ports.end())
                     ports[port].setRXValue(value);
             }
         }
         Thread::wait(RXRate);
     }
 }
-*/
-
-
-void functionSender (void const* arg);
-void functionReceiver (void const* arg);
-
-RtosTimer *senderTimer;
-RtosTimer *receiverTimer;
-
-bool ssWi_init (ssWiChannel* c, int rateTX, int rateRX)
-{
-    if (channel!=NULL)
-        return false;
-    mutexChannel.lock();
-    channel = c;
-    mutexChannel.unlock();
-    senderTimer = new RtosTimer(functionSender, osTimerPeriodic, NULL);
-    receiverTimer = new RtosTimer(functionReceiver, osTimerPeriodic, NULL);
-    if (rateTX>0) {
-        TXRate = 1000/rateTX;
-        senderTimer->start(TXRate);
-    } else
-       TXRate = 0;
-    if (rateRX>0) {
-        RXRate = 1000/rateRX;
-        receiverTimer->start(RXRate);
-    } else
-       RXRate = 0;
-    return true;
-}
-
-void functionSender (void const* arg)
-{
-    static char buffer[INTERNAL_BUFFER_SIZE];
-    int n = 3;
-    int numFrames = 0;
-    for (std::map<int, ssWiPort>::iterator it = ports.begin(); it!=ports.end(); it++) {
-        if ((*it).second.isModified()) {
-            buffer[n++] = (*it).first;
-            PortValue tmp = (*it).second.getTXValue();
-            memcpy(&buffer[n], &tmp, sizeof(PortValue));
-            n += sizeof(PortValue);
-            numFrames++;
-        }
-    }
-    if (numFrames>0) {
-        buffer[0] = START_0;
-        buffer[1] = START_1;
-        buffer[2] = START_2;
-        if (!mutexChannel.trylock())
-            return;
-        channel->write(buffer, n);
-        mutexChannel.unlock();
-    }
-}
-
-void functionReceiver (void const* arg)
-{
-    static char buffer[INTERNAL_BUFFER_SIZE];
-    if (!mutexChannel.trylock())
-        return;
-    int n = channel->read(buffer);
-    mutexChannel.unlock();
-    bool alreadyIn = false;
-    for (int i=0; i<(n-2);) {
-        bool found = false;
-        if (buffer[i]==START_0 && buffer[i+1]==START_1 && buffer[i+2]==START_2) {
-            found = true;
-            alreadyIn = true;
-            i += 3;
-        }
-        if ((found || alreadyIn) && (n-i)>=(sizeof(PortID)+sizeof(PortValue))) {
-            PortID port = buffer[i++];
-            PortValue value = 0;
-            memcpy(&value, &buffer[i], sizeof(PortValue));
-            i += sizeof(PortValue);
-            if (ports.find(port)!=ports.end())
-                ports[port].setRXValue(value);
-        }
-    }
-}
-
-
-
 
 inline bool ssWi_isActive (PortID port)
 {
-    return channel!=NULL && ports.find(port)!=ports.end();
+    return channel != NULL && ports.find(port) != ports.end();
 }
 
-
 bool ssWi_setPort (PortID port)
 {
-    if (channel==NULL)
+    if (channel == NULL)
         return false;
     ports[port];
     return true;
 }
 
-
 bool ssWi_unsetPort (PortID port)
 {
     if (!ssWi_isActive(port))