Browse Source

th_rcvFromMon Watchdog et RobotStart faits et a tester

Nabil Moukhlis 10 months ago
parent
commit
a100a35c08

+ 103
- 35
software/raspberry/superviseur-robot/tasks.cpp View File

@@ -276,31 +276,53 @@ void Tasks::ReceiveFromMonTask(void *arg) {
276 276
     /**************************************************************************************/
277 277
     /* The task receiveFromMon starts here                                                */
278 278
     /**************************************************************************************/
279
+    while(1){
279 280
     rt_sem_p(&sem_serverOk, TM_INFINITE);
280 281
     cout << "Received message from monitor activated" << endl << flush;
281 282
 
282
-    while (1) {
283
-        msgRcv = monitor.Read();
284
-        cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
285
-
286
-        if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
287
-            delete(msgRcv);
288
-            exit(-1);
289
-        } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
290
-            rt_sem_v(&sem_openComRobot);
291
-        } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
292
-            rt_sem_v(&sem_startRobot);
293
-        } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
294
-                msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
295
-                msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
296
-                msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) ||
297
-                msgRcv->CompareID(MESSAGE_ROBOT_STOP)) {
283
+        while (1) {
284
+            msgRcv = monitor.Read();
285
+            cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
286
+
287
+            if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
288
+                delete(msgRcv);
289
+                WriteInQueue(&q_messageComRobot, new Message(MESSAGE_ROBOT_COM_CLOSE));
290
+                WriteInQueue(&q_messageControlRobot, new Message(MESSAGE_ROBOT_RESET));
291
+                WriteInQueue(&q_messageControlCam, new Message(MESSAGE_CAM_CLOSE));
292
+                rt_sem_v(&sem_restartServer, TM_INFINITE);  // A VERIFIER
293
+                break;
294
+
295
+            } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN) || msgRcv->CompareID(MESSAGE_ROBOT_COM_CLOSE)) {
296
+                WriteInQueue(&q_messageComRobot, msgRcv);
297
+                //rt_sem_v(&sem_openComRobot);
298
+            } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD) || msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
299
+                WriteInQueue(&q_messageControlRobot, msgRcv);
300
+                //rt_sem_v(&sem_startRobot);
301
+            } 
302
+            else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
303
+                    msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
304
+                    msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
305
+                    msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) ||
306
+                    msgRcv->CompareID(MESSAGE_ROBOT_STOP)) {
307
+                rt_mutex_acquire(&mutex_move, TM_INFINITE);
308
+                move = msgRcv->GetID();
309
+                rt_mutex_release(&mutex_move);
310
+            }
298 311
 
299
-            rt_mutex_acquire(&mutex_move, TM_INFINITE);
300
-            move = msgRcv->GetID();
301
-            rt_mutex_release(&mutex_move);
312
+            else if (msgRcv->CompareID(MESSAGE_CAM_OPEN) || 
313
+                    msgRcv->CompareID(MESSAGE_CAM_CLOSE) || 
314
+                    msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA) || 
315
+                    msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) || 
316
+                    msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM) || 
317
+                    msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START) || 
318
+                    msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP) || 
319
+                    )
320
+            {
321
+                WriteInQueue(&q_messageControlCam, msgRcv);
322
+            }
323
+            delete(msgRcv); // mus be deleted manually, no consumer
302 324
         }
303
-        delete(msgRcv); // mus be deleted manually, no consumer
325
+        
304 326
     }
305 327
 }
306 328
 
