source: flair-src/trunk/tools/FlairGCS/src/UdtSocket.cpp

Last change on this file was 443, checked in by Sanahuja Guillaume, 5 weeks ago

update buffering (gcs part)

File size: 10.9 KB
RevLine 
[10]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[10]4// %flair:license}
[9]5#include "UdtSocket.h"
[234]6#include "ConnectionLayout.h"
[9]7#include <stdio.h>
8#include <stdlib.h>
9#include <QApplication>
10#include <QtEndian>
11#include <QDir>
12#include <QDate>
13#include "communication.h"
[234]14#include <zlib.h>
15#include <assert.h>
16#include <QThread>
[399]17
[234]18#define COMPRESS_CHUNK 1024*100
[9]19
20#ifndef WIN32
[15]21#include <arpa/inet.h>
[9]22#else
[15]23#include <winsock2.h>
24#include <ws2tcpip.h>
[9]25#endif
26
[234]27#define COMPUTE_UDT_STATS_TIMER 2000
28#define HEARTBEAT_TIMER 200
29
[9]30using namespace std;
31
[234]32UdtSocket::UdtSocket(UDTSOCKET socket,QString name): QObject() {
33  this->socket = socket;
34  this->name=name;
[15]35  stop = false;
[234]36  destroySocket=true;
37  socketType=unknown;
38  total_received=0;
[248]39  pktSndLossTotal=0;
[399]40  lastpktRecvTotal=0;
41 
[234]42  bool blocking = false;
43  if (UDT::setsockopt(socket, 0, UDT_RCVSYN, &blocking, sizeof(bool)) != 0)
[244]44    fprintf(stderr,"UDT::setsockopt error (UDT_RCVSYN)\n");
[234]45   
[15]46  heartbeat_timer = new QTimer(this);
47  connect(heartbeat_timer, SIGNAL(timeout()), this, SLOT(heartbeat()));
[234]48  heartbeat_timer->start(HEARTBEAT_TIMER);
[253]49  //heartbeat();//send directly the first one, but conflicts if this socket is for logs... TODO: delay start of watchdog timer on uav side
[234]50  udtstats_timer = new QTimer(this);
51  connect(udtstats_timer, SIGNAL(timeout()), this, SLOT(getUTDStats()));
52  udtstats_timer->start(COMPUTE_UDT_STATS_TIMER);
[9]53}
54
55UdtSocket::~UdtSocket() {
[15]56  heartbeat_timer->stop();
[234]57  udtstats_timer->stop();
[247]58  if(destroySocket) UDT::close(socket);
[234]59}
[9]60
[234]61void UdtSocket::setName(QString name) {
[244]62  fprintf(stderr,"  %s is %s\n",this->name.toLocal8Bit().constData(),name.toLocal8Bit().constData());
[234]63  this->name=name;
[9]64}
65
[15]66// send signal to uav, to check connectivity through a watchdog
67// this is necessary because we use udt (udp based), and we cannot check
68// disconnection of ground station
[9]69void UdtSocket::heartbeat(void) {
[269]70  //printf("h %s %i.%i\n",name.toLocal8Bit().constData(),QTime::currentTime().second(),QTime::currentTime().msec());
[15]71  char data = WATCHDOG_HEADER;
[254]72  quint64 sent=write(&data, 1,HEARTBEAT_TIMER,true);
[234]73  if (sent != 1 && UDT::getlasterror().getErrorCode() == 2001) {
74    heartbeat_timer->stop();
75  }
76 
[9]77}
78
[234]79void UdtSocket::getUTDStats(void) {
80  float rxRate=((float)total_received/(COMPUTE_UDT_STATS_TIMER/1000))/1000;//in Ko/s
81  total_received=0;
82 
83  UDT::TRACEINFO perf;
84  if (UDT::ERROR == UDT::perfmon(socket, &perf))   {
[244]85    fprintf(stderr,"perfmon: %s\n",UDT::getlasterror().getErrorMessage());
[234]86  }/* else {
[244]87    fprintf(stderr,"%s socket stats:\n",name.toLocal8Bit().constData());
88    fprintf(stderr,"total number of sent packets, including retransmissions: %i\n",perf.pktSentTotal);
89    fprintf(stderr,"total number of received packets: %i\n",perf.pktRecvTotal);
90    fprintf(stderr,"total number of lost packets, measured in the sending side: %i\n",perf.pktSndLossTotal);
91    fprintf(stderr,"total number of lost packets, measured in the receiving side: %i\n",perf.pktRcvLossTotal);
92    fprintf(stderr,"total number of retransmitted packets, measured in the sending side: %i\n",perf.pktRetransTotal);
93    fprintf(stderr,"total number of sent ACK packets: %i\n",perf.pktSentACKTotal);
94    fprintf(stderr,"total number of received ACK packets: %i\n",perf.pktRecvACKTotal);
95    fprintf(stderr,"total number of sent NAK packets: %i\n",perf.pktSentNAKTotal);
96    fprintf(stderr,"total number of received NAK packets: %i\n",perf.pktRecvNAKTotal);
97    fprintf(stderr,"round trip time: %fms\n",perf.msRTT);
[234]98  }*/
[248]99  bool loosingPackets=false;
100  if(perf.pktSndLossTotal>pktSndLossTotal) loosingPackets=true;
101  pktSndLossTotal=perf.pktSndLossTotal;
[399]102  unsigned int pktPerSec=(perf.pktRecvTotal-lastpktRecvTotal)*1000/COMPUTE_UDT_STATS_TIMER;
103  QString stylesheet="color: black";
104  if(pktPerSec>=100) {
105    stylesheet="color: red";
106  }
107  else if(pktPerSec>=50) {
108    stylesheet="color: orange";
109  }
110  stats=QString("rx rate %1kB/s, %2 rx packets/s, round trip %3ms, lost packets %4").arg(rxRate,0,'f',3).arg(pktPerSec).arg(perf.msRTT,0,'f',3).arg(perf.pktSndLossTotal);
111  lastpktRecvTotal=perf.pktRecvTotal;
112  UDTStats(stats,stylesheet,loosingPackets);
[234]113}
114
115QString UdtSocket::getUDTStats() {
116  return stats;
117}
118
[9]119void UdtSocket::kill(void) {
[15]120  stop = true;
121  deleteLater();
[9]122}
123
[234]124void UdtSocket::receiveData(void) {
125  int buf_size;
126  int opt_size;
127  UDT::getsockopt(socket, 0, UDT_RCVBUF, &buf_size, &opt_size);
128  char *buf = (char *)malloc(buf_size);
129  if (!buf) {
[244]130    fprintf(stderr,"error malloc UdtSocket::receiveData buffer\n");
[234]131    return;
132  }
133  char *uncompressbuf=(char *)malloc(COMPRESS_CHUNK);
134  if (!uncompressbuf) {
[244]135    fprintf(stderr,"error malloc UdtSocket::receiveData uncompress buffer\n");
[234]136    free(buf);
137    return;
138  }
[248]139  //fprintf(stderr,stderr,"receiveData %x\n",thread());
[234]140 
[15]141  while (!stop) {
[234]142    QCoreApplication::processEvents();
143   
144    //we need to use epoll?
145    //tests are showing that UDT::recvmsg is waiting for the entire timeout before returning
146    //note that in flair::core the behaviour of UDT::recvmsg timeout is as expected
147    //is it a difference between client and server??
148    //tests also show that we need to recreate the eid every time, otherwise wait returns immediately
[15]149    int eid = UDT::epoll_create();
150    if (eid < 0) {
[244]151      fprintf(stderr,"%s: epoll_create error (%s)\n",name.toLocal8Bit().constData(),UDT::getlasterror().getErrorMessage());
[15]152    }
[9]153
[234]154    if (UDT::epoll_add_usock(eid, socket) < 0) {
[15]155      if (UDT::getlasterror().getErrorCode() == 5004) {
[244]156        fprintf(stderr,"disconnected from %s\n",name.toLocal8Bit().constData());
[234]157        heartbeat_timer->stop();
158        deleteLater();
159        stop=true;;
[15]160      } else {
[244]161        fprintf(stderr,"%s: epoll_add_usock error (%s)\n",name.toLocal8Bit().constData(),UDT::getlasterror().getErrorMessage());
[15]162      }
163    }
[234]164 
165    int num = 1;
166    UDTSOCKET readfds;
[247]167 
[234]168    int rv = UDT::epoll_wait2(eid, &readfds, &num,NULL, NULL,100);
[15]169    if (rv == -1) {
170      if (UDT::getlasterror().getErrorCode() != 6003)
[244]171        fprintf(stderr,"prob %i\n", UDT::getlasterror().getErrorCode());
[234]172    } else if(readfds==socket && num==1 && rv==1) {
[247]173      int size;
[269]174      QTime initTime;
175      initTime.start();
[247]176      do {
[269]177        //we are in "infinite" loop, so let the heartbeat run if needed
178        if(initTime.elapsed()>HEARTBEAT_TIMER) {
179          initTime.start();
180          //do not use error check of heartbeat() method: (otherwise, closes com too early)
181          char data = WATCHDOG_HEADER;
182          UDT::sendmsg(socket, &data, 1, HEARTBEAT_TIMER, true);
183        }
184       
[247]185        size=UDT::recvmsg(socket, buf, buf_size);
186     
[253]187        //fprintf(stderr,"recu %i %x\n",size,buf[0]);
[247]188        if (size > 0) {
189          total_received+=size;
190         
191          switch ((unsigned char)buf[0]) {
192            case ZLIB_HEADER: {
193              ssize_t out_size;
194              uncompressBuffer(buf, size, uncompressbuf, &out_size);
195              if((unsigned char)uncompressbuf[0]==XML_HEADER && socketType==unknown) {
196                socketType=gui;
197                QString remoteName=ConnectionLayout::getDocRootName(uncompressbuf, out_size);
198                setName(remoteName);
199                emit newConnectionLayout(remoteName);//connection is Qt::BlockingQueuedConnection
200              }
201              emit dataReady(uncompressbuf, out_size);//connection is Qt::BlockingQueuedConnection, as we have only one buffer
202              break;
[234]203            }
[247]204            case START_SENDING_FILES: {
[255]205              //printf("log\n");
[247]206              if((unsigned char)uncompressbuf[0]==XML_HEADER && socketType==unknown) {
207                socketType=log;
208              }
209              setName("log files");
210              heartbeat_timer->stop();
211              emit newFileUI(socket);
212              deleteLater();
213              stop=true;
[253]214              size=-1;//exit from do while loop
[247]215              destroySocket=false;
216              break;
[234]217            }
[247]218            case XML_HEADER:
219              if(socketType==unknown) {
220                socketType=gui;
221                QString remoteName=ConnectionLayout::getDocRootName(buf, size );
222                setName(remoteName);
223                emit newConnectionLayout(remoteName);
224              }
[253]225            case DATA_BIG_ENDIAN:
226            case DATA_LITTLE_ENDIAN:
[443]227            case MULTIPLE_DATA_BIG_ENDIAN:
228            case MULTIPLE_DATA_LITTLE_ENDIAN:
229              emit dataReady(buf, size);
[247]230              break;
231            case CLOSING_CONNECTION:
[255]232              fprintf(stderr,"%s, connection closed by remote\n",name.toLocal8Bit().constData());
[247]233              emit dataReady(buf, size );
234              stop = true;
235              heartbeat_timer->stop();
236              deleteLater();
237              break;
238            default:
[253]239              fprintf(stderr,"udt trame non supportée %x\n", buf[0]);
[234]240          }
[247]241        } else {
242          //not necessary to check, watchdog (heartbeat_timer) can do it
243          //if(UDT::getlasterror().getErrorCode()!=6002 && !stop)
244            //fprintf(stderr,"udt socket: %s\n",UDT::getlasterror().getErrorMessage());
245          //UDT::close(socket);//si deconnecté
246          //free(buf);
247          //break;
[234]248        }
[247]249      } while(size>0);
[15]250    } else {
[247]251      //not necessary to check, watchdog (heartbeat_timer) can do it
252      //fprintf(stderr,"udt socket: %s\n",UDT::getlasterror().getErrorMessage());
[15]253    }
[234]254    UDT::epoll_remove_usock(eid, socket);
[15]255    UDT::epoll_release(eid);
256  }
[247]257 
[234]258  free(uncompressbuf);
259  free(buf);
260  thread()->quit();
[9]261}
262
[234]263qint64 UdtSocket::write(const char *buf, qint64 size,int ttl,bool inOrder) {
264  qint64 sent = UDT::sendmsg(socket, buf, size, ttl, inOrder);
265  if (sent != size) {
[258]266    fprintf(stderr,"sent %lld/%lld %s\n",sent ,size,UDT::getlasterror().getErrorMessage());
[244]267    fprintf(stderr,"%s, error writting to udt (%s)\n",name.toLocal8Bit().constData(), UDT::getlasterror().getErrorMessage());
[234]268    if (UDT::getlasterror().getErrorCode() == 2001) {
[244]269      fprintf(stderr,"%s, closing connection\n",name.toLocal8Bit().constData());
[234]270      stop = true;
271      heartbeat_timer->stop();
272      deleteLater();
[15]273    }
274  }
[234]275  return sent;
[9]276}
277
[234]278int UdtSocket::uncompressBuffer(char *in, ssize_t in_size, char *out,
279                                       ssize_t *out_size) {
280  int ret;
281  unsigned have;
282  z_stream strm;
[9]283
[234]284  // allocate inflate state
285  strm.zalloc = Z_NULL;
286  strm.zfree = Z_NULL;
287  strm.opaque = Z_NULL;
288  strm.avail_in = 0;
289  strm.next_in = Z_NULL;
290  ret = inflateInit(&strm);
291  if (ret != Z_OK)
292    return ret;
[9]293
[234]294  strm.avail_in = in_size;
295  strm.next_in = (unsigned char *)in;
296  strm.avail_out = COMPRESS_CHUNK;
297  strm.next_out = (unsigned char *)out;
298
299  ret = inflate(&strm, Z_NO_FLUSH);
300  assert(ret != Z_STREAM_ERROR); // state not clobbered
301  switch (ret) {
302  case Z_NEED_DICT:
303    ret = Z_DATA_ERROR; // and fall through
304  case Z_DATA_ERROR:
305  case Z_MEM_ERROR:
306    (void)inflateEnd(&strm);
307    return ret;
[15]308  }
[234]309  have = COMPRESS_CHUNK - strm.avail_out;
310  *out_size = have;
[9]311
[234]312  // clean up and return
313  (void)inflateEnd(&strm);
314  return ret == Z_STREAM_END ? Z_OK : Z_DATA_ERROR;
[307]315}
Note: See TracBrowser for help on using the repository browser.