source: flair-src/branches/mavlink/tools/FlairGCS/src/UdtSocket.cpp@ 60

Last change on this file since 60 was 30, checked in by Sanahuja Guillaume, 8 years ago

show rx rate in GCS

File size: 6.1 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 <stdio.h>
7#include <stdlib.h>
8#include <QApplication>
9#include <QtEndian>
10#include <QDir>
11#include <QDate>
12#include "file_ui.h"
13#include "communication.h"
14
15#ifndef WIN32
16#include <arpa/inet.h>
17#else
18#include <winsock2.h>
19#include <ws2tcpip.h>
20#endif
21
22using namespace std;
23
24UdtSocket::UdtSocket(UDTSOCKET file_socket, UDTSOCKET com_socket, QString name)
25 : QObject() {
26 this->file_socket = file_socket;
27 this->com_socket = com_socket;
28 this->name = name;
29 stop = false;
30 file_dialog = new file_ui();
31
32 bool blocking = true;
33 UDT::setsockopt(file_socket, 0, UDT_RCVSYN, &blocking, sizeof(bool));
34
35 heartbeat_timer = new QTimer(this);
36 connect(heartbeat_timer, SIGNAL(timeout()), this, SLOT(heartbeat()));
37 heartbeat_timer->start(200);
38}
39
40UdtSocket::~UdtSocket() {
41 heartbeat_timer->stop();
42
43 UDT::close(file_socket);
44 UDT::close(com_socket);
45
46 file_dialog->deleteLater();
47}
48
49// send signal to uav, to check connectivity through a watchdog
50// this is necessary because we use udt (udp based), and we cannot check
51// disconnection of ground station
52void UdtSocket::heartbeat(void) {
53 char data = WATCHDOG_HEADER;
54 write(&data, 1);
55}
56
57void UdtSocket::kill(void) {
58 printf("disconnected\n");
59 stop = true;
60 deleteLater();
61}
62
63void UdtSocket::handleConnections(void) {
64 while (!stop) {
65 int eid = UDT::epoll_create();
66 if (eid < 0) {
67 printf("epoll_create error: %s\n", UDT::getlasterror().getErrorMessage());
68 }
69
70 if (UDT::epoll_add_usock(eid, file_socket) < 0) {
71 if (UDT::getlasterror().getErrorCode() == 5004) {
72 printf("epoll_add_usock error\n");
73 break;
74 } else {
75 printf("epoll_add_usock error: %s\n",
76 UDT::getlasterror().getErrorMessage());
77 }
78 }
79 if (UDT::epoll_add_usock(eid, com_socket) < 0) {
80 if (UDT::getlasterror().getErrorCode() == 5004) {
81 printf("epoll_add_usock error\n");
82 break;
83 } else {
84 printf("epoll_add_usock error: %s\n",
85 UDT::getlasterror().getErrorMessage());
86 }
87 }
88
89 set<UDTSOCKET> readfds;
90
91 int rv = UDT::epoll_wait(eid, &readfds, NULL, 10);
92
93 if (rv == -1) {
94 if (UDT::getlasterror().getErrorCode() != 6003)
95 printf("prob %i\n", UDT::getlasterror().getErrorCode());
96 // printf("wait\n");
97 } else if (rv == 0) {
98 printf("timeout\n"); // a timeout occured
99 } else {
100 /*
101 if(UDT::getlasterror().getErrorCode()==2001) {
102 UDT::epoll_release(eid);
103 }*/
104 for (set<UDTSOCKET>::iterator i = readfds.begin(); i != readfds.end();
105 i++) {
106 // printf("a\n");
107 if (*i == file_socket)
108 receiveFile();
109 if (*i == com_socket)
110 receiveData();
111 }
112 }
113
114 UDT::epoll_remove_usock(eid, file_socket);
115 UDT::epoll_remove_usock(eid, com_socket);
116 UDT::epoll_release(eid);
117
118 QCoreApplication::processEvents();
119 }
120 kill();
121}
122
123void UdtSocket::receiveFile(void) {
124 char *recv_buf;
125 int bytesRead;
126 static bool flag_new_seq = true;
127 static QString folder_name;
128
129 // receive file info
130 recv_buf = (char *)malloc(1024);
131 bytesRead = UDT::recvmsg(file_socket, recv_buf, 1024);
132
133 if (bytesRead <= 0) {
134 free(recv_buf);
135 return;
136 }
137
138 int size;
139 memcpy(&size, &recv_buf[1], sizeof(int));
140 if (recv_buf[0] == FILE_INFO_BIG_ENDIAN)
141 size = qFromBigEndian(size);
142
143 // printf("file_ui recu %i %x\n",bytesRead,recv_buf[0]);
144 if ((recv_buf[0] == FILE_INFO_LITTLE_ENDIAN ||
145 recv_buf[0] == FILE_INFO_BIG_ENDIAN) &&
146 size > 0) {
147 if (flag_new_seq == true) {
148 // create directory for storage
149 QDateTime dateTime = QDateTime::currentDateTime();
150 folder_name = dateTime.toString("yyyyMMdd_hhmm") + "_" + name;
151 if (QDir().exists(folder_name) == true) {
152 folder_name = dateTime.toString("yyyyMMdd_hhmm_ss") + "_" + name;
153 }
154 QDir().mkdir(folder_name);
155
156 flag_new_seq = false;
157 file_dialog->log("Creating directory " + folder_name);
158 }
159
160 QString file_name =
161 QString::fromAscii((const char *)&recv_buf[5], bytesRead - 5);
162 QString file_path = folder_name + "/" + file_name;
163 file_dialog->log(
164 QString("receiving %1 (%2 bytes)").arg(file_name).arg(size));
165 QFile fichier(file_path);
166
167 if (!fichier.open(QIODevice::WriteOnly)) {
168 file_dialog->log(" could not write to file!");
169 } else {
170 // receive file
171 recv_buf = (char *)realloc((void *)recv_buf, size);
172 bytesRead = UDT::recvmsg(file_socket, recv_buf, size);
173 if (bytesRead != size) {
174 file_dialog->log(QString(" error receiving file! (%1/%2)")
175 .arg(bytesRead)
176 .arg(size));
177 free(recv_buf);
178 return;
179 } else {
180 file_dialog->log(" ok");
181 }
182
183 QDataStream stream(&fichier);
184 stream.writeRawData(recv_buf, size);
185 fichier.close();
186
187 file_dialog->addFile(file_path);
188 }
189
190 free(recv_buf);
191 } else if (recv_buf[0] == END) {
192 file_dialog->endOfFiles();
193 flag_new_seq = true;
194 //end ack
195 UDT::sendmsg(file_socket,&recv_buf[0],1);
196 }
197}
198
199void UdtSocket::receiveData(void) {
200 while (1) {
201 int buf_size;
202 int opt_size;
203 UDT::getsockopt(com_socket, 0, UDT_RCVBUF, &buf_size, &opt_size);
204
205 char *buf = (char *)malloc(buf_size);
206 int size;
207 size = UDT::recvmsg(com_socket, buf, buf_size);
208 buf = (char *)realloc(buf, size + 1);
209
210 if (size > 0) {
211 buf[size] = 0;
212 emit dataReady(buf, size + 1);
213 } else {
214 // if(UDT::getlasterror().getErrorCode()!=6002) printf("udt socket:
215 // %s\n",UDT::getlasterror().getErrorMessage());
216 free(buf);
217 break;
218 }
219 }
220}
221
222void UdtSocket::write(const char *buf, qint64 size) {
223 // printf("write\n%s\n",buf);
224 qint64 sent = UDT::sendmsg(com_socket, buf, size, -1, true);
225 if (sent != size) {
226 printf("erreur envoi: %s\n", UDT::getlasterror().getErrorMessage());
227 if (UDT::getlasterror().getErrorCode() == 2001) {
228 stop = true;
229 }
230 }
231}
Note: See TracBrowser for help on using the repository browser.