SensorNetwork.cpp

00001 /**************************************************************************************
00002  * Copyright (c) 2016, Tomoaki Yamaguchi
00003  *
00004  * All rights reserved. This program and the accompanying materials
00005  * are made available under the terms of the Eclipse Public License v1.0
00006  * and Eclipse Distribution License v1.0 which accompany this distribution.
00007  *
00008  * The Eclipse Public License is available at
00009  *    http://www.eclipse.org/legal/epl-v10.html
00010  * and the Eclipse Distribution License is available at
00011  *   http://www.eclipse.org/org/documents/edl-v10.php.
00012  *
00013  * Contributors:
00014  *    Tomoaki Yamaguchi - initial API and implementation 
00015  **************************************************************************************/
00016 
00017 #include <stdio.h>
00018 #include <unistd.h>
00019 #include <termios.h>
00020 #include <fcntl.h>
00021 #include <string.h>
00022 #include <stdlib.h>
00023 #include <sys/time.h>
00024 #include <sys/types.h>
00025 #include <sys/select.h>
00026 #include "SensorNetwork.h"
00027 #include "MQTTSNGWProcess.h"
00028 
00029 using namespace std;
00030 using namespace MQTTSNGW;
00031 
00032 /*===========================================
00033  Class  SensorNetAddreess
00034  ============================================*/
00035 SensorNetAddress::SensorNetAddress()
00036 {
00037     memset(_address64, 0, 8);
00038     memset(_address16, 0, 2);
00039 }
00040 
00041 SensorNetAddress::~SensorNetAddress()
00042 {
00043 
00044 }
00045 
00046 void SensorNetAddress::setAddress(uint8_t* address64, uint8_t* address16)
00047 {
00048     memcpy(_address64, address64, 8);
00049     memcpy(_address16, address16, 2);
00050 }
00051 
00052 
00053 int SensorNetAddress::setAddress(string* address64)
00054 {
00055     memcpy(_address64, address64->c_str(), 8);
00056     memset(_address16, 0, sizeof(_address16));
00057     return 0;
00058 }
00059 
00060 void SensorNetAddress::setBroadcastAddress(void)
00061 {
00062     memset(_address64, 0, 6);
00063     _address64[6] = 0xff;
00064     _address64[7] = 0xff;
00065     _address16[0] = 0xff;
00066     _address16[1] = 0xfe;
00067 }
00068 
00069 bool SensorNetAddress::isMatch(SensorNetAddress* addr)
00070 {
00071 
00072     return (memcmp(this->_address64, addr->_address64, 8 ) == 0 &&  memcmp(this->_address16, addr->_address16, 2) == 0);
00073 }
00074 
00075 SensorNetAddress& SensorNetAddress::operator =(SensorNetAddress& addr)
00076 {
00077     memcpy(_address64, addr._address64, 8);
00078     memcpy(_address16, addr._address16, 2);
00079     return *this;
00080 }
00081 
00082 char* SensorNetAddress::sprint(char* buf)
00083 {
00084     char* pbuf = buf;
00085     for ( int i = 0; i < 8; i++ )
00086     {
00087         sprintf(pbuf, "%02X", _address64[i]);
00088         pbuf += 2;
00089     }
00090     return buf;
00091 }
00092 
00093 /*===========================================
00094  Class  SensorNetwork
00095  ============================================*/
00096 SensorNetwork::SensorNetwork()
00097 {
00098 
00099 }
00100 
00101 SensorNetwork::~SensorNetwork()
00102 {
00103 
00104 }
00105 
00106 int SensorNetwork::unicast(const uint8_t* payload, uint16_t payloadLength, SensorNetAddress* sendToAddr)
00107 {
00108     return XBee::unicast(payload, payloadLength, sendToAddr);
00109 }
00110 
00111 int SensorNetwork::broadcast(const uint8_t* payload, uint16_t payloadLength)
00112 {
00113     return XBee::broadcast(payload, payloadLength);
00114 }
00115 
00116 int SensorNetwork::read(uint8_t* buf, uint16_t bufLen)
00117 {
00118     return XBee::recv(buf, bufLen, &_clientAddr);
00119 }
00120 
00121 int SensorNetwork::initialize(void)
00122 {
00123     char param[MQTTSNGW_PARAM_MAX];
00124     uint32_t baudrate = 9600;
00125     uint8_t apimode = 2;
00126 
00127     if (theProcess->getParam("ApiMode", param) == 0)
00128     {
00129         apimode = (uint8_t)atoi(param);
00130     }
00131     setApiMode(apimode);
00132     _description = "API mode ";
00133     sprintf(param, "%d", apimode);
00134     _description += param;
00135 
00136     if (theProcess->getParam("Baudrate", param) == 0)
00137     {
00138         baudrate = (uint32_t)atoi(param);
00139     }
00140     _description += ", Baudrate ";
00141     sprintf(param ,"%d", baudrate);
00142     _description += param;
00143 
00144     theProcess->getParam("SerialDevice", param);
00145     _description += ", SerialDevice ";
00146     _description += param;
00147 
00148     return XBee::open(param, baudrate);
00149 }
00150 
00151 const char* SensorNetwork::getDescription(void)
00152 {
00153     return _description.c_str();
00154 }
00155 
00156 SensorNetAddress* SensorNetwork::getSenderAddress(void)
00157 {
00158     return &_clientAddr;
00159 }
00160 
00161 /*===========================================
00162               Class  XBee
00163  ============================================*/
00164 XBee::XBee(){
00165     _serialPort = new SerialPort();
00166     _respCd = 0;
00167     _respId = 0;
00168     _dataLen = 0;
00169     _frameId = 0;
00170     _apiMode = 2;
00171 }
00172 
00173 XBee::~XBee(){
00174     if ( _serialPort )
00175     {
00176         delete _serialPort;
00177     }
00178 }
00179 
00180 int XBee::open(char* device, int baudrate)
00181 {
00182     int rate = B9600;
00183 
00184     switch (baudrate)
00185     {
00186     case 9600:
00187         rate = B9600;
00188         break;
00189     case 19200:
00190         rate = B19200;
00191         break;
00192     case 38400:
00193         rate = B38400;
00194         break;
00195     case 57600:
00196         rate = B57600;
00197         break;
00198     case 115200:
00199         rate = B115200;
00200         break;
00201     default:
00202         return -1;
00203     }
00204 
00205     return _serialPort->open(device, rate, false, 1, O_RDWR | O_NOCTTY);
00206 }
00207 
00208 int XBee::broadcast(const uint8_t* payload, uint16_t payloadLen){
00209     SensorNetAddress addr;
00210     addr.setBroadcastAddress();
00211     return send(payload, (uint8_t) payloadLen, &addr);
00212 }
00213 
00214 int XBee:: unicast(const uint8_t* payload, uint16_t payloadLen, SensorNetAddress* addr){
00215     return send(payload, (uint8_t) payloadLen, addr);
00216 }
00217 
00218 int XBee::recv(uint8_t* buf, uint16_t bufLen, SensorNetAddress* clientAddr)
00219 {
00220     uint8_t data[128];
00221     int len;
00222 
00223     while ( true )
00224     {
00225 
00226         if ( (len = readApiFrame(data)) > 0 )
00227         {
00228 
00229             if ( data[0] == API_RESPONSE )
00230             {
00231                 memcpy(clientAddr->_address64, data + 1, 8);
00232                 memcpy(clientAddr->_address16, data + 9, 2);
00233                 len -= 12;
00234                 memcpy( buf, data + 12, len);
00235                 return len;
00236             }
00237             else if ( data[0] == API_XMITSTATUS )
00238             {
00239                 _respCd = data[5];
00240                 _respId = data[1];
00241                 _sem.post();
00242             }
00243         }
00244         else
00245         {
00246             return 0;
00247         }
00248     }
00249 }
00250 
00251 int XBee::readApiFrame(uint8_t* recvData){
00252     uint8_t buf;
00253     uint8_t pos = 0;
00254     uint8_t checksum = 0;
00255     uint8_t len = 0;
00256 
00257      while ( _serialPort->recv(&buf) )
00258      {
00259          if ( buf == START_BYTE)
00260          {
00261             pos = 1;
00262              D_NWSTACK("\r\n===> Recv:    ");
00263              break;
00264          }
00265      }
00266 
00267     if ( pos == 0 )
00268     {
00269         goto errexit;
00270     }
00271 
00272     if ( recv(&buf) < 0 ) // MSB length
00273     {
00274         goto errexit;
00275     }
00276 
00277     if ( recv(&buf) < 0 ) // LSB length
00278     {
00279         goto errexit;
00280     }
00281     else
00282     {
00283         len = buf;
00284     }
00285 
00286     pos = 0;
00287     while ( len-- )
00288     {
00289         if ( recv(&buf) < 0 )
00290         {
00291             goto errexit;
00292         }
00293         recvData[pos++] = buf;
00294         checksum += buf;
00295     }
00296 
00297     recv(&buf);        // checksum
00298     if ( (0xff - checksum ) == buf ){
00299         D_NWSTACK("    checksum ok\r\n");
00300         return pos;
00301     }
00302     else
00303     {
00304         D_NWSTACK("    checksum error  %02x\r\n", 0xff - checksum);
00305         goto errexit;
00306     }
00307 errexit:
00308     _serialPort->flush();
00309     return -1;
00310 }
00311 
00312 int XBee::send(const uint8_t* payload, uint8_t pLen, SensorNetAddress* addr){
00313     D_NWSTACK("\r\n===> Send:    ");
00314     uint8_t checksum = 0;
00315     _respCd = -1;
00316 
00317     _serialPort->send(START_BYTE);
00318     send(0x00);              // Message Length
00319     send(14 + pLen);         // Message Length
00320 
00321     _serialPort->send(API_XMITREQUEST); // Transmit Request API
00322     checksum += API_XMITREQUEST;
00323 
00324     if (_frameId++ == 0x00 ) // Frame ID
00325     {
00326         _frameId = 1;
00327     }
00328     send(_frameId);
00329     checksum += _frameId;
00330 
00331     for ( int i = 0; i < 8; i++)    // Address64
00332     {
00333         send(addr->_address64[i]);
00334         checksum += addr->_address64[i];
00335     }
00336     for ( int i = 0; i < 2; i++)    // Address16
00337     {
00338         send(addr->_address16[i]);
00339         checksum += addr->_address16[i];
00340     }
00341 
00342     send(0x00);   // Broadcast Radius
00343     checksum += 0x00;
00344 
00345     send(0x00);   // Option: Use the extended transmission timeout 0x40
00346     checksum += 0x00;
00347 
00348     D_NWSTACK("\r\n     Payload: ");
00349 
00350     for ( uint8_t i = 0; i < pLen; i++ ){
00351         send(payload[i]);     // Payload
00352         checksum += payload[i];
00353     }
00354 
00355     checksum = 0xff - checksum;
00356     D_NWSTACK("   checksum  ");
00357     send(checksum);
00358     D_NWSTACK("\r\n");
00359 
00360     /* wait Txim Status 0x8B */
00361     _sem.timedwait(XMIT_STATUS_TIME_OVER);
00362 
00363     if ( _respCd || _frameId != _respId )
00364     {
00365          D_NWSTACK(" frameId = %02x  Not Acknowleged\r\n", _frameId);
00366         return -1;
00367     }
00368     return (int)pLen;
00369 }
00370 
00371 void XBee::send(uint8_t c)
00372 {
00373   if(_apiMode == 2 && (c == START_BYTE || c == ESCAPE || c == XON || c == XOFF)){
00374       _serialPort->send(ESCAPE);
00375       _serialPort->send(c ^ 0x20);
00376   }else{
00377       _serialPort->send(c);
00378   }
00379 }
00380 
00381 int XBee::recv(uint8_t* buf)
00382 {
00383     if (_serialPort->recv(buf) )
00384     {
00385         if ( *buf == ESCAPE && _apiMode == 2 )
00386         {
00387             _serialPort->recv(buf);
00388             *buf = 0x20 ^ *buf;
00389         }
00390         return 0;
00391     }
00392     return -1;
00393 }
00394 
00395 void XBee::setApiMode(uint8_t mode)
00396 {
00397     _apiMode = mode;
00398 }
00399 
00400 /*=========================================
00401  Class SerialPort
00402  =========================================*/
00403 SerialPort::SerialPort()
00404 {
00405     _tio.c_iflag = IGNBRK | IGNPAR;
00406     _tio.c_cflag = CS8 | CLOCAL | CRTSCTS | CREAD;
00407     _tio.c_cc[VINTR] = 0;
00408     _tio.c_cc[VTIME] = 10;   // 1 sec.
00409     _tio.c_cc[VMIN] = 1;
00410     _fd = 0;
00411 }
00412 
00413 SerialPort::~SerialPort()
00414 {
00415     if (_fd)
00416     {
00417         ::close(_fd);
00418     }
00419 }
00420 
00421 int SerialPort::open(char* devName, unsigned int baudrate, bool parity,
00422         unsigned int stopbit, unsigned int flg)
00423 {
00424     _fd = ::open(devName, flg);
00425     if (_fd < 0)
00426     {
00427         return _fd;
00428     }
00429 
00430     if (parity)
00431     {
00432         _tio.c_cflag = _tio.c_cflag | PARENB;
00433     }
00434     if (stopbit == 2)
00435     {
00436         _tio.c_cflag = _tio.c_cflag | CSTOPB;
00437     }
00438 
00439     if (cfsetspeed(&_tio, baudrate) < 0)
00440     {
00441         return errno;
00442     }
00443     return tcsetattr(_fd, TCSANOW, &_tio);
00444 }
00445 
00446 bool SerialPort::send(unsigned char b)
00447 {
00448     if (write(_fd, &b, 1) < 0)
00449     {
00450         return false;
00451     }
00452     else
00453     {
00454         D_NWSTACK( " %02x", b);
00455         return true;
00456     }
00457 }
00458 
00459 bool SerialPort::recv(unsigned char* buf)
00460 {
00461     struct timeval timeout;
00462     fd_set rfds;
00463     FD_ZERO(&rfds);
00464     FD_SET(_fd, &rfds);
00465     timeout.tv_sec = 0;
00466     timeout.tv_usec = 500000;    // 500ms
00467     if ( select(_fd + 1, &rfds, 0, 0, &timeout) > 0 )
00468     {
00469         if (read(_fd, buf, 1) > 0)
00470         {
00471             D_NWSTACK( " %02x",buf[0] );
00472             return true;
00473         }
00474     }
00475     return false;
00476 }
00477 
00478 void SerialPort::flush(void)
00479 {
00480     tcsetattr(_fd, TCSAFLUSH, &_tio);
00481 }
00482