From 7be6073378f1b6aff36aa3fc52ab955bd49385d6 Mon Sep 17 00:00:00 2001 From: Raphael Date: Sat, 28 Mar 2020 16:04:32 +0100 Subject: [PATCH] Create DetectLostSupRob --- .../raspberry/superviseur-robot/tasks.cpp | 84 ++++++++++++++++++- software/raspberry/superviseur-robot/tasks.h | 11 ++- 2 files changed, 92 insertions(+), 3 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index f4acecf..c812d0e 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/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 diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index 83c57c7..21d3786 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/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 */