|
@@ -97,6 +97,11 @@ void Tasks::Init() {
|
97
|
97
|
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
98
|
98
|
exit(EXIT_FAILURE);
|
99
|
99
|
}
|
|
100
|
+ if (err = rt_sem_create(&sem_restartServer, NULL, 0, S_FIFO)) {
|
|
101
|
+ cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
|
102
|
+ exit(EXIT_FAILURE);
|
|
103
|
+ }
|
|
104
|
+
|
100
|
105
|
cout << "Semaphores created successfully" << endl << flush;
|
101
|
106
|
|
102
|
107
|
/**************************************************************************************/
|
|
@@ -224,18 +229,22 @@ void Tasks::ServerTask(void *arg) {
|
224
|
229
|
/**************************************************************************************/
|
225
|
230
|
/* The task server starts here */
|
226
|
231
|
/**************************************************************************************/
|
227
|
|
- rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
|
228
|
|
- status = monitor.Open(SERVER_PORT);
|
229
|
|
- rt_mutex_release(&mutex_monitor);
|
230
|
|
-
|
231
|
|
- cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl;
|
232
|
|
-
|
233
|
|
- if (status < 0) throw std::runtime_error {
|
234
|
|
- "Unable to start server on port " + std::to_string(SERVER_PORT)
|
235
|
|
- };
|
236
|
|
- monitor.AcceptClient(); // Wait the monitor client
|
237
|
|
- cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
|
238
|
|
- rt_sem_broadcast(&sem_serverOk);
|
|
232
|
+ while(1){
|
|
233
|
+ rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
|
|
234
|
+ status = monitor.Open(SERVER_PORT);
|
|
235
|
+ rt_mutex_release(&mutex_monitor);
|
|
236
|
+
|
|
237
|
+ cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl;
|
|
238
|
+
|
|
239
|
+ if (status < 0) throw std::runtime_error {
|
|
240
|
+ "Unable to start server on port " + std::to_string(SERVER_PORT)
|
|
241
|
+ };
|
|
242
|
+ monitor.AcceptClient(); // Wait the monitor client
|
|
243
|
+ cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
|
|
244
|
+ rt_sem_broadcast(&sem_serverOk);
|
|
245
|
+ rt_sem_p(&sem_restartServer);
|
|
246
|
+ }
|
|
247
|
+
|
239
|
248
|
}
|
240
|
249
|
|
241
|
250
|
/**
|
|
@@ -287,7 +296,8 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
287
|
296
|
delete(msgRcv);
|
288
|
297
|
exit(-1);
|
289
|
298
|
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
|
290
|
|
- rt_sem_v(&sem_openComRobot);
|
|
299
|
+ //rt_sem_v(&sem_openComRobot);
|
|
300
|
+ WriteInQueue(&q_messageComRobot, msgRcv);
|
291
|
301
|
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
|
292
|
302
|
rt_sem_v(&sem_startRobot);
|
293
|
303
|
} else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
|
|
@@ -319,21 +329,42 @@ void Tasks::OpenComRobot(void *arg) {
|
319
|
329
|
/* The task openComRobot starts here */
|
320
|
330
|
/**************************************************************************************/
|
321
|
331
|
while (1) {
|
322
|
|
- rt_sem_p(&sem_openComRobot, TM_INFINITE);
|
323
|
|
- cout << "Open serial com (";
|
324
|
|
- rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
325
|
|
- status = robot.Open();
|
326
|
|
- rt_mutex_release(&mutex_robot);
|
327
|
|
- cout << status;
|
328
|
|
- cout << ")" << endl << flush;
|
329
|
|
-
|
330
|
|
- Message * msgSend;
|
331
|
|
- if (status < 0) {
|
332
|
|
- msgSend = new Message(MESSAGE_ANSWER_NACK);
|
333
|
|
- } else {
|
334
|
|
- msgSend = new Message(MESSAGE_ANSWER_ACK);
|
|
332
|
+ Message * msg;
|
|
333
|
+ msg=ReadInQueue(&q_messageComRobot);
|
|
334
|
+ if(msg->CompareID(MESSAGE_ROBOT_COM_OPEN)){
|
|
335
|
+ cout << "Open serial com (";
|
|
336
|
+ rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
|
337
|
+ status = robot.Open();
|
|
338
|
+ rt_mutex_release(&mutex_robot);
|
|
339
|
+ cout << status;
|
|
340
|
+ cout << ")" << endl << flush;
|
|
341
|
+
|
|
342
|
+ Message * msgSend;
|
|
343
|
+ if (status < 0) {
|
|
344
|
+ msgSend = new Message(MESSAGE_ANSWER_NACK);
|
|
345
|
+ } else {
|
|
346
|
+ msgSend = new Message(MESSAGE_ANSWER_ACK);
|
|
347
|
+ }
|
|
348
|
+ WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
|
|
349
|
+ }else if(msg->CompareID(MESSAGE_ROBOT_COM_CLOSE)){
|
|
350
|
+ cout << "Close serial com (";
|
|
351
|
+ rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
|
352
|
+ status = robot.Close();
|
|
353
|
+ rt_mutex_release(&mutex_robot);
|
|
354
|
+ cout << status;
|
|
355
|
+ cout << ")" << endl << flush;
|
|
356
|
+
|
|
357
|
+ Message * msgSend;
|
|
358
|
+ if (status < 0) {
|
|
359
|
+ msgSend = new Message(MESSAGE_ANSWER_NACK);
|
|
360
|
+ } else {
|
|
361
|
+ msgSend = new Message(MESSAGE_ANSWER_ACK);
|
|
362
|
+ }
|
|
363
|
+ WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
|
335
|
364
|
}
|
336
|
|
- WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
|
|
365
|
+ delete(msg);
|
|
366
|
+ //rt_sem_p(&sem_openComRobot, TM_INFINITE);
|
|
367
|
+
|
337
|
368
|
}
|
338
|
369
|
}
|
339
|
370
|
|