source: flair-src/trunk/lib/FlairCore/src/ui_com.cpp @ 157

Last change on this file since 157 was 157, checked in by Sanahuja Guillaume, 4 years ago

iadded isready to iodevice:
avoid problem of imu not ready in ardrone2

File size: 15.2 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//  created:    2011/05/01
6//  filename:   ui_com.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    classe permettant la lecture et l'ecriture sur socket UDT
14//
15//
16/*********************************************************************/
17
18#include "ui_com.h"
19#include <cstdlib>
20#include <string.h>
21#include <unistd.h>
22#include "Mutex.h"
23#include "SendData.h"
24#include "communication.h"
25#include "FrameworkManager.h"
26#include "zlib.h"
27#include <assert.h>
28#include "config.h"
29
30#ifdef __XENO__
31#include <pthread.h>
32#include <native/task.h>
33#include <native/task.h>
34#include <fcntl.h>
35#endif
36
37using std::string;
38using namespace flair::core;
39using namespace flair::gui;
40
41ui_com::ui_com(const Object *parent, UDTSOCKET sock)
42    : Thread(parent, "send", 2) {
43  // buffer envoi
44  send_buffer = (char *)malloc(3);
45  send_size = 3; // id(1)+period(2)
46
47  // mutex
48  send_mutex = NULL;
49  send_mutex = new Mutex(this, ObjectName());
50
51  socket_fd = sock;
52  connection_lost = false;
53
54#ifdef __XENO__
55  int status;
56  string tmp_name;
57
58  is_running = true;
59
60  // pipe
61  tmp_name = getFrameworkManager()->ObjectName() + "-" + ObjectName() + "-pipe";
62  // xenomai limitation
63  if (tmp_name.size() > 31)
64    Err("rt_pipe_create error (%s is too long)\n", tmp_name.c_str());
65#ifdef RT_PIPE_SIZE
66  status = rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO, RT_PIPE_SIZE);
67#else
68  status = rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO, 0);
69#endif
70
71  if (status != 0) {
72                char errorMsg[256];
73    Err("rt_pipe_create error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
74    // return -1;
75  }
76
77// start user side thread
78#ifdef NRT_STACK_SIZE
79  // Initialize thread creation attributes
80  pthread_attr_t attr;
81  if (pthread_attr_init(&attr) != 0) {
82    Err("pthread_attr_init error\n");
83  }
84
85  if (pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0) {
86    Err("pthread_attr_setstacksize error\n");
87  }
88
89  if (pthread_create(&thread, &attr, user_thread, (void *)this) < 0)
90#else  // NRT_STACK_SIZE
91  if (pthread_create(&thread, NULL, user_thread, (void *)this) < 0)
92#endif // NRT_STACK_SIZE
93  {
94    Err("pthread_create error\n");
95    // return -1;
96  }
97#ifdef NRT_STACK_SIZE
98  if (pthread_attr_destroy(&attr) != 0) {
99    Err("pthread_attr_destroy error\n");
100  }
101#endif
102
103#endif //__XENO__
104  int timeout = 100;
105  if (UDT::setsockopt(socket_fd, 0, UDT_RCVTIMEO, &timeout, sizeof(int)) != 0)
106    Err("UDT::setsockopt error (UDT_RCVTIMEO)\n");
107
108  bool blocking = true;
109  if (UDT::setsockopt(socket_fd, 0, UDT_SNDSYN, &blocking, sizeof(bool)) != 0)
110    Err("UDT::setsockopt error (UDT_SNDSYN)\n");
111
112  if (UDT::setsockopt(socket_fd, 0, UDT_RCVSYN, &blocking, sizeof(bool)) != 0)
113    Err("UDT::setsockopt error (UDT_RCVSYN)\n");
114  //#endif //__XENO__
115
116  Start();
117}
118
119ui_com::~ui_com() {
120// printf("destruction ui_com\n");
121
122#ifdef __XENO__
123  is_running = false;
124
125  pthread_join(thread, NULL);
126
127  int status = rt_pipe_delete(&pipe);
128  if (status != 0) {
129                char errorMsg[256];
130    Err("rt_pipe_delete error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
131        }
132#endif
133
134  SafeStop();
135
136  if (IsSuspended() == true)
137    Resume();
138
139  Join();
140
141  if (send_buffer != NULL)
142    free(send_buffer);
143  send_buffer = NULL;
144
145  // printf("destruction ui_com ok\n");
146}
147
148void ui_com::Send(char *buf, ssize_t size) {
149  if (connection_lost == true)
150    return;
151
152  char *tosend = buf;
153  ssize_t nb_write;
154
155  if (buf[0] == XML_HEADER) {
156    // cut xml header
157    tosend = strstr(buf, "<root");
158    size -= tosend - buf;
159  }
160
161#ifdef __XENO__
162  nb_write = rt_pipe_write(&pipe, tosend, size, P_NORMAL);
163
164  if (nb_write < 0) {
165                char errorMsg[256];
166    Err("rt_pipe_write error (%s)\n", strerror_r(-nb_write, errorMsg, sizeof(errorMsg)));
167  } else if (nb_write != size) {
168    Err("rt_pipe_write error %i/%i\n", nb_write, size);
169  }
170#else //__XENO__
171
172#ifdef COMPRESS_FRAMES
173  bool sendItCompressed = false;
174  if (buf[0] == XML_HEADER) {
175    sendItCompressed = true;
176  }
177
178  if (sendItCompressed) {
179    char *out;
180    ssize_t out_size;
181    if (compressBuffer(tosend, size, &out, &out_size, 9) == Z_OK) {
182      size = out_size;
183      nb_write = UDT::sendmsg(socket_fd, out, size, -1, true);
184      free(out);
185    } else {
186      Warn("Compress error, sending it uncompressed\n");
187      sendItCompressed = false;
188    }
189  }
190
191  if (!sendItCompressed) {
192    nb_write = UDT::sendmsg(socket_fd, tosend, size, -1, true);
193  }
194
195#else // COMPRESS_FRAMES
196  nb_write = UDT::sendmsg(socket_fd, tosend, size, -1, true);
197#endif // COMPRESS_FRAMES
198  // Printf("write %i %i\n",nb_write,size);
199  if (nb_write < 0) {
200    Err("UDT::sendmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
201    if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST ||
202        UDT::getlasterror().getErrorCode() == CUDTException::EINVSOCK) {
203      connection_lost = true;
204    }
205  } else if (nb_write != size) {
206    Err("%s, code %i (%ld/%ld)\n", UDT::getlasterror().getErrorMessage(),
207        UDT::getlasterror().getErrorCode(), nb_write, size);
208  }
209#endif //__XENO__
210}
211
212ssize_t ui_com::Receive(char *buf, ssize_t buf_size) {
213  ssize_t bytesRead = UDT::recvmsg(socket_fd, buf, buf_size);
214
215  if (bytesRead < 0) {
216    if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST) {
217      Err("UDT::recvmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
218      connection_lost = true;
219    }
220  }
221
222  return bytesRead;
223}
224
225void ui_com::Run(void) {
226  // check endianness
227  char header;
228  if (IsBigEndian()) {
229    header = DATAS_BIG_ENDIAN;
230    Printf("System is big endian\n");
231  } else {
232    header = DATAS_LITTLE_ENDIAN;
233    Printf("System is little endian\n");
234  }
235
236#ifdef __XENO__
237  WarnUponSwitches(true);
238  printf("\n"); // a revoir pourquoi??
239// sans ce printf, dans le simu_roll, le suspend ne fonctionne pas...
240#endif
241  // on attend d'avoir des choses à faire
242  Suspend();
243
244  while (!ToBeStopped()) {
245    size_t resume_id;
246    Time min = 0xffffffffffffffffULL;
247
248    // on recpuere l'id de la prochaine execution
249    send_mutex->GetMutex();
250    resume_id = resume_time.size();
251    for (size_t i = 0; i < resume_time.size(); i++) {
252      if (resume_time.at(i) < min && data_to_send.at(i)->IsEnabled() == true) {
253        min = resume_time.at(i);
254        resume_id = i;
255      }
256    }
257
258    // attente
259    if (resume_id < resume_time.size()) {
260      Time time = resume_time.at(resume_id);
261      uint16_t resume_period = data_to_send.at(resume_id)->SendPeriod();
262      send_mutex->ReleaseMutex();
263      // on dort jusqu'a la prochaine execution
264      SleepUntil(time);
265
266      // envoi des donnees
267      int offset = 3;
268      send_buffer[0] = header;
269      send_mutex->GetMutex();
270
271      for (size_t i = 0; i < data_to_send.size(); i++) {
272        if (data_to_send.at(i)->SendPeriod() == resume_period &&
273            data_to_send.at(i)->IsEnabled() == true) {
274          data_to_send.at(i)->CopyDatas(send_buffer + offset);
275          offset += data_to_send.at(i)->SendSize();
276        }
277      }
278      if (offset != 3) {
279        memcpy(&send_buffer[1], &resume_period, sizeof(uint16_t));
280        // printf("send %i %i %i %x %x\n",resume_period,offset,sizeof(uint16_t),send_buffer,&send_buffer[1]);
281        // for(int i=0;i<offset;i++) printf("%x ",send_buffer[i]);
282        // printf("\n");
283        Send(send_buffer, offset);
284      }
285
286      // on planifie la prochaine execution
287      for (size_t i = 0; i < data_to_send.size(); i++) {
288        if (data_to_send.at(i)->SendPeriod() == resume_period) {
289          resume_time.at(i) += data_to_send.at(i)->SendPeriod() * 1000000;
290        }
291      }
292      send_mutex->ReleaseMutex();
293      //Printf("%i %lld\n",resume_period,GetTime()/1000000);
294    } else {
295      send_mutex->ReleaseMutex();
296      // rien a faire, suspend
297      //Printf("rien a faire suspend\n");
298      Suspend();
299      //Printf("wake\n");
300      // on planifie la prochaine execution
301      Time time = GetTime();
302      send_mutex->GetMutex();
303      for (size_t i = 0; i < data_to_send.size(); i++) {
304        resume_time.at(i) =
305            time + (Time)data_to_send.at(i)->SendPeriod() * 1000000;
306      }
307      send_mutex->ReleaseMutex();
308    }
309  }
310}
311#ifdef __XENO__
312void *ui_com::user_thread(void *arg) {
313  int pipe_fd = -1;
314  string devname;
315  char *buf = NULL;
316  ui_com *caller = (ui_com *)arg;
317  int rv;
318  fd_set set;
319  struct timeval timeout;
320  ssize_t nb_read, nb_write;
321
322  buf = (char *)malloc(NRT_PIPE_SIZE);
323  if (buf == NULL) {
324    caller->Err("malloc error\n");
325  }
326
327  devname = NRT_PIPE_PATH + getFrameworkManager()->ObjectName() + "-" +
328            caller->ObjectName() + "-pipe";
329  while (pipe_fd < 0) {
330    pipe_fd = open(devname.c_str(), O_RDWR);
331    if (pipe_fd < 0 && errno != ENOENT) {
332                        char errorMsg[256];
333      caller->Err("open pipe_fd error (%s)\n", strerror_r(errno, errorMsg, sizeof(errorMsg)));
334                }
335    usleep(1000);
336  }
337
338  while (1) {
339    FD_ZERO(&set);         // clear the set
340    FD_SET(pipe_fd, &set); // add our file descriptor to the set
341
342    timeout.tv_sec = 0;
343    timeout.tv_usec = SELECT_TIMEOUT_MS * 1000;
344
345    rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
346
347    if (rv == -1) {
348      if (caller->is_running == false &&
349          UDT::getlasterror().getErrorCode() == CUDTException::ETIMEOUT)
350        break; // timeout
351      if (UDT::getlasterror().getErrorCode() != CUDTException::ETIMEOUT)
352        caller->Err("epoll_wait, %s, code %i\n",
353                    UDT::getlasterror().getErrorMessage(),
354                    UDT::getlasterror().getErrorCode());
355    } else if (rv == 0) {
356      // printf("timeout\n"); // a timeout occured
357      if (caller->is_running == false)
358        break;
359
360    } else {
361      nb_read = read(pipe_fd, buf, NRT_PIPE_SIZE);
362      buf[nb_read] = 0;
363// printf("envoi\n%s\n",buf);
364
365#ifdef COMPRESS_FRAMES
366      char *out;
367      ssize_t out_size;
368
369      if (buf[0] == XML_HEADER) {
370        if (compressBuffer(buf, nb_read, &out, &out_size, 9) == Z_OK) {
371          nb_read = out_size;
372          nb_write = UDT::sendmsg(caller->socket_fd, out, out_size, -1, true);
373          free(out);
374        } else {
375          caller->Warn("Compress error, sending it uncompressed\n");
376          nb_write = UDT::sendmsg(caller->socket_fd, buf, nb_read, -1, true);
377        }
378      } else {
379        nb_write = UDT::sendmsg(caller->socket_fd, buf, nb_read, -1, true);
380      }
381#else
382      nb_write = UDT::sendmsg(caller->socket_fd, buf, nb_read, -1, true);
383#endif
384
385      if (nb_write < 0) {
386        caller->Err("UDT::sendmsg error (%s)\n",
387                    UDT::getlasterror().getErrorMessage());
388        if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST ||
389            UDT::getlasterror().getErrorCode() == CUDTException::EINVSOCK) {
390          caller->connection_lost = true;
391        }
392      } else if (nb_write != nb_read) {
393        caller->Err("UDT::sendmsg error %i/%i\n", nb_write, nb_read);
394      }
395    }
396  }
397
398  close(pipe_fd);
399  if (buf != NULL)
400    free(buf);
401  pthread_exit(0);
402}
403#endif
404
405int ui_com::compressBuffer(char *in, ssize_t in_size, char **out,
406                           ssize_t *out_size, int level) {
407  int ret, flush;
408  unsigned have;
409  z_stream strm;
410
411  /* allocate deflate state */
412  strm.zalloc = Z_NULL;
413  strm.zfree = Z_NULL;
414  strm.opaque = Z_NULL;
415  ret = deflateInit(&strm, level);
416  if (ret != Z_OK)
417    return ret;
418
419  *out = (char *)malloc(COMPRESS_CHUNK);
420  if (!(*out))
421    return Z_BUF_ERROR;
422
423  strm.next_in = (unsigned char *)in;
424  strm.avail_out = COMPRESS_CHUNK;
425  strm.next_out = (unsigned char *)*out;
426  strm.avail_in = in_size;
427  flush = Z_FINISH;
428
429  ret = deflate(&strm, flush); /* no bad return value */
430  if (ret == Z_STREAM_ERROR) {
431    free(*out);
432    return ret;
433  }
434
435  have = COMPRESS_CHUNK - strm.avail_out;
436  *out_size = have;
437  // printf("%i -> %i\n",in_size,have);
438  /* clean up and return */
439  (void)deflateEnd(&strm);
440
441  if (strm.avail_out != 0) {
442    return Z_OK;
443  } else {
444    return Z_STREAM_ERROR;
445  }
446}
447
448int ui_com::uncompressBuffer(unsigned char *in, ssize_t in_size,
449                             unsigned char **out, ssize_t *out_size) {
450  int ret;
451  // unsigned have;
452  z_stream strm;
453
454  /* allocate inflate state */
455  strm.zalloc = Z_NULL;
456  strm.zfree = Z_NULL;
457  strm.opaque = Z_NULL;
458  strm.avail_in = 0;
459  strm.next_in = Z_NULL;
460  ret = inflateInit(&strm);
461  if (ret != Z_OK)
462    return ret;
463
464  *out = (unsigned char *)malloc(COMPRESS_CHUNK);
465  if (!(*out))
466    return Z_BUF_ERROR;
467
468  strm.avail_in = in_size;
469  strm.next_in = in;
470  strm.avail_out = COMPRESS_CHUNK;
471  strm.next_out = *out;
472
473  ret = inflate(&strm, Z_NO_FLUSH);
474  assert(ret != Z_STREAM_ERROR); /* state not clobbered */
475  switch (ret) {
476  case Z_NEED_DICT:
477    ret = Z_DATA_ERROR; /* and fall through */
478  case Z_DATA_ERROR:
479  case Z_MEM_ERROR:
480    (void)inflateEnd(&strm);
481    return ret;
482  }
483  // have = COMPRESS_CHUNK - strm.avail_out;
484
485  /* clean up and return */
486  (void)inflateEnd(&strm);
487  return ret == Z_STREAM_END ? Z_OK : Z_DATA_ERROR;
488}
489
490bool ui_com::ConnectionLost() { return connection_lost; }
491
492void ui_com::AddSendData(const SendData *obj) {
493  send_mutex->GetMutex();
494
495  resume_time.push_back(GetTime() + (Time)obj->SendPeriod() * 1000000);
496
497  // on resume en meme temps tout ceux qui ont la meme periode
498  for (size_t i = 0; i < data_to_send.size(); i++) {
499    if (data_to_send.at(i)->SendPeriod() == obj->SendPeriod()) {
500      resume_time.at(resume_time.size() - 1) = resume_time.at(i);
501      break;
502    }
503  }
504
505  data_to_send.push_back(obj);
506
507  send_mutex->ReleaseMutex();
508}
509
510void ui_com::UpdateSendData(const SendData *obj) {
511  // le mutex est deja pris par l'appellant
512  size_t id, i;
513
514  // on recupere l'id
515  for (i = 0; i < data_to_send.size(); i++) {
516    if (data_to_send.at(i) == obj) {
517      id = i;
518      break;
519    }
520  }
521
522  // on resume en meme temps tout ceux qui ont la meme periode
523  for (i = 0; i < data_to_send.size(); i++) {
524    if (i == id)
525      continue;
526    if (data_to_send.at(i)->IsEnabled() == true &&
527        data_to_send.at(i)->SendPeriod() == obj->SendPeriod()) {
528      resume_time.at(id) = resume_time.at(i);
529      break;
530    }
531  }
532
533  // si aucun match, on planifie l'execution
534  if (i == data_to_send.size())
535    resume_time.at(id) = GetTime() + (Time)obj->SendPeriod() * 1000000;
536
537  if (IsSuspended() == true)
538    Resume();
539
540  return;
541}
542
543void ui_com::UpdateDataToSendSize(void) {
544  send_mutex->GetMutex();
545  send_size = 3; // id(1)+period(2)
546  for (size_t i = 0; i < data_to_send.size(); i++) {
547    if (data_to_send[i] != NULL)
548      send_size += data_to_send[i]->SendSize();
549  }
550
551  // send_buffer=(char*)realloc((void*)send_buffer,send_size*sizeof(char));
552  if (send_buffer != NULL)
553    free(send_buffer);
554  send_buffer = (char *)malloc(send_size * sizeof(char));
555  send_mutex->ReleaseMutex();
556}
557
558void ui_com::RemoveSendData(const SendData *obj) {
559  // printf("remove_data_to_send %i\n",data_to_send.size());
560
561  send_mutex->GetMutex();
562  // on recupere l'id
563  for (size_t i = 0; i < data_to_send.size(); i++) {
564    if (data_to_send.at(i) == obj) {
565      data_to_send.erase(data_to_send.begin() + i);
566      resume_time.erase(resume_time.begin() + i);
567      // printf("remove_data_to_send %i ok\n",data_to_send.size());
568      break;
569    }
570  }
571  send_mutex->ReleaseMutex();
572
573  return;
574}
575
576void ui_com::Block(void) { send_mutex->GetMutex(); }
577
578void ui_com::UnBlock(void) { send_mutex->ReleaseMutex(); }
Note: See TracBrowser for help on using the repository browser.