ssWi.cpp

00001 /** \file ssWi.cpp
00002  *
00003  *  \brief implementation of the internal functions for mananing the protocol
00004  *
00005  */
00006  
00007 #include "ssWiChannel.hpp"
00008 #include "ssWiPort.hpp"
00009 #include "ssWi.hpp"
00010 
00011 #include "mbed.h"
00012 #include "rtos.h"
00013 
00014 #include <map>
00015 
00016 
00017 /** \brief first byte of the header
00018  *
00019  * The header is composed of 3 bytes: START_0 START_1 START_2
00020  */
00021 #define START_0 255
00022 #define START_1 130
00023 #define START_2 255
00024 
00025 
00026 /** 
00027  * dimension of the buffers used by the sender and receiver procedure
00028  */ 
00029 #define INTERNAL_BUFFER_SIZE 100
00030 
00031 
00032 /** \brief channel abstraction
00033  *
00034  * This object represent the communication channel. It can be different means
00035  */
00036 ssWiChannel* channel = NULL;
00037 
00038 /** \brief serialize read and write operations
00039  *
00040  * simultaneous multiple accesses to the mean are avoided
00041  */
00042 Mutex mutexChannel;
00043 
00044 /** \brief set of registered communication ports
00045  *
00046  */
00047 std::map<int, ssWiPort> ports;
00048 
00049 /** \brief transmission rate
00050  *
00051  */
00052 int TXRate;
00053 
00054 /** \brief reading rate
00055  *
00056  */
00057 int RXRate;
00058 
00059 
00060 void functionSender();
00061 void functionReceiver();
00062 
00063 static Thread readingThread;
00064 static Thread writingThread;
00065 
00066 bool ssWi_init (ssWiChannel* c, int rateTX, int rateRX)
00067 {
00068     if (channel!=NULL)
00069         return false;
00070     
00071     // nothing else is using the channel
00072     channel = c;
00073     TXRate = rateTX > 0 ? 1000/rateTX : 0;
00074     RXRate = rateRX > 0 ? 1000/rateRX : 0;
00075     // start txer and rxer
00076     readingThread.start(functionReceiver);
00077     writingThread.start(functionSender);
00078 
00079     return true;
00080 }
00081 
00082 void functionSender() {
00083     static char buffer[INTERNAL_BUFFER_SIZE];
00084 
00085     while (true) {
00086         int n = 3;
00087         int numFrames = 0;
00088         
00089         // copy data
00090         for (std::map<int, ssWiPort>::iterator it = ports.begin();
00091                                                       it != ports.end(); it++) {
00092             if ((n + sizeof(PortValue) + sizeof(PortID)) > INTERNAL_BUFFER_SIZE)
00093                 break;
00094             if ((*it).second.isModified()) {
00095                 buffer[n++] = (*it).first;
00096                 PortValue tmp = (*it).second.getTXValue();
00097                 memcpy(&buffer[n], &tmp, sizeof(PortValue));
00098                 n += sizeof(PortValue);
00099                 numFrames++;
00100             }
00101         }
00102         // add header and send
00103         if (numFrames > 0) {
00104             buffer[0] = START_0;
00105             buffer[1] = START_1;
00106             buffer[2] = START_2;
00107             mutexChannel.lock();
00108             channel->write(buffer, n);
00109             mutexChannel.unlock();
00110         }
00111         
00112         thread_sleep_for(TXRate);
00113     }
00114 }
00115 
00116 void functionReceiver ()
00117 {
00118     static char buffer[INTERNAL_BUFFER_SIZE];
00119     int offset = 0;
00120 
00121     while(1) {
00122         thread_sleep_for(RXRate);
00123 
00124         // read buffer
00125         mutexChannel.lock();
00126         int n = channel->read(buffer + offset);
00127         mutexChannel.unlock();
00128         // not enough to find the header, merge with next message as the buffer 
00129         // can contain part of the header
00130         if (n < 3) {
00131             offset = n;
00132             continue;
00133         }
00134         // find header
00135         int index = -1;
00136         for (int i=0; i<(n-2); i++) {
00137             if (buffer[i]==START_0 && buffer[i+1]==START_1 && buffer[i+2]==START_2) {
00138                 index = i + 3;
00139                 break;
00140             }
00141         }
00142         // header not found, discard everthing
00143         offset = 0;
00144         if (index < 0)
00145             continue;
00146         // read samples
00147         while((index + sizeof(PortID) + sizeof(PortValue)) <= n) {
00148             PortID port = buffer[index++];
00149             PortValue value = 0;
00150             memcpy(&value, &buffer[index], sizeof(PortValue));
00151             index += sizeof(PortValue);
00152             if (ports.find(port)!=ports.end())
00153                 ports[port].setRXValue(value);
00154         }
00155     }
00156 }
00157 
00158 
00159 inline bool ssWi_isActive (PortID port)
00160 {
00161     return channel != NULL && ports.find(port) != ports.end();
00162 }
00163 
00164 
00165 bool ssWi_setPort (PortID port)
00166 {
00167     if (channel == NULL)
00168         return false;
00169         
00170     ports[port];
00171     
00172     return true;
00173 }
00174 
00175 
00176 bool ssWi_unsetPort (PortID port)
00177 {
00178     if (!ssWi_isActive(port))
00179         return false;
00180 
00181     ports.erase(port);
00182 
00183     return true;
00184 }