From 07c5afc82aab7a291612b8557a3ec96d4155921a Mon Sep 17 00:00:00 2001
From: Raphael <benistan@etud.insa-toulouse.fr>
Date: Sat, 28 Mar 2020 18:59:31 +0100
Subject: [PATCH] Modified semaphores for all function for detectLostSupRob

---
 .../raspberry/superviseur-robot/tasks.cpp     | 33 ++++++++-----------
 1 file changed, 14 insertions(+), 19 deletions(-)

diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp
index a3c726f..d810a68 100644
--- a/software/raspberry/superviseur-robot/tasks.cpp
+++ b/software/raspberry/superviseur-robot/tasks.cpp
@@ -171,11 +171,6 @@ void Tasks::Init() {
         exit(EXIT_FAILURE);
     }
 
-     if (err = rt_sem_create(&sem_restart, NULL, 0, S_FIFO)) {
-        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
-        exit(EXIT_FAILURE);
-    }
-
      if (err = rt_sem_create(&sem_allowSendToMon, NULL, 0, S_FIFO)) {
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
@@ -248,9 +243,6 @@ void Tasks::Init() {
  * @brief Démarrage des tâches
  */
 void Tasks::Run() {
-    
-    while(1){
-        rt_sem_p(&sem_restart, TM_INFINITE);
 
         rt_task_set_priority(NULL, T_LOPRIO);
         int err;
@@ -290,7 +282,7 @@ void Tasks::Run() {
         }    
 
         cout << "Tasks launched" << endl << flush;
-    }
+
 }
 
 /**
@@ -436,7 +428,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             rt_mutex_release(&mutex_acquireImage);
             
             //Restart all the process
-            rt_sem_v(&sem_restart);
+            //Tasks::Run();
             
             //exit(-1);
         
@@ -449,6 +441,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             cout << "Command Open Communication with Robot Received" << endl << flush;
             rt_sem_v(&sem_openComRobot);
             
+            
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
             cout << "Command Start Robot without Watchdog Received" << endl << flush;
             rt_sem_v(&sem_startRobotWithoutWatchdog);
@@ -534,19 +527,19 @@ void Tasks::OpenComRobot(void *arg) {
     // Synchronization barrier (waiting that all tasks are starting)
     rt_sem_p(&sem_barrier, TM_INFINITE);
     
-    rt_sem_p(&sem_allowOpenComRobot, TM_INFINITE);
+    /*rt_sem_p(&sem_allowOpenComRobot, TM_INFINITE);
     
     rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE);
         killOpenComRobot = 0;
-    rt_mutex_release(&mutex_killOpenComRobot);
-
-    rt_sem_p(&sem_openComRobot, TM_INFINITE);
+    rt_mutex_release(&mutex_killOpenComRobot);*/
     
     /**************************************************************************************/
     /* The task openComRobot starts here                                                  */
     /**************************************************************************************/
-    while (!killOpenComRobot) {
-        rt_sem_p(&sem_openComRobot, TM_INFINITE);
+    //while (!killOpenComRobot) {
+    while (1) {
+    
+        rt_sem_p(&sem_openComRobot, TM_INFINITE);    
         cout << "Open serial com (";
         rt_mutex_acquire(&mutex_robot, TM_INFINITE);
         status = robot.Open();
@@ -556,8 +549,10 @@ void Tasks::OpenComRobot(void *arg) {
 
         Message * msgSend;
         if (status < 0) {
+            cout << "J'ai raté la co" << endl << flush;
             msgSend = new Message(MESSAGE_ANSWER_NACK);
         } else {
+            cout << "J'ai réussi la co" << endl << flush;            
             msgSend = new Message(MESSAGE_ANSWER_ACK);
         }
         WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
@@ -565,9 +560,9 @@ void Tasks::OpenComRobot(void *arg) {
         //Trigger Detection of Communication Loss with Robot
         rt_sem_v(&sem_detectLostSupRob);
         
-        rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE);
+        /*rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE);
             killOpenComRobotOk = killOpenComRobot;
-        rt_mutex_release(&mutex_killOpenComRobot);
+        rt_mutex_release(&mutex_killOpenComRobot);*/
         
     }
     
@@ -748,7 +743,7 @@ void Tasks::DetectLostSupRob(void *arg){
     rt_mutex_release(&mutex_killDetectLostSupRob);
     
     //Period = 1s
-    rt_task_set_periodic(NULL, TM_NOW, 500000000);
+    rt_task_set_periodic(NULL, TM_NOW, 1000000000);
     
     while(!kill_detectLostSupRobOk){
         rt_task_wait_period(NULL);