Create DetectLostSupRob

software/raspberry/superviseur-robot/tasks.cpp

@@ -27,6 +27,7 @@
 #define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 22
 #define PRIORITY_TSTARTROBOTWITHWATCHDOG 22
 #define PRIORITY_TCAMERA 21
+#define PRIORITY_DETECTLOSTSUPROB 21
 
 
 /*
@@ -155,9 +156,19 @@ void Tasks::Init() {
         exit(EXIT_FAILURE);
     }   
     
+    if (err = rt_sem_create(&sem_allowStartDetectLostSupRob, NULL, 0, S_FIFO)) {
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    } 
+    
+     if (err = rt_sem_create(&sem_DetectLostSupRob, NULL, 0, S_FIFO)) {
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     
     //Initialization for some specific sem: Allow to run the first time
     rt_sem_v(&sem_allowStartReceive);
+    rt_sem_v(&sem_allowStartDetectLostSupRob);
     
     cout << "Semaphores created successfully" << endl << flush;
 
@@ -193,6 +204,11 @@ void Tasks::Init() {
         exit(EXIT_FAILURE);
     }
     
+    if (err = rt_task_create(&th_detectLostSupRob, "th_detectLostSupRob", 0, PRIORITY_DETECTLOSTSUPROB, 0)) {
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    
     cout << "Tasks created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -242,6 +258,11 @@ void Tasks::Run() {
         exit(EXIT_FAILURE);
     }
 
+    if (err = rt_task_start(&th_detectLostSupRob, (void(*)(void*)) & Tasks::DetectLostSupRob, this)) {
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }    
+    
     cout << "Tasks launched" << endl << flush;
 }
 
@@ -337,6 +358,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
     /**************************************************************************************/
     rt_sem_p(&sem_serverOk, TM_INFINITE);
     cout << "Received message from monitor activated" << endl << flush;
+    
     while (!killReceiveFromMonOk) {
         msgRcv = monitor.Read();
         cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
@@ -363,7 +385,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             rt_mutex_release(&mutex_killBattery);
 
             rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
-                killDetectlostSupRob=1;
+                killDetectLostSupRob=1;
             rt_mutex_release(&mutex_killDetectLostSupRob);
             
             rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
@@ -483,6 +505,9 @@ void Tasks::OpenComRobot(void *arg) {
             msgSend = new Message(MESSAGE_ANSWER_ACK);
         }
         WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
+        
+        //Trigger Detection of Communication Loss with Robot
+        rt_sem_v(&sem_DetectLostSupRob);
     }
 }
 
@@ -536,6 +561,9 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
             rt_mutex_release(&mutex_killBattery);
         }
     }
+    
+    //Trigger New Opening of the Communication with the Robot
+    //rt_sem_v(&sem_openComRobot);
 }
 
 
@@ -639,6 +667,60 @@ void Tasks::MoveTask(void *arg) {
     }
 }
 
+void Tasks::DetectLostSupRob(void *arg){
+    //counter to detect loss of communication
+    int cpt=0;
+    bool kill_detectLostSupRobOk=0;
+    Message* msgSend;
+    
+    //garanties twin task is dead if applicable
+    rt_sem_p(&sem_allowStartDetectLostSupRob, TM_INFINITE);
+    
+    //Wait the Communication with the Robot to be Set
+    rt_sem_p(&sem_DetectLostSupRob, TM_INFINITE);
+    
+    //Initialize the variable for the loop condition
+    rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
+        killDetectLostSupRob=0;
+    rt_mutex_release(&mutex_killDetectLostSupRob);
+    
+    while(!kill_detectLostSupRobOk){
+        
+        //Test Communication with Robot
+        rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+            msgSend = robot.Write(robot.Ping());
+        rt_mutex_release(&mutex_robot);
+        
+        if(msgSend->GetID() == MESSAGE_ANSWER_COM_ERROR || msgSend->GetID() == MESSAGE_ANSWER_ROBOT_TIMEOUT){
+            
+            cout << "Didn't Get Any Answer from Robot" << endl << flush;
+            cpt++;
+            
+            if(cpt==3){
+                //Trigger Kill of DetectLostSupRob
+                rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
+                    killDetectLostSupRob=0;
+                rt_mutex_release(&mutex_killDetectLostSupRob);
+                
+                //Trigger Kill of the Battery acquisition
+                rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
+                    killBattery=0;
+                rt_mutex_release(&mutex_killBattery);  
+               
+            }
+        }
+        
+        else{
+            cout << "I got an Answer from Robot" << endl << flush;
+        }
+        
+        rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
+            kill_detectLostSupRobOk=killDetectLostSupRob;
+        rt_mutex_release(&mutex_killDetectLostSupRob);        
+    }
+         
+    rt_sem_v(&sem_allowStartDetectLostSupRob);
+}
 
 /**
  * Write a message in a given queue

software/raspberry/superviseur-robot/tasks.h

@@ -71,7 +71,7 @@ private:
     bool killVision=0;
     bool searchArena=0;
     bool killSendMon=0;
-    bool killDetectlostSupRob=0;
+    bool killDetectLostSupRob=0;
     bool acquireImage=0;
     bool getPosition=0;
     bool drawArena=0;
@@ -87,6 +87,8 @@ private:
     RT_TASK th_startRobotWithoutWatchdog;
     RT_TASK th_startRobotWithWatchdog;
     RT_TASK th_move;
+    RT_TASK th_detectLostSupRob;
+    
     
     /**********************************************************************/
     /* Mutex                                                              */
@@ -116,6 +118,8 @@ private:
     RT_SEM sem_startRobotWithoutWatchdog;
     RT_SEM sem_startRobotWithWatchdog;
     RT_SEM sem_allowStartReceive;
+    RT_SEM sem_allowStartDetectLostSupRob;
+    RT_SEM sem_DetectLostSupRob;
     
     /**********************************************************************/
     /* Message queues                                                     */
@@ -161,7 +165,10 @@ private:
      */
     void MoveTask(void *arg);
     
-
+    /**
+     * @brief Thread handling the loss of the communication between the robot and th supervisor.
+     */
+    void DetectLostSupRob(void *arg);
     
     /**********************************************************************/
     /* Queue services                                                     */