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

Modified semaphores for all function for detectLostSupRob

Browse source
This commit is contained in:
Raphael Benistant 2020-03-28 18:59:31 +01:00
parent 67f843142d
commit 07c5afc82a
1 changed files with 14 additions and 19 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -171,11 +171,6 @@ void Tasks::Init() {
exit(EXIT_FAILURE); 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)) { if (err = rt_sem_create(&sem_allowSendToMon, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush; cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -249,9 +244,6 @@ void Tasks::Init() {
*/ */
void Tasks::Run() { void Tasks::Run() {
while(1){
rt_sem_p(&sem_restart, TM_INFINITE);
rt_task_set_priority(NULL, T_LOPRIO); rt_task_set_priority(NULL, T_LOPRIO);
int err; int err;
@ -290,7 +282,7 @@ void Tasks::Run() {
} }
cout << "Tasks launched" << endl << flush; cout << "Tasks launched" << endl << flush;
}
} }
/** /**
@ -436,7 +428,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_release(&mutex_acquireImage); rt_mutex_release(&mutex_acquireImage);
//Restart all the process //Restart all the process
rt_sem_v(&sem_restart); //Tasks::Run();
//exit(-1); //exit(-1);
@ -449,6 +441,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
cout << "Command Open Communication with Robot Received" << endl << flush; cout << "Command Open Communication with Robot Received" << endl << flush;
rt_sem_v(&sem_openComRobot); rt_sem_v(&sem_openComRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) { } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
cout << "Command Start Robot without Watchdog Received" << endl << flush; cout << "Command Start Robot without Watchdog Received" << endl << flush;
rt_sem_v(&sem_startRobotWithoutWatchdog); rt_sem_v(&sem_startRobotWithoutWatchdog);
@ -534,18 +527,18 @@ void Tasks::OpenComRobot(void *arg) {
// Synchronization barrier (waiting that all tasks are starting) // Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
rt_sem_p(&sem_allowOpenComRobot, TM_INFINITE); /*rt_sem_p(&sem_allowOpenComRobot, TM_INFINITE);
rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE); rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE);
killOpenComRobot = 0; killOpenComRobot = 0;
rt_mutex_release(&mutex_killOpenComRobot); rt_mutex_release(&mutex_killOpenComRobot);*/
rt_sem_p(&sem_openComRobot, TM_INFINITE);
/**************************************************************************************/ /**************************************************************************************/
/* The task openComRobot starts here */ /* The task openComRobot starts here */
/**************************************************************************************/ /**************************************************************************************/
while (!killOpenComRobot) { //while (!killOpenComRobot) {
while (1) {
rt_sem_p(&sem_openComRobot, TM_INFINITE); rt_sem_p(&sem_openComRobot, TM_INFINITE);
cout << "Open serial com ("; cout << "Open serial com (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
@ -556,8 +549,10 @@ void Tasks::OpenComRobot(void *arg) {
Message * msgSend; Message * msgSend;
if (status < 0) { if (status < 0) {
cout << "J'ai raté la co" << endl << flush;
msgSend = new Message(MESSAGE_ANSWER_NACK); msgSend = new Message(MESSAGE_ANSWER_NACK);
} else { } else {
cout << "J'ai réussi la co" << endl << flush;
msgSend = new Message(MESSAGE_ANSWER_ACK); msgSend = new Message(MESSAGE_ANSWER_ACK);
} }
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
@ -565,9 +560,9 @@ void Tasks::OpenComRobot(void *arg) {
//Trigger Detection of Communication Loss with Robot //Trigger Detection of Communication Loss with Robot
rt_sem_v(&sem_detectLostSupRob); rt_sem_v(&sem_detectLostSupRob);
rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE); /*rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE);
killOpenComRobotOk = killOpenComRobot; killOpenComRobotOk = killOpenComRobot;
rt_mutex_release(&mutex_killOpenComRobot); rt_mutex_release(&mutex_killOpenComRobot);*/
} }
@ -748,7 +743,7 @@ void Tasks::DetectLostSupRob(void *arg){
rt_mutex_release(&mutex_killDetectLostSupRob); rt_mutex_release(&mutex_killDetectLostSupRob);
//Period = 1s //Period = 1s
rt_task_set_periodic(NULL, TM_NOW, 500000000); rt_task_set_periodic(NULL, TM_NOW, 1000000000);
while(!kill_detectLostSupRobOk){ while(!kill_detectLostSupRobOk){
rt_task_wait_period(NULL); rt_task_wait_period(NULL);