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