added 3 loss timeout

software/raspberry/superviseur-robot/tasks.cpp

@@ -27,6 +27,7 @@
 #define PRIORITY_TSTARTROBOT 20
 #define PRIORITY_TCAMERA 21
 #define PRIORITY_TBATTERY 25
+#define PRIORITY_TLOSSCOUNTER 25
 #define PRIORITY_TSTARTCAMERA 25
 
 /*
@@ -52,6 +53,58 @@
  * @brief Initialisation des structures de l'application (tâches, mutex, 
  * semaphore, etc.)
  */
+void Tasks::robotLossCounter(){
+
+    // wait for connexion establishement
+    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
+    // Synchronization barrier (waiting that all tasks are starting)
+    rt_sem_p(&sem_barrier, TM_INFINITE);
+    rt_sem_p(&sem_connexionEstablished);
+    cout << "Starting to monitor packet losses " << endl << flush;
+    rt_task_set_periodic(NULL, TM_NOW, 100*1000*1000);
+    while(1){
+        rt_task_wait_period(NULL);
+
+        rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
+        if(compteurRobotLoss >= 3){
+            // send info to monitor
+            WriteInQueue(&q_messageToMon, new Message(MESSAGE_MONITOR_LOST));
+
+            // close robot
+            rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+            state = robot.Close();
+            rt_mutex_release(&mutex_robot);
+
+            // send info to monitor
+            if (state < 0) {
+                WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
+            } else {
+                WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
+            }
+        }
+        rt_mutex_release(&mutex_compteurRobotLoss);
+    }
+
+}
+
+MessageID traitementMessage(MessageID msg){
+    if (msg == MESSAGE_ANSWER_COM_ERROR){
+
+        // increment
+        rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
+        compteurRobotLoss ++;
+        rt_mutex_release(&mutex_compteurRobotLoss);
+    } else {
+
+        // reset
+        rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
+        compteurRobotLoss = 0;
+        rt_mutex_release(&mutex_compteurRobotLoss);
+    }
+    return msg
+}
+
+
 void Tasks::Init() {
     int status;
     int err;
@@ -75,6 +128,11 @@ void Tasks::Init() {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_mutex_create(&mutex_compteurRobotLoss, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+
     if (err = rt_mutex_create(&mutex_cameraStarted, NULL)) {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
@@ -104,6 +162,10 @@ void Tasks::Init() {
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_sem_create(&sem_connexionEstablished, NULL, 0, S_FIFO)) {
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     if (err = rt_sem_create(&sem_serverOk, NULL, 0, S_FIFO)) {
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
@@ -153,6 +215,10 @@ void Tasks::Init() {
         cerr << "Error task create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_task_create(&th_lossCounter, "th_lossCounter", 0, PRIORITY_TLOSSCOUNTER, 0)) {
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     if (err = rt_task_create(&th_camera, "th_camera", 0, PRIORITY_TCAMERA, 0)) {
         cerr << "Error task create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
@@ -213,7 +279,12 @@ void Tasks::Run() {
         cerr << "Error task start: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
-    
+
+    if (err = rt_task_start(&th_lossCounter, (void(*)(void*)) & Tasks::RobotLossCounter, this)) {
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+
     cout << "Tasks launched" << endl << flush;
 }
 
@@ -323,17 +394,17 @@ void Tasks::ReceiveFromMonTask(void *arg) {
         } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)){
             rt_sem_v(&sem_startCamera);
         }else if(msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){
-            rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); 
+            rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
-            searchArena = 1; 
+            searchArena = 1;
-            rt_mutex_release(&mutex_search_arena); 
+            rt_mutex_release(&mutex_search_arena);
         }else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)){
-            rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); 
+            rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
-            robotPos = 1; 
+            robotPos = 1;
-            rt_mutex_release(&mutex_robot_pos); 
+            rt_mutex_release(&mutex_robot_pos);
         }else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)){
-            rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); 
+            rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
-            robotPos = 0; 
+            robotPos = 0;
-            rt_mutex_release(&mutex_robot_pos); 
+            rt_mutex_release(&mutex_robot_pos);
         }
         delete(msgRcv); // must be deleted manually, no consumer
     }
