software/raspberry/superviseur-robot/tasks.cpp

@@ -368,14 +368,13 @@ void Tasks::ReceiveFromMonTask(void *arg) {
     
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
         
+    while(1){
         // Synchronization barrier (waiting that all tasks are starting)
         rt_sem_p(&sem_barrier, TM_INFINITE);
 
-    while(1){
         //rt_sem_p(&sem_restart,TM_INFINITE);
 
         //Reinitialize control boolean 
-        killReceiveFromMonOk = 0;
         rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
             killReceiveFromMon=0;
         rt_mutex_release(&mutex_killReceiveFromMon);
@@ -391,6 +390,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
 
             if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
+                delete(msgRcv);
                 cout << "Connection to monitor lost" << endl;
 
                 rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
@@ -419,19 +419,16 @@ void Tasks::ReceiveFromMonTask(void *arg) {
                     acquireImage=0;
                 rt_mutex_release(&mutex_acquireImage);
 
-                //Wait every task to die
                 sleep(1);
 
-                //All closes
                 monitor.Close();
                 robot.Close();
 
                 //Tasks::Join();
                 
-                //Release restarted tasks
+                cout << "End sleep" << endl << flush;
-//                cout << "End sleep" << endl << flush;
                 rt_sem_broadcast(&sem_barrier);             
-//                cout << "Mes couilles" << endl << flush;
+                cout << "Mes couilles" << endl << flush;
 
                 //exit(-1);
 
@@ -521,9 +518,13 @@ void Tasks::ReceiveFromMonTask(void *arg) {
 
             //Update loop condition
 
+            cout << "J'arrive la" << endl << flush;
+
             rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
                 killReceiveFromMonOk = killReceiveFromMon;
             rt_mutex_release(&mutex_killReceiveFromMon);
+
+            cout << "Kill Receive From Mon Ok = " << killReceiveFromMonOk << endl << flush;
         }
 
         cout << "ReceiveFromMon dies" << endl << flush;
@@ -642,6 +643,8 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) {
     
     while(1){
     
+        rt_sem_p(&sem_restart, TM_INFINITE);
+        
         rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE);
         cout << "Start robot with watchdog (";
         rt_mutex_acquire(&mutex_robot, TM_INFINITE);
@@ -731,21 +734,16 @@ void Tasks::DetectLostSupRob(void *arg){
     bool kill_detectLostSupRobOk=0;
     Message* msgSend;
     
-    //Period = 1s
-    rt_task_set_periodic(NULL, TM_NOW, 1000000000);
-
-    while(1){
-    
     //Wait the Communication with the Robot to be Set
     rt_sem_p(&sem_detectLostSupRob, TM_INFINITE);
-        cout << "Start DetectLostSupRob" << endl << flush;
     
-        kill_detectLostSupRobOk = 0;
     //Initialize the variable for the loop condition
     rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
         killDetectLostSupRob=0;
     rt_mutex_release(&mutex_killDetectLostSupRob);
     
+    //Period = 1s
+    rt_task_set_periodic(NULL, TM_NOW, 1000000000);
     
     while(!kill_detectLostSupRobOk){
         rt_task_wait_period(NULL);
@@ -754,8 +752,6 @@ void Tasks::DetectLostSupRob(void *arg){
         rt_mutex_acquire(&mutex_robot, TM_INFINITE);
             msgSend = robot.Write(robot.Ping());
         rt_mutex_release(&mutex_robot);
-            cout << "J'écris un message" << endl << flush;
-            
         
         if(msgSend->GetID() == MESSAGE_ANSWER_COM_ERROR || msgSend->GetID() == MESSAGE_ANSWER_ROBOT_TIMEOUT){
             
@@ -786,7 +782,6 @@ void Tasks::DetectLostSupRob(void *arg){
     }
     cout << "DetectLostSupRob dies";
 }
-}
 
 /**
  * Write a message in a given queue