From 0a74b96c5b6b9fc72dc440edf1e5c4902a116449 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rapha=C3=ABl=20LACROIX?= Date: Mon, 3 Apr 2023 13:10:30 +0200 Subject: [PATCH] added 3 loss timeout --- .../raspberry/superviseur-robot/tasks.cpp | 138 +++++++++++++----- software/raspberry/superviseur-robot/tasks.h | 19 ++- 2 files changed, 119 insertions(+), 38 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 186e3f2..3b31e75 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/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); - searchArena = 1; - rt_mutex_release(&mutex_search_arena); + rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); + searchArena = 1; + rt_mutex_release(&mutex_search_arena); }else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)){ - rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); - robotPos = 1; - rt_mutex_release(&mutex_robot_pos); + rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); + robotPos = 1; + rt_mutex_release(&mutex_robot_pos); }else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)){ - rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); - robotPos = 0; - rt_mutex_release(&mutex_robot_pos); + rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); + robotPos = 0; + 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 <