@@ -350,23 +372,70 @@ void Tasks::StartRobotTask(void *arg) {
350 372
     /**************************************************************************************/
351 373
     while (1) {
352 374
 
375
+        Message * tmp;
353 376
         Message * msgSend;
354
-        rt_sem_p(&sem_startRobot, TM_INFINITE);
355
-        cout << "Start robot without watchdog (";
356
-        rt_mutex_acquire(&mutex_robot, TM_INFINITE);
357
-        msgSend = robot.Write(robot.StartWithoutWD());
358
-        rt_mutex_release(&mutex_robot);
359
-        cout << msgSend->GetID();
360
-        cout << ")" << endl;
361
-
362
-        cout << "Movement answer: " << msgSend->ToString() << endl << flush;
363
-        WriteInQueue(&q_messageToMon, msgSend);  // msgSend will be deleted by sendToMon
364
-
365
-        if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
377
+        tmp = ReadInQueue(&q_messageControlRobot);
378
+        if (tmp -> CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)){
379
+            cout << "Start robot without watchdog (";
380
+            rt_mutex_acquire(&mutex_robot, TM_INFINITE);
381
+            msgSend = robot.Write(robot.StartWithoutWD());
382
+            rt_mutex_release(&mutex_robot);
383
+            cout << msgSend->GetID();
384
+            cout << ")" << endl;
385
+            cout << "Movement answer: " << msgSend->ToString() << endl << flush;
386
+            WriteInQueue(&q_messageToMon, msgSend);  // msgSend will be deleted by sendToMon
387
+            if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
388
+                rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
389
+                robotStarted = 1;
390
+                rt_mutex_release(&mutex_robotStarted);
391
+            }
392
+        } else if (tmp -> CompareID(MESSAGE_ROBOT_START_WITH_WD)){
393
+            cout << "Start robot with watchdog (";
394
+            rt_mutex_acquire(&mutex_robot, TM_INFINITE);
395
+            msgSend = robot.Write(robot.StartWithWD());
396
+            rt_mutex_release(&mutex_robot);
397
+            cout << msgSend->GetID();
398
+            cout << ")" << endl;
399
+            cout << "Movement answer: " << msgSend->ToString() << endl << flush;
400
+            WriteInQueue(&q_messageToMon, msgSend);  // msgSend will be deleted by sendToMon
401
+            if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
402
+                rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
403
+                robotStarted = 1;
404
+                rt_mutex_release(&mutex_robotStarted);
405
+                rt_task_set_periodic(&th_watchDog,TM_NOW,1000000000);
406
+            }
407
+            
408
+        } else if (tmp -> CompareID(MESSAGE_ROBOT_RESET)){
409
+            rt_task_set_periodic(&th_watchDog,TM_NOW,0);
410
+            cout << "Stopping Robot (";
411
+            rt_mutex_acquire(&mutex_robot, TM_INFINITE);
412
+            msgSend = robot.Write(new Message(MESSAGE_ROBOT_RESET));
413
+            rt_mutex_release(&mutex_robot);
414
+            cout << msgSend->GetID();
415
+            cout << ")" << endl;
416
+            cout << "Movement answer: " << msgSend->ToString() << endl << flush;
417
+            WriteInQueue(&q_messageToMon, msgSend);  // msgSend will be deleted by sendToMon
366 418
             rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
367
-            robotStarted = 1;
419
+            robotStarted = 0;
368 420
             rt_mutex_release(&mutex_robotStarted);
369
-        }
421
+        } 
422
+        
423
+   
424
+    }
425
+}
426
+
427
+
428
+void Tasks::WatchDog(void *arg) {
429
+    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
430
+    // Synchronization barrier (waiting that all tasks are starting)
431
+    rt_sem_p(&sem_barrier, TM_INFINITE);
432
+    rt_task_set_periodic(NULL, TM_NOW, 0);
433
+    while (1) {
434
+        rt_task_wait_period(NULL);
435
+        Message * msgSend;
436
+        rt_mutex_acquire(&mutex_robot, TM_INFINITE);
437
+        msgSend = robot.ReloadWD();
438
+        rt_mutex_release(&mutex_robot);
370 439
     }
371 440
 }
372 441
 
@@ -403,7 +472,6 @@ void Tasks::MoveTask(void *arg) {
403 472
                 rt_mutex_acquire(&mutex_robot, TM_INFINITE);
404 473
                 robot.Write(new Message((MessageID)cpMove));
405 474
                 rt_mutex_release(&mutex_robot);
406
-                
407 475
                 previousMove = cpMove;
408 476
             }
409 477
         }

+ 8
- 2
software/raspberry/superviseur-robot/tasks.h View File

@@ -99,6 +99,8 @@ private:
99 99
     RT_SEM sem_openComRobot;
100 100
     RT_SEM sem_serverOk;
101 101
     RT_SEM sem_startRobot;
102
+    RT_SEM sem_watchdogStart;
103
+    RT_SEM sem_restartServer;
102 104
     
103 105
     
104 106
 
@@ -107,7 +109,6 @@ private:
107 109
     /**********************************************************************/
108 110
     int MSG_QUEUE_SIZE;
109 111
     RT_QUEUE q_messageToMon;
110
-    
111 112
     RT_QUEUE q_messageComRobot;
112 113
     RT_QUEUE q_messageControlRobot;
113 114
     RT_QUEUE q_messageControlCam;
@@ -168,7 +169,12 @@ private:
168 169
      * @return Nothing
169 170
      */
170 171
     void ReadBattery(void *arg);
171
-
172
+    
173
+    /**
174
+     * Sends periodically a message to the robot
175
+     * @return Nothing
176
+     */
177
+    void Watchdog(void *arg);
172 178
 
173 179
 };
174 180
 

Loading…
Cancel
Save