@@ -366,6 +437,7 @@ void Tasks::OpenComRobot(void *arg) {
         if (status < 0) {
             msgSend = new Message(MESSAGE_ANSWER_NACK);
         } else {
+            rt_sem_v(&sem_connexionEstablished);
             msgSend = new Message(MESSAGE_ANSWER_ACK);
         }
         WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
@@ -389,7 +461,7 @@ void Tasks::StartRobotTask(void *arg) {
         rt_sem_p(&sem_startRobot, TM_INFINITE);
         cout << "Start robot without watchdog (";
         rt_mutex_acquire(&mutex_robot, TM_INFINITE);
-        msgSend = robot.Write(robot.StartWithoutWD());
+        msgSend = call(robot.Write(robot.StartWithoutWD()));
         rt_mutex_release(&mutex_robot);
         cout << msgSend->GetID();
         cout << ")" << endl;
@@ -412,7 +484,7 @@ void Tasks::StartCameraTask(void *arg) {
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
     // Synchronization barrier (waiting that all tasks are starting)
     rt_sem_p(&sem_barrier, TM_INFINITE);
-    
+
     /**************************************************************************************/
     /* The task startRobot starts here                                                    */
     /**************************************************************************************/
@@ -423,12 +495,12 @@ void Tasks::StartCameraTask(void *arg) {
         rt_sem_p(&sem_startCamera, TM_INFINITE);
         cout << "Start Camera ";
         rt_mutex_acquire(&mutex_camera, TM_INFINITE);
-        bool status = camera.Open(); 
+        bool status = camera.Open();
         if(!status){
             msgSend = new Message(MESSAGE_ANSWER_NACK);
             WriteInQueue(&q_messageToMon, msgSend);  // msgSend will be deleted by sendToMon
             cout << "NACK !" << endl << flush ;
-            cout << msgSend->ToString() << endl << flush; 
+            cout << msgSend->ToString() << endl << flush;
             cout << "NACK1 ! " << endl << flush ;
             rt_mutex_release(&mutex_camera);
             cout << "NACK2 ! " << endl << flush ;
@@ -443,9 +515,9 @@ void Tasks::StartCameraTask(void *arg) {
         rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
         cout << "ff" << endl << flush ;
         cameraStarted = 1;
-        cout << "Cam success! " << endl <<flush; 
+        cout << "Cam success! " << endl <<flush;
         rt_mutex_release(&mutex_cameraStarted);
-  
+
         cout << "Fin de start cam" << endl << flush;
     }
 }
@@ -518,8 +590,8 @@ void Tasks::BatteryTask(void *arg) {
 * @brief task in charge of managing the camera.
 */
 void Tasks::CameraTask(void *arg){
-    
+
-    int cam; 
+    int cam;
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
     // Synchronization barrier (waiting that all tasks are starting)
     rt_sem_p(&sem_barrier, TM_INFINITE);
@@ -527,37 +599,37 @@ void Tasks::CameraTask(void *arg){
     while(1){
         rt_task_wait_period(NULL);
         rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
-        cam = cameraStarted; 
+        cam = cameraStarted;
         rt_mutex_release(&mutex_cameraStarted);
         if(!cam){
-        
+
             continue;
         }
 
         rt_mutex_acquire(&mutex_camera, TM_INFINITE);
-        // Lancer le traitement périodique 
+        // Lancer le traitement périodique
-        Img img = camera.Grab(); 
+        Img img = camera.Grab();
-        Arena arena; 
+        Arena arena;
         rt_mutex_release(&mutex_camera);
-        rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); 
+        rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
         if(searchArena){
-            // chopper l'arene 
+            // chopper l'arene
             cout << "Search arena pls ! " << endl << flush;
             arena = img.SearchArena();
-            // searchArena = 0; // toggle? 
+            // searchArena = 0; // toggle?
         }
-        rt_mutex_release(&mutex_search_arena); 
+        rt_mutex_release(&mutex_search_arena);
 
-        rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); 
+        rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
         if(robotPos){
             // chopper le robot
             cout << "Pos du robot pls !" ;
-            
+
         }
-        rt_mutex_release(&mutex_robot_pos); 
+        rt_mutex_release(&mutex_robot_pos);
         if(arena.IsEmpty()){
                 cout << "Arena empty" << endl << flush;
-                // envoi d'un no ack au moniteur 
+                // envoi d'un no ack au moniteur
             }else{
                 cout << "Draw Arena" << endl << flush;
                 img.DrawArena(arena);
@@ -565,8 +637,8 @@ void Tasks::CameraTask(void *arg){
                 // envoi de l'arene pour vérif
         }
         cout << "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAARENE" << endl << flush;
-        WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img)); 
+        WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img));
-        
+
     }
 }
 

software/raspberry/superviseur-robot/tasks.h

@@ -66,9 +66,10 @@ private:
     ComRobot robot;
     Camera camera;
     int robotStarted = 0;
+    int compteurRobotLoss = 0;
     int cameraStarted = 0;
     int searchArena = 0;
-    int robotPos = 0; 
+    int robotPos = 0;
     int move = MESSAGE_ROBOT_STOP;
     
     /**********************************************************************/
@@ -82,6 +83,7 @@ private:
     RT_TASK th_startCamera;
     RT_TASK th_move;
     RT_TASK th_battery;
+    RT_TASK th_lossCounter;
     RT_TASK th_camera;
     /**********************************************************************/
     /* Mutex                                                              */
@@ -91,9 +93,10 @@ private:
     RT_MUTEX mutex_robotStarted;
     RT_MUTEX mutex_cameraStarted;
     RT_MUTEX mutex_move;
+    RT_MUTEX mutex_compteurRobotLoss;
     RT_MUTEX mutex_camera;
     RT_MUTEX mutex_search_arena;
-    RT_MUTEX mutex_robot_pos; 
+    RT_MUTEX mutex_robot_pos;
 
     /**********************************************************************/
     /* Semaphores                                                         */
@@ -103,6 +106,7 @@ private:
     RT_SEM sem_serverOk;
     RT_SEM sem_startRobot;
     RT_SEM sem_startCamera;
+    RT_SEM sem_connexionEstablished;
 
     /**********************************************************************/
     /* Message queues                                                     */
@@ -153,13 +157,18 @@ private:
     * @brief task in charge of the battery status.
     */
     void BatteryTask(void *arg);
-    
+
-    
+    /**
+    * @brief task in charge of counting packet losses.
+    */
+    void RobotLossCounter(void *arg);
+
+
     /**
     * @brief task in charge of managing the camera.
     */
     void CameraTask(void *arg);
-    
+
     /**********************************************************************/
     /* Queue services                                                     */
     /**********************************************************************/