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);
@@ -214,6 +280,11 @@ void Tasks::Run() {
         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;
 }
 
@@ -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;

software/raspberry/superviseur-robot/tasks.h

@@ -66,6 +66,7 @@ private:
     ComRobot robot;
     Camera camera;
     int robotStarted = 0;
+    int compteurRobotLoss = 0;
     int cameraStarted = 0;
     int searchArena = 0;
     int robotPos = 0;
@@ -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,6 +93,7 @@ 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;
@@ -103,6 +106,7 @@ private:
     RT_SEM sem_serverOk;
     RT_SEM sem_startRobot;
     RT_SEM sem_startCamera;
+    RT_SEM sem_connexionEstablished;
 
     /**********************************************************************/
     /* Message queues                                                     */
@@ -154,6 +158,11 @@ private:
     */
     void BatteryTask(void *arg);
 
+    /**
+    * @brief task in charge of counting packet losses.
+    */
+    void RobotLossCounter(void *arg);
+
 
     /**
     * @brief task in charge of managing the camera.