source: flair-src/trunk/lib/FlairCore/src/FrameworkManager_impl.cpp@ 466

Last change on this file since 466 was 466, checked in by Sanahuja Guillaume, 13 months ago

unlock memory in simulator to avoid mmap problems when running simulator as non root
File size: 28.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// created: 2011/08/31
6// filename: FrameworkManager.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: Classe de base de la librairie
14//
15//
16/*********************************************************************/
17
18#include "FrameworkManager_impl.h"
19#include "FrameworkManager.h"
20#include "Widget_impl.h"
21#include <unistd.h>
22#include "IODevice.h"
23#include "IODevice_impl.h"
24#include "TabWidget.h"
25#include "Layout.h"
26#include "PushButton.h"
27#include "communication.h"
28#include "config.h"
29#include "Watchdog.h"
30#include <errno.h>
31#include <string.h>
32#include <arpa/inet.h>
33#include <sys/mman.h>
34#include <execinfo.h>
35#include <signal.h>
36#include <fcntl.h>
37#include <utmp.h>
38#ifdef __XENO__
39#include <native/task.h>
40#include <rtdk.h>
41#endif
42
43using namespace std;
44using namespace flair::core;
45using namespace flair::gui;
46
47ui_com *FrameworkManager_impl::com = NULL;
48FrameworkManager_impl *FrameworkManager_impl::_this = NULL;
49
50namespace {
51#ifdef __XENO__
52
53#ifdef SIGDEBUG
54static const char *reason_str[] = {
55 "undefined", "received signal", "invoked syscall", "triggered fault",
56 "affected by priority inversion", "missing mlockall", "runaway thread",
57};
58
59void warn_upon_switch(int sig, siginfo_t *si, void *context) {
60 unsigned int reason = si->si_value.sival_int;
61 void *bt[32];
62 int nentries;
63#ifdef SIGDEBUG_WATCHDOG
64 printf("\nSIGDEBUG received, reason %d: %s\n", reason,
65 reason <= SIGDEBUG_WATCHDOG ? reason_str[reason] : "<unknown>");
66 printf("entering secondary mode\n");
67#endif
68 // Dump a backtrace of the frame which caused the switch to secondary mode:
69 nentries = backtrace(bt, sizeof(bt) / sizeof(bt[0]));
70 backtrace_symbols_fd(bt, nentries, fileno(stdout));
71}
72#else // SIGDEBUG
73void warn_upon_switch(int sig __attribute__((unused))) {
74 void *bt[32];
75 int nentries;
76
77 /* Dump a backtrace of the frame which caused the switch to
78 secondary mode: */
79 nentries = backtrace(bt, sizeof(bt) / sizeof(bt[0]));
80 backtrace_symbols_fd(bt, nentries, fileno(stdout));
81}
82#endif // SIGDEBUG
83#endif //__XENO__
84void seg_fault(int sig __attribute__((unused))) {
85 void *bt[32];
86 int nentries;
87
88 printf("Segmentation fault:\n");
89 /* Dump a backtrace of the frame which caused the segfault: */
90 nentries = backtrace(bt, sizeof(bt) / sizeof(bt[0]));
91 backtrace_symbols_fd(bt, nentries, fileno(stdout));
92
93 exit(1);
94}
95}
96
97FrameworkManager_impl::FrameworkManager_impl(FrameworkManager *self,
98 string name)
99 : Thread(self, "FrameworkManager", FRAMEWORK_TASK_PRIORITY) {
100 this->self = self;
101 is_logging = false;
102 logger_defined = false;
103 disable_errors = false;
104 ui_defined = false;
105 rcv_buf = NULL;
106 gcs_watchdog = NULL;
107 _this = this;
108 tabwidget=NULL;
109
110 // Avoids memory swapping for this program
111 // GL simulator will unlock memory, otherwise it can cause problems when running simlator as non root
112 mlockall(MCL_CURRENT | MCL_FUTURE);
113
114 // catch segfault
115 signal(SIGSEGV, seg_fault);
116
117// catch primary->secondary switch
118#ifdef __XENO__
119#ifdef SIGDEBUG
120 struct sigaction sa;
121 sigemptyset(&sa.sa_mask);
122 sa.sa_sigaction = warn_upon_switch;
123 sa.sa_flags = SA_SIGINFO;
124 sigaction(SIGDEBUG, &sa, NULL);
125#else //SIGDEBUG
126 signal(SIGXCPU, warn_upon_switch);
127#endif //SIGDEBUG
128 string task_name = "Framework_" + name;
129
130 // Perform auto-init of rt_print buffers if the task doesn't do so
131 rt_print_auto_init(1);
132 // Initialise the rt_print buffer for this task explicitly
133 rt_print_init(512, task_name.c_str());
134
135 int status = rt_task_shadow(NULL, task_name.c_str(), 10, 0);
136 if (status != 0) {
137 char errorMsg[256];
138 self->Err("rt_task_shadow error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
139 }
140
141#endif //__XENO__
142}
143
144void FrameworkManager_impl::SetConnectionLost(void) {
145 Err("connection lost\n");
146 if(gcs_watchdog!=NULL) gcs_watchdog->SafeStop();
147 connection_lost = true;
148}
149
150FrameworkManager_impl::~FrameworkManager_impl() {
151 // Printf("destruction FrameworkManager_impl\n");
152 int status;
153
154 SafeStop();
155 Join();
156
157 if(gcs_watchdog!=NULL) gcs_watchdog->SafeStop();
158
159 if (rcv_buf != NULL)
160 free(rcv_buf);
161
162 if (logger_defined == true) {
163 continuer = false;
164 (void)pthread_join(log_th, NULL);
165
166 status = DeletePipe(&cmd_pipe);
167 if (status != 0) {
168 char errorMsg[256];
169 Err("Error deleting pipe (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
170 }
171 status = DeletePipe(&data_pipe);
172 if (status != 0) {
173 char errorMsg[256];
174 Err("Error deleting pipe (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
175 }
176
177#ifdef __XENO__
178 status = rt_heap_delete(&log_heap);
179 if (status != 0) {
180 char errorMsg[256];
181 Err("rt_heap_delete error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
182 }
183#endif
184
185 logs.clear();
186 }
187
188 if (file_doc != NULL)
189 xmlFreeDoc(file_doc);
190
191 if (ui_defined)
192 delete top_layout;
193
194 if (com != NULL) {
195 delete com;
196 //avoid waiting on closing if connection is lost
197 if(connection_lost) {
198 bool blocking = false;
199 if (UDT::setsockopt(com_sock, 0, UDT_SNDSYN, &blocking, sizeof(bool)) != 0)
200 Err("UDT::setsockopt error (UDT_SNDSYN)\n");
201 }
202 status = UDT::close(com_sock);
203 if (status != 0)
204 Printf("Error udt::close %s", UDT::getlasterror().getErrorMessage());
205
206 SleepMS(200); // a revoir, sinon UDT::cleanup bloque en RT
207
208 if(connection_lost) {
209 //don't know why?
210 Warn("Cleaning up UDT socket, this can take some time\n");
211 }
212 if (UDT::cleanup() != 0)
213 Err("UDT::cleanup error\n");
214 }
215
216 // Printf("destruction FrameworkManager_impl ok\n");
217}
218
219void FrameworkManager_impl::SetupConnection(string address, uint16_t port,Time watchdogTimeout,
220 size_t rcv_buf_size) {
221 if (com != NULL) {
222 Err("SetupConnection should be called only one time\n");
223 return;
224 }
225 if(address=="autodetect") {
226 Printf("Autodetecting gcs ip address based on ssh connection\n");
227 address=AutoDetectGCS();
228 }
229 this->address=address;
230 this->port=port;
231
232 UDT::startup2(1024*256,1024*256,1024*256);
233 this->rcv_buf_size = rcv_buf_size;
234
235 // socket file_socket, doit être créé en premier, cf station sol
236 Printf("Connecting to FlairGCS on %s:%i\n", address.c_str(), port);
237 //file_sock = GetSocket(address, port);
238 com_sock = GetSocket();
239
240 // receive buffer allocation
241 rcv_buf = (char *)malloc(rcv_buf_size);
242 if (rcv_buf == NULL) {
243 Err("receive buffer malloc error\n");
244 }
245
246 com = new ui_com(this, com_sock);
247
248 Start();
249
250 // watchdog for connection with ground station
251 connection_lost = false;
252 gcs_watchdog = new Watchdog(this, std::bind(&FrameworkManager_impl::SetConnectionLost, this),watchdogTimeout);
253 gcs_watchdog->Start();
254}
255
256string FrameworkManager_impl::AutoDetectGCS(void) {
257 int logsize = 10;//should be sufficient
258 int file;
259 struct utmp log[logsize];
260
261 file = open("/var/run/utmp", O_RDONLY);
262
263//return only first matching occurence
264//TODO: check also log->ut_addr_v6[0]
265 if (file) {
266 int read_size=read(file, &log, sizeof(log));
267 int nb=read_size/sizeof(struct utmp);
268
269 for(int i = 0; i < nb; i++) {
270 //printf("\n ut_user: %s host: %s %x\n ut_type: %ld line %s\n", log[i].ut_user,log[i].ut_host,log[i].ut_addr_v6[0], log[i].ut_type,log[i].ut_line);
271 if(log[i].ut_type==USER_PROCESS) {
272 close(file);
273 return log[i].ut_host;
274 }
275
276 }
277
278 close(file);
279 }
280
281 Err("Failed to run autodetect command\n" );
282 return "";
283}
284
285void FrameworkManager_impl::SetupUserInterface(string xml_file) {
286 ui_defined = true;
287 this->xml_file = xml_file;
288
289 // top_layout=new Layout(NULL,XML_ROOT_ELEMENT,XML_ROOT_TYPE);
290 top_layout = new Layout(NULL, self->ObjectName(), XML_ROOT_TYPE);
291
292 // xml setup of the main widget
293 if (xml_file != "") {
294 xmlNodePtr *file_node = &(((Widget *)(top_layout))->pimpl_->file_node);
295 file_doc = xmlParseFile(xml_file.c_str());
296 if (file_doc == NULL) {
297 self->Warn("XML document not parsed successfully. Creating a new one.\n");
298 file_doc = xmlNewDoc((xmlChar *)"1.0");
299 *file_node = xmlNewNode(NULL, (xmlChar *)XML_ROOT_TYPE);
300 xmlSetProp(*file_node, (xmlChar *)"name",
301 (xmlChar *)ObjectName().c_str());
302 xmlDocSetRootElement(file_doc, *file_node);
303 // PrintXml();
304 } else {
305 *file_node = xmlDocGetRootElement(file_doc);
306 if (xmlStrcmp((*file_node)->name, (xmlChar *)XML_ROOT_TYPE)) {
307 self->Warn("%s, no match found in xml file\n", XML_ROOT_TYPE);
308 *file_node = xmlNewNode(NULL, (xmlChar *)XML_ROOT_TYPE);
309 xmlSetProp(*file_node, (xmlChar *)"name",
310 (xmlChar *)ObjectName().c_str());
311 xmlDocSetRootElement(file_doc, *file_node);
312 }
313 }
314 } else {
315 self->Err("xml file not defined\n");
316 }
317
318 // gui
319 tabwidget =
320 new TabWidget(top_layout->At(0, 0), XML_MAIN_TABWIDGET, TabWidget::North);
321 save_button =
322 new PushButton(top_layout->At(1, 0),
323 "save config on target (" + self->ObjectName() + ")");
324 // load_button=new PushButton(top_layout->At(1,1),"load config on target (" +
325 // self->ObjectName() + ")");
326}
327
328// in case of RT, this thread switches to secondary mode when calling
329// com->Receive
330// it switches back to RT in ProcessXML when mutex are locked
331void FrameworkManager_impl::Run(void) {
332
333 if (self->ErrorOccured() == true) {
334 return ;
335 }
336
337
338 while (!ToBeStopped()) {
339 ssize_t bytesRead;
340 bytesRead = com->Receive(rcv_buf, rcv_buf_size);
341 com->CheckConnection();
342//printf("%i\n",bytesRead);
343 if (bytesRead == (ssize_t)rcv_buf_size)
344 Err("FrameworkManager max receive size, augmenter le buffer size!\n");
345
346 if (bytesRead > 0) {
347 //printf("recu %ld, trame %x %lld\n",bytesRead,(uint8_t)rcv_buf[0],GetTime()/(1000000));
348 //rcv_buf[bytesRead-1]=0;//pour affichage
349 //printf("%s\n",rcv_buf);
350
351 switch ((uint8_t)rcv_buf[0]) {
352 case XML_HEADER: {
353 xmlDoc *doc;
354 rcv_buf[bytesRead] = 0;
355 //Printf("%s\n",rcv_buf);
356 doc = xmlReadMemory(rcv_buf, (int)bytesRead, "include.xml",
357 "ISO-8859-1", 0);
358 xmlNode *cur_node = NULL;
359
360 for (cur_node = xmlDocGetRootElement(doc); cur_node;
361 cur_node = cur_node->next) {
362 if (cur_node->type == XML_ELEMENT_NODE) {
363 if (!xmlStrcmp(cur_node->name, (xmlChar *)XML_ROOT_TYPE)) {
364#ifdef __XENO__
365 WarnUponSwitches(
366 true); // ProcessXML should not switch to secondary mode
367#endif
368 top_layout->Widget::pimpl_->ProcessXML(cur_node->children);
369#ifdef __XENO__
370 WarnUponSwitches(false); // other parts of this thread can switch
371 // to secondary mode
372#endif
373 break;
374 }
375 }
376 }
377
378 xmlFreeDoc(doc);
379
380 if (save_button->Clicked())
381 SaveXml();
382
383 SaveXmlChange(rcv_buf);
384
385 break;
386 }
387 case WATCHDOG_HEADER: {
388 if(gcs_watchdog!=NULL) gcs_watchdog->Touch();
389 break;
390 }
391 default:
392 Err("unknown id: %x\n", (uint8_t)rcv_buf[0]);
393 break;
394 }
395 } else {
396 if (com->ConnectionLost())
397 SleepMS(10); // avoid infinite loop in this case
398 }
399 }
400}
401
402void FrameworkManager_impl::SaveXml(void) {
403 if (ui_defined)
404 xmlSaveFormatFile(xml_file.c_str(), file_doc, 1);
405}
406
407void FrameworkManager_impl::SaveXmlChange(char *buf) {
408 if (is_logging == true) {
409 FILE *xml_change;
410 char filename[256];
411 Time time = GetTime();
412
413 sprintf(filename, "%s/changes_at_%lld.xml", log_path.c_str(), time);
414
415 xml_change = fopen(filename, "a");
416 fprintf(xml_change, "%s", buf);
417 fclose(xml_change);
418
419 sprintf(filename, "changes_at_%lld.xml", time);
420 xml_changes.push_back(filename);
421 }
422}
423
424void FrameworkManager_impl::SendFile(UDTSOCKET socket,string path, string name) {
425 char *buf, *more_buf;
426 int size;
427 filebuf *pbuf;
428 ssize_t nb_write;
429 string filename = path + "/" + name;
430
431 // open the file
432 fstream ifs(filename.c_str(), ios::in | ios::binary);
433 ifs.seekg(0, ios::end);
434 size = ifs.tellg();
435 ifs.seekg(0, ios::beg);
436 pbuf = ifs.rdbuf();
437
438 if (size <= 0) {
439 Err("error opening file %s\n", filename.c_str());
440 return;
441 }
442
443 buf = (char *)malloc(sizeof(uint8_t) + sizeof(int) + name.size());
444 if (buf == NULL) {
445 Err("malloc error, not sending file\n");
446 return;
447 }
448
449 if (IsBigEndian()) {
450 buf[0] = FILE_INFO_BIG_ENDIAN;
451 } else {
452 buf[0] = FILE_INFO_LITTLE_ENDIAN;
453 }
454
455 memcpy(buf + 1, &size, sizeof(int));
456 memcpy(buf + 1 + sizeof(int), name.c_str(), name.size());
457 Printf("sending %s, size: %i\n", filename.c_str(), size);
458 // send file information
459 UDT::sendmsg(socket, buf, sizeof(uint8_t) + sizeof(int) + name.size(), -1,true);
460
461 more_buf = (char *)realloc((void *)buf, size);
462 if (more_buf == NULL) {
463 Err("realloc error, not sending file\n");
464 free(buf);
465 return;
466 } else {
467 buf = more_buf;
468 }
469
470 pbuf->sgetn(buf, size);
471 // send the file
472 nb_write = UDT::sendmsg(socket, buf, size, -1, true);
473
474 if (nb_write < 0) {
475 Err("UDT::sendmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
476 } else if (nb_write != size) {
477 Err("UDT::sendmsg error, sent %ld/%i\n", nb_write, size);
478 }
479
480 ifs.close();
481 free(buf);
482}
483
484void FrameworkManager_impl::FinishSending(UDTSOCKET socket) {
485 char rm_cmd[256];
486
487 // send orignal xml
488 SendFile(socket,log_path, "setup.xml");
489 sprintf(rm_cmd, "rm %s/setup.xml", log_path.c_str());
490 system(rm_cmd);
491
492 // send xml changes
493 for (size_t i = 0; i < xml_changes.size(); i++) {
494 // Printf("%s\n",xml_changes.at(i).c_str());
495 SendFile(socket,log_path, xml_changes.at(i).c_str());
496 sprintf(rm_cmd, "rm %s/%s", log_path.c_str(), xml_changes.at(i).c_str());
497 system(rm_cmd);
498 }
499 xml_changes.clear();
500
501 // end notify
502 char buf = END_SENDING_FILES;
503 int nb_write = UDT::sendmsg(socket, &buf, 1, -1, true);
504
505 if (nb_write < 0) {
506 Err("UDT::sendmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
507 } else if (nb_write != 1) {
508 Err("UDT::sendmsg error, sent %i/%i\n", nb_write, 1);
509 }
510
511 //wait end ACK
512 int nb_read = UDT::recvmsg(socket,&buf,1);
513 if(nb_read<0) {
514 Err("UDT::recvmsg error (%s)\n",UDT::getlasterror().getErrorMessage());
515 } else if (nb_read != 1) {
516 Err("UDT::recvmsg error, sent %i/%i\n",nb_read,1);
517 }
518}
519
520UDTSOCKET FrameworkManager_impl::GetSocket(void) {
521 while (1) {
522 UDTSOCKET new_fd;
523 new_fd = UDT::socket(AF_INET, SOCK_DGRAM, 0);
524 if (new_fd == UDT::INVALID_SOCK) {
525 Err("socket error: %s\n", UDT::getlasterror().getErrorMessage());
526 return 0;
527 }
528
529 sockaddr_in serv_addr;
530 serv_addr.sin_family = AF_INET;
531 serv_addr.sin_port = htons(short(port));
532
533 if (inet_pton(AF_INET, address.c_str(), &serv_addr.sin_addr) <= 0) {
534 Err("incorrect network address\n");
535 return 0;
536 }
537
538 memset(&(serv_addr.sin_zero), '\0', 8);
539
540 if (UDT::ERROR ==
541 UDT::connect(new_fd, (sockaddr *)&serv_addr, sizeof(serv_addr))) {
542 // Printf("connect error: %s
543 // %i\n",UDT::getlasterror().getErrorMessage(),UDT::getlasterror().getErrorCode());
544 UDT::close(new_fd);
545 if (UDT::getlasterror().getErrorCode() != 1001 &&
546 UDT::getlasterror().getErrorCode() != 1002) {
547 Err("connect error: %s\n", UDT::getlasterror().getErrorMessage());
548 return 0;
549 }
550 } else {
551 // printf("connected to
552 // %s:%i\n",inet_ntoa(serv_addr.sin_addr),serv_addr.sin_port);
553 return new_fd;
554 }
555 }
556}
557
558#ifdef __XENO__
559int FrameworkManager_impl::CreatePipe(RT_PIPE *fd, string name) {
560 name = self->ObjectName() + "-" + name;
561 // xenomai limitation
562 if (name.size() > 31)
563 self->Err("rt_pipe_create error (%s is too long)\n", name.c_str());
564// start log writter
565#ifdef RT_PIPE_SIZE
566 return rt_pipe_create(fd, name.c_str(), P_MINOR_AUTO, RT_PIPE_SIZE);
567#else
568 return rt_pipe_create(fd, name.c_str(), P_MINOR_AUTO, 0);
569#endif
570}
571#else
572int FrameworkManager_impl::CreatePipe(int (*fd)[2], string name) {
573 // if(pipe2(fd[0],O_NONBLOCK) == -1)
574 if (pipe(fd[0]) == -1) {
575 return errno;
576 } else {
577 int attr = fcntl((*fd)[0], F_GETFL, 0);
578 if (attr == -1) {
579 return errno;
580 }
581 if (fcntl((*fd)[0], F_SETFL, attr | O_NONBLOCK) == -1) {
582 return errno;
583 }
584 attr = fcntl((*fd)[1], F_GETFL, 0);
585 if (attr == -1) {
586 return errno;
587 }
588 if (fcntl((*fd)[1], F_SETFL, attr | O_NONBLOCK) == -1) {
589 return errno;
590 }
591
592 return 0;
593 }
594}
595#endif
596
597#ifdef __XENO__
598int FrameworkManager_impl::DeletePipe(RT_PIPE *fd) {
599 return rt_pipe_delete(fd);
600}
601#else
602int FrameworkManager_impl::DeletePipe(int (*fd)[2]) {
603 int status1 = close((*fd)[0]);
604 int status2 = close((*fd)[1]);
605 if (status1 == 0 && status2 == 0)
606 return 0;
607 if (status1 != 0)
608 return status1;
609 if (status2 != 0)
610 return status2;
611}
612#endif
613
614void FrameworkManager_impl::SetupLogger(string log_path,uint32_t stackSize) {
615
616 if (logger_defined == true) {
617 Warn("SetupLogger() was already called.\n");
618 return;
619 }
620
621 this->log_path = log_path;
622
623 int status = CreatePipe(&cmd_pipe, "log_cmd");
624 if (status != 0) {
625 char errorMsg[256];
626 Err("Error creating pipe (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
627 return;
628 }
629
630 status = CreatePipe(&data_pipe, "log_data");
631 if (status != 0) {
632 char errorMsg[256];
633 Err("Error creating pipe (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
634 return;
635 }
636
637#ifdef __XENO__
638 string tmp_name;
639 tmp_name = self->ObjectName() + "-log_heap";
640 status = rt_heap_create(&log_heap, tmp_name.c_str(), RT_LOG_HEAP, H_FIFO);
641 if (status != 0) {
642 char errorMsg[256];
643 Err("rt_heap_create error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
644 return;
645 }
646#endif //__XENO__
647
648 continuer = true;
649
650 // Initialize thread creation attributes
651 pthread_attr_t attr;
652 status=pthread_attr_init(&attr);
653 if (status != 0) {
654 char errorMsg[256];
655 Err("pthread_attr_init error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
656 return;
657 }
658
659 status=pthread_attr_setstacksize(&attr, stackSize);
660 if (status != 0) {
661 char errorMsg[256];
662 Err("pthread_attr_setstacksize error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
663 return;
664 }
665
666 status=pthread_create(&log_th, &attr, write_log_user, (void *)this);
667 if (status < 0) {
668 char errorMsg[256];
669 Err("pthread_create error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
670 return;
671 }
672
673 status=pthread_attr_destroy(&attr);
674 if (status != 0) {
675 char errorMsg[256];
676 Err("pthread_attr_destroy error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
677 return;
678 }
679
680 logger_defined = true;
681}
682
683void FrameworkManager_impl::AddDeviceToLog(IODevice *device) {
684 if (logger_defined == false) {
685 Warn("SetupLogger() was not called, not adding to log\n");
686 return;
687 }
688
689 if (is_logging == false) {
690 if (!device->pimpl_->IsSetToBeLogged()) {
691 device->pimpl_->SetToBeLogged();
692 log_desc_t tmp;
693 tmp.device = device;
694 logs.push_back(tmp);
695 } else {
696 Warn("not adding it twice\n");
697 }
698 } else {
699 Err("impossible while logging\n");
700 }
701}
702
703bool FrameworkManager_impl::IsDeviceLogged(const IODevice *device) const {
704 return device->pimpl_->IsSetToBeLogged();
705}
706
707void FrameworkManager_impl::StartLog(void) {
708 if (logger_defined == false) {
709 Err("SetupLogger() was not called, not starting log\n");
710 return;
711 }
712
713 ssize_t written;
714 size_t nb_err = 0;
715
716 if (logs.size() == 0) {
717 Warn("Not starting log: nothing to log!\n");
718 return;
719 }
720
721 if (is_logging == false) {
722 for (size_t i = 0; i < logs.size(); i++) {
723
724 logs.at(i).running = true;
725 logs.at(i).dbtFile = NULL;
726 logs.at(i).size = logs.at(i).device->pimpl_->LogSize();
727#ifdef __XENO__
728 written =
729 rt_pipe_write(&cmd_pipe, &logs.at(i), sizeof(log_desc_t), P_NORMAL);
730#else
731 written = write(cmd_pipe[1], &logs.at(i), sizeof(log_desc_t));
732#endif
733
734 if (written < 0) {
735 char errorMsg[256];
736 Err("write pipe error (%s)\n", strerror_r(-written, errorMsg, sizeof(errorMsg)));
737 nb_err++;
738 logs.at(i).running = false;
739 } else if (written != sizeof(log_desc_t)) {
740 Err("write pipe error %ld/%ld\n", written, sizeof(log_desc_t));
741 nb_err++;
742 logs.at(i).running = false;
743 }
744 }
745
746 if (nb_err != logs.size())
747 is_logging = true;
748 } else {
749 Warn("Already logging\n");
750 }
751}
752
753void FrameworkManager_impl::StopLog(void) {
754 ssize_t written;
755
756 if (is_logging == true) {
757 for (size_t i = 0; i < logs.size(); i++) {
758 logs.at(i).running = false;
759 }
760// send only one running false condition, user thread will stop and send all
761#ifdef __XENO__
762 written =
763 rt_pipe_write(&cmd_pipe, &logs.at(0), sizeof(log_desc_t), P_NORMAL);
764#else
765 written = write(cmd_pipe[1], &logs.at(0), sizeof(log_desc_t));
766#endif
767
768 if (written < 0) {
769 char errorMsg[256];
770 Err("write pipe error (%s)\n", strerror_r(-written, errorMsg, sizeof(errorMsg)));
771 return;
772 } else if (written != sizeof(log_desc_t)) {
773 Err("write pipe error %ld/%ld\n", written, sizeof(log_desc_t));
774 return;
775 }
776
777 is_logging = false;
778 } else {
779 Warn("Not logging\n");
780 }
781}
782
783char *FrameworkManager_impl::GetBuffer(size_t sz) {
784// Printf("alloc %i\n",sz);
785#ifdef __XENO__
786 void *ptr;
787 int status = rt_heap_alloc(&log_heap, sz, TM_NONBLOCK, &ptr);
788 if (status != 0) {
789 char errorMsg[256];
790 Err("rt_heap_alloc error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
791 ptr = NULL;
792 }
793 return (char *)ptr;
794#else
795 return (char *)malloc(sz);
796#endif
797}
798
799void FrameworkManager_impl::ReleaseBuffer(char *buf) {
800#ifdef __XENO__
801 int status = rt_heap_free(&log_heap, buf);
802
803 if (status != 0) {
804 char errorMsg[256];
805 Err("rt_heap_free error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
806 }
807#else
808 free(buf);
809#endif
810}
811
812void FrameworkManager_impl::WriteLog(const char *buf, size_t size) {
813 ssize_t written;
814
815#ifdef __XENO__
816 written = rt_pipe_write(&data_pipe, buf, size, P_NORMAL);
817#else
818 written = write(data_pipe[1], buf, size);
819#endif
820
821 if (written < 0) {
822 char errorMsg[256];
823 Err("error write pipe (%s)\n", strerror_r(-written, errorMsg, sizeof(errorMsg)));
824 } else if (written != (ssize_t)size) {
825 Err("error write pipe %ld/%ld\n", written, size);
826 }
827}
828
829void *FrameworkManager_impl::write_log_user(void *arg) {
830 int cmd_pipe = -1;
831 int data_pipe = -1;
832 fd_set set;
833 struct timeval timeout;
834 FrameworkManager_impl *caller = (FrameworkManager_impl *)arg;
835 int rv;
836
837 vector<log_desc_t> logs;
838
839#ifdef __XENO__
840 while (cmd_pipe < 0) {
841 string filename = NRT_PIPE_PATH + caller->self->ObjectName() + "-log_cmd";
842 cmd_pipe = open(filename.c_str(), O_RDWR);
843 if (cmd_pipe < 0 && errno != ENOENT) {
844 char errorMsg[256];
845 caller->self->Err("open rt_pipe error: %s %s\n", filename.c_str(), strerror_r(errno, errorMsg, sizeof(errorMsg)));
846 }
847 usleep(1000);
848 }
849 while (data_pipe < 0) {
850 string filename = NRT_PIPE_PATH + caller->self->ObjectName() + "-log_data";
851 data_pipe = open(filename.c_str(), O_RDWR);
852 if (data_pipe < 0 && errno != ENOENT) {
853 char errorMsg[256];
854 caller->self->Err("open rt_pipe error: %s %s\n", filename.c_str(), strerror_r(errno, errorMsg, sizeof(errorMsg)));
855 }
856 usleep(1000);
857 }
858#else
859 cmd_pipe = caller->cmd_pipe[0];
860 data_pipe = caller->data_pipe[0];
861#endif
862
863 while (caller->continuer == true) {
864 FD_ZERO(&set);
865 FD_SET(cmd_pipe, &set);
866 FD_SET(data_pipe, &set);
867
868 timeout.tv_sec = 0;
869 timeout.tv_usec = SELECT_TIMEOUT_MS * 1000;
870 rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
871
872 if (rv == -1) {
873 caller->Err("select error\n"); // an error accured
874 } else if (rv == 0) {
875 // printf("timeout write_log_user %s\n",caller->ObjectName().c_str()); //
876 // a timeout occured
877 } else {
878 if (FD_ISSET(cmd_pipe, &set)) {
879 log_desc_t tmp;
880 read(cmd_pipe, &tmp, sizeof(log_desc_t));
881
882 if (tmp.running == true) {// start logging
883 string filename = caller->log_path + "/" + caller->FileName(tmp.device) + ".dbt";
884 printf("Creating log file %s (log size %i)\n", filename.c_str(), (int)tmp.size);
885 tmp.dbtFile = inithdFile((char *)filename.c_str(), UAV, tmp.size);
886 logs.push_back(tmp);
887
888 if (logs.size() == 1) {
889 filename = caller->log_path + "/setup.xml";
890 xmlSaveFile(filename.c_str(), caller->file_doc);
891 }
892 } else {// stop logging
893 //disable watchdog temporarly
894 //this is necessary because GCS is no longer sending the heartbeat when receiving files...
895 //TODO: add a thread in GCS for receiving file
896 //but be careful that with a xbee modem for exemple, sending files can saturate communication and
897 //avoid the heartbeat to be received... so disabling watchdog is not a so bad option...
898 if(caller->gcs_watchdog!=NULL) {
899 caller->gcs_watchdog->SafeStop();
900 caller->gcs_watchdog->Join();
901 }
902 //create a socket for files
903 UDTSOCKET file_sock = caller->GetSocket();
904 char data=START_SENDING_FILES;
905 ssize_t nb_write = UDT::sendmsg(file_sock, &data, 1, -1, true);
906 if (nb_write < 0) {
907 caller->Err("UDT::sendmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
908 if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST ||
909 UDT::getlasterror().getErrorCode() == CUDTException::EINVSOCK) {
910 }
911 } else if (nb_write != 1) {
912 caller->Err("%s, code %i (%ld/%ld)\n", UDT::getlasterror().getErrorMessage(),
913 UDT::getlasterror().getErrorCode(), nb_write, 1);
914 }
915 for (size_t i = 0; i < logs.size(); i++) {
916 if (logs.at(i).dbtFile != NULL) {
917 close_hdfile(logs.at(i).dbtFile);
918
919 string filename = caller->FileName(logs.at(i).device) + ".dbt";
920 caller->SendFile(file_sock,caller->log_path, filename);
921
922 fstream txt_file;
923 filename = caller->FileName(logs.at(i).device) + ".txt";
924 txt_file.open((caller->log_path + "/" + filename).c_str(),
925 fstream::out);
926 txt_file << "1: time (us)\n2: time (ns)\n";
927 int index = 3;
928 logs.at(i).device->pimpl_->WriteLogsDescriptors(txt_file, &index);
929 txt_file.close();
930
931 caller->SendFile(file_sock,caller->log_path, filename);
932 }
933 }
934 // a revoir celui ci est le xml enregistré et pas forcement l'actuel
935 // if(caller->xml_file!="") caller->SendFile(caller->xml_file);
936 caller->FinishSending(file_sock);
937
938 int status = UDT::close(file_sock);
939 if (status != 0)
940 caller->Err("Error udt::close %s", UDT::getlasterror().getErrorMessage());
941
942 logs.clear();
943 //enable watchdog again
944 if(caller->gcs_watchdog!=NULL) {
945 caller->gcs_watchdog->Start();
946 }
947 }
948 }
949
950 if (FD_ISSET(data_pipe, &set)) {
951 log_header_t header;
952 read(data_pipe, &header, sizeof(log_header_t));
953
954 for (size_t i = 0; i < logs.size(); i++) {
955 if (logs.at(i).device == header.device) {
956 char *buf = (char *)malloc(header.size);
957 read(data_pipe, buf, header.size);
958 // printf("%s \n",header.device->ObjectName().c_str());
959 // for(int i=0;i<header.size;i++) printf("%x ",buf[i]);
960 // printf("\n");
961 if (logs.at(i).size == header.size) {
962 if (logs.at(i).dbtFile != NULL) {
963 write_hdfile(
964 logs.at(i).dbtFile, buf, (road_time_t)(header.time / 1000),
965 (road_timerange_t)(header.time % 1000), header.size);
966 } else {
967 printf("err\n");
968 }
969 } else {
970 caller->self->Err("%s log size is not correct %i/%i\n",
971 header.device->ObjectName().c_str(),
972 header.size, logs.at(i).size);
973 }
974 free(buf);
975 }
976 }
977 }
978 }
979 }
980
981#ifdef __XENO__
982 close(cmd_pipe);
983 close(data_pipe);
984#endif
985
986 pthread_exit(0);
987}
988
989string FrameworkManager_impl::FileName(IODevice *device) {
990 return getFrameworkManager()->ObjectName() + "_" + device->ObjectName();
991}
992
993void FrameworkManager_impl::PrintXml(void) const {
994 xmlChar *xmlbuff;
995 int buffersize;
996 xmlDocDumpFormatMemory(file_doc, &xmlbuff, &buffersize, 1);
997 Printf("xml:\n%s\n", xmlbuff);
998 xmlFree(xmlbuff);
999}
Note: See TracBrowser for help on using the repository browser.