benistan/real_time
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Restart detectLostSupMon ok but ping doesn't work after restart

Browse source
This commit is contained in:
Raphael Benistant 2020-03-29 12:34:28 +02:00
parent 153fc3a049
commit e479fec3eb
1 changed files with 50 additions and 42 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

86
software/raspberry/superviseur-robot/tasks.cpp
View file

@ -731,53 +731,61 @@ void Tasks::DetectLostSupRob(void *arg){
bool kill_detectLostSupRobOk=0;
Message* msgSend;
//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);
//Period = 1s
rt_task_set_periodic(NULL, TM_NOW, 1000000000);
while(!kill_detectLostSupRobOk){
rt_task_wait_period(NULL);
while(1){
//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=1;
rt_mutex_release(&mutex_killDetectLostSupRob);
//Trigger Kill of the Battery acquisition
rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
killBattery=1;
rt_mutex_release(&mutex_killBattery);
}
}
else{
cout << "I got an Answer from Robot" << endl << flush;
}
//Wait the Communication with the Robot to be Set
rt_sem_p(&sem_detectLostSupRob, TM_INFINITE);
cout << "Start DetectLostSupRob" << endl << flush;
kill_detectLostSupRobOk = 0;
//Initialize the variable for the loop condition
rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
kill_detectLostSupRobOk=killDetectLostSupRob;
killDetectLostSupRob = 0;
rt_mutex_release(&mutex_killDetectLostSupRob);
while(!kill_detectLostSupRobOk){
rt_task_wait_period(NULL);
//Test Communication with Robot
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.Write(robot.Ping());
rt_mutex_release(&mutex_robot);
cout << "J'écris un message" << endl << flush;
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=1;
rt_mutex_release(&mutex_killDetectLostSupRob);
//Trigger Kill of the Battery acquisition
rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
killBattery=1;
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);
}
cout << "DetectLostSupRob dies";
}
cout << "DetectLostSupRob dies";
}
/**