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

Last change on this file since 414 was 414, checked in by Sanahuja Guillaume, 23 months ago

added possibility to autodetect gcs ip when using autodetect as address parameter, see demos. This only works when ssh connected to a target (not in simulation)

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