source: flair-src/branches/sanscv/tools/FlairGCS/src/UdtSocket.cpp@ 355

Last change on this file since 355 was 324, checked in by Sanahuja Guillaume, 5 years ago

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