Modified detectLostSupRob
This commit is contained in:
parent
7be6073378
commit
67f843142d
2 changed files with 118 additions and 46 deletions
|
|
@ -116,6 +116,11 @@ void Tasks::Init() {
|
|||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
if (err = rt_mutex_create(&mutex_killOpenComRobot, NULL)) {
|
||||
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -161,7 +166,22 @@ void Tasks::Init() {
|
|||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
if (err = rt_sem_create(&sem_DetectLostSupRob, NULL, 0, S_FIFO)) {
|
||||
if (err = rt_sem_create(&sem_detectLostSupRob, NULL, 0, S_FIFO)) {
|
||||
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
if (err = rt_sem_create(&sem_restart, NULL, 0, S_FIFO)) {
|
||||
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
if (err = rt_sem_create(&sem_allowSendToMon, NULL, 0, S_FIFO)) {
|
||||
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
if (err = rt_sem_create(&sem_allowOpenComRobot, NULL, 0, S_FIFO)) {
|
||||
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
|
@ -169,6 +189,8 @@ void Tasks::Init() {
|
|||
//Initialization for some specific sem: Allow to run the first time
|
||||
rt_sem_v(&sem_allowStartReceive);
|
||||
rt_sem_v(&sem_allowStartDetectLostSupRob);
|
||||
rt_sem_v(&sem_restart);
|
||||
rt_sem_v(&sem_allowOpenComRobot);
|
||||
|
||||
cout << "Semaphores created successfully" << endl << flush;
|
||||
|
||||
|
|
@ -226,6 +248,10 @@ void Tasks::Init() {
|
|||
* @brief Démarrage des tâches
|
||||
*/
|
||||
void Tasks::Run() {
|
||||
|
||||
while(1){
|
||||
rt_sem_p(&sem_restart, TM_INFINITE);
|
||||
|
||||
rt_task_set_priority(NULL, T_LOPRIO);
|
||||
int err;
|
||||
|
||||
|
|
@ -265,6 +291,7 @@ void Tasks::Run() {
|
|||
|
||||
cout << "Tasks launched" << endl << flush;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Arrêt des tâches
|
||||
|
|
@ -314,24 +341,40 @@ void Tasks::ServerTask(void *arg) {
|
|||
*/
|
||||
void Tasks::SendToMonTask(void* arg) {
|
||||
Message *msg;
|
||||
bool kill_sendToMonOk=0;
|
||||
|
||||
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_allowSendToMon, TM_INFINITE);
|
||||
|
||||
//Initialize the loop condition
|
||||
rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
|
||||
killSendMon=0; // The message is deleted with the Write
|
||||
rt_mutex_release(&mutex_killSendMon);
|
||||
|
||||
/**************************************************************************************/
|
||||
/* The task sendToMon starts here */
|
||||
/**************************************************************************************/
|
||||
rt_sem_p(&sem_serverOk, TM_INFINITE);
|
||||
|
||||
while (1) {
|
||||
while (!kill_sendToMonOk) {
|
||||
cout << "wait msg to send" << endl << flush;
|
||||
msg = ReadInQueue(&q_messageToMon);
|
||||
cout << "Send msg to mon: " << msg->ToString() << endl << flush;
|
||||
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
|
||||
monitor.Write(msg); // The message is deleted with the Write
|
||||
rt_mutex_release(&mutex_monitor);
|
||||
|
||||
rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
|
||||
kill_sendToMonOk=killSendMon; // The message is deleted with the Write
|
||||
rt_mutex_release(&mutex_killSendMon);
|
||||
|
||||
}
|
||||
|
||||
cout << "Task SendToMon dies" << endl << flush;
|
||||
rt_sem_v(&sem_allowSendToMon);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -392,6 +435,9 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
|||
acquireImage=0;
|
||||
rt_mutex_release(&mutex_acquireImage);
|
||||
|
||||
//Restart all the process
|
||||
rt_sem_v(&sem_restart);
|
||||
|
||||
//exit(-1);
|
||||
|
||||
|
||||
|
|
@ -418,7 +464,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
|||
rt_mutex_release(&mutex_searchArena);
|
||||
|
||||
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
|
||||
cout << "Command Get Root Position Received" << endl << flush;
|
||||
cout << "Command Get Robot Position Received" << endl << flush;
|
||||
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
|
||||
getPosition=1;
|
||||
rt_mutex_release(&mutex_getPosition);
|
||||
|
|
@ -472,6 +518,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
|||
|
||||
}
|
||||
|
||||
cout << "Task ReceiveFromMon dies" << __PRETTY_FUNCTION__ << endl << flush;
|
||||
rt_sem_v(&sem_allowStartReceive);
|
||||
}
|
||||
|
||||
|
|
@ -481,15 +528,24 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
|||
void Tasks::OpenComRobot(void *arg) {
|
||||
int status;
|
||||
int err;
|
||||
bool killOpenComRobotOk = 0;
|
||||
|
||||
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_allowOpenComRobot, TM_INFINITE);
|
||||
|
||||
rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE);
|
||||
killOpenComRobot = 0;
|
||||
rt_mutex_release(&mutex_killOpenComRobot);
|
||||
|
||||
rt_sem_p(&sem_openComRobot, TM_INFINITE);
|
||||
|
||||
/**************************************************************************************/
|
||||
/* The task openComRobot starts here */
|
||||
/**************************************************************************************/
|
||||
while (1) {
|
||||
while (!killOpenComRobot) {
|
||||
rt_sem_p(&sem_openComRobot, TM_INFINITE);
|
||||
cout << "Open serial com (";
|
||||
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
||||
|
|
@ -507,8 +563,15 @@ void Tasks::OpenComRobot(void *arg) {
|
|||
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
|
||||
|
||||
//Trigger Detection of Communication Loss with Robot
|
||||
rt_sem_v(&sem_DetectLostSupRob);
|
||||
rt_sem_v(&sem_detectLostSupRob);
|
||||
|
||||
rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE);
|
||||
killOpenComRobotOk = killOpenComRobot;
|
||||
rt_mutex_release(&mutex_killOpenComRobot);
|
||||
|
||||
}
|
||||
|
||||
rt_sem_v(&sem_allowOpenComRobot);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -677,14 +740,18 @@ void Tasks::DetectLostSupRob(void *arg){
|
|||
rt_sem_p(&sem_allowStartDetectLostSupRob, TM_INFINITE);
|
||||
|
||||
//Wait the Communication with the Robot to be Set
|
||||
rt_sem_p(&sem_DetectLostSupRob, TM_INFINITE);
|
||||
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);
|
||||
|
||||
//Period = 1s
|
||||
rt_task_set_periodic(NULL, TM_NOW, 500000000);
|
||||
|
||||
while(!kill_detectLostSupRobOk){
|
||||
rt_task_wait_period(NULL);
|
||||
|
||||
//Test Communication with Robot
|
||||
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
||||
|
|
|
|||
|
|
@ -75,6 +75,7 @@ private:
|
|||
bool acquireImage=0;
|
||||
bool getPosition=0;
|
||||
bool drawArena=0;
|
||||
bool killOpenComRobot=0;
|
||||
|
||||
|
||||
|
||||
|
|
@ -108,6 +109,7 @@ private:
|
|||
RT_MUTEX mutex_killDetectLostSupRob;
|
||||
RT_MUTEX mutex_killStartRob;
|
||||
RT_MUTEX mutex_acquireImage;
|
||||
RT_MUTEX mutex_killOpenComRobot;
|
||||
|
||||
/**********************************************************************/
|
||||
/* Semaphores */
|
||||
|
|
@ -119,7 +121,10 @@ private:
|
|||
RT_SEM sem_startRobotWithWatchdog;
|
||||
RT_SEM sem_allowStartReceive;
|
||||
RT_SEM sem_allowStartDetectLostSupRob;
|
||||
RT_SEM sem_DetectLostSupRob;
|
||||
RT_SEM sem_detectLostSupRob;
|
||||
RT_SEM sem_restart;
|
||||
RT_SEM sem_allowSendToMon;
|
||||
RT_SEM sem_allowOpenComRobot;
|
||||
|
||||
/**********************************************************************/
|
||||
/* Message queues */
|
||||
|
|
|
|||
Loading…
Reference in a new issue