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

Modified detectLostSupRob

Browse source
This commit is contained in:
Raphael Benistant 2020-03-28 17:19:31 +01:00
parent 7be6073378
commit 67f843142d
2 changed files with 118 additions and 46 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -114,7 +114,12 @@ void Tasks::Init() {
if (err = rt_mutex_create(&mutex_acquireImage, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
}
if (err = rt_mutex_create(&mutex_killOpenComRobot, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
@ -161,14 +166,31 @@ 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);
}
//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,44 +248,49 @@ void Tasks::Init() {
* @brief Démarrage des tâches
*/
void Tasks::Run() {
rt_task_set_priority(NULL, T_LOPRIO);
int err;
if (err = rt_task_start(&th_server, (void(*)(void*)) & Tasks::ServerTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_receiveFromMon, (void(*)(void*)) & Tasks::ReceiveFromMonTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_startRobotWithoutWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithoutWatchdog, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_startRobotWithWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithWatchdog, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_move, (void(*)(void*)) & Tasks::MoveTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
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;
while(1){
rt_sem_p(&sem_restart, TM_INFINITE);
rt_task_set_priority(NULL, T_LOPRIO);
int err;
if (err = rt_task_start(&th_server, (void(*)(void*)) & Tasks::ServerTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_receiveFromMon, (void(*)(void*)) & Tasks::ReceiveFromMonTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_startRobotWithoutWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithoutWatchdog, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_startRobotWithWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithWatchdog, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_move, (void(*)(void*)) & Tasks::MoveTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
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;
}
}
/**
@ -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);
}
/**
@ -390,7 +433,10 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=0;
rt_mutex_release(&mutex_acquireImage);
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);

7
software/raspberry/superviseur-robot/tasks.h
View file

@ -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 */