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