vergnet/very-dumb
1
1
Fork 0
Code Issues Pull requests Releases Wiki Activity

add back open com robot semaphore

Browse source
This commit is contained in:
Arnaud Vergnet 2021-03-17 09:43:56 +01:00
parent ebe5dc44de
commit 45ce856660
2 changed files with 9 additions and 4 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -97,6 +97,10 @@ void Tasks::Init() {
cerr << "Error semaphore create sem_serverOk: " << strerror(-err) << endl; cerr << "Error semaphore create sem_serverOk: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if ((err = rt_sem_create(&sem_openComRobot, nullptr, 0, S_FIFO))) {
cerr << "Error semaphore create sem_openComRobot: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
if ((err = rt_sem_create(&sem_startRobot, nullptr, 0, S_FIFO))) { if ((err = rt_sem_create(&sem_startRobot, nullptr, 0, S_FIFO))) {
cerr << "Error semaphore create sem_startRobot: " << strerror(-err) << endl; cerr << "Error semaphore create sem_startRobot: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -304,7 +308,7 @@ void Tasks::Join() {
rt_sem_v(&sem_stopServer); rt_sem_v(&sem_stopServer);
lostConnection = true; lostConnection = true;
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) { } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); rt_sem_v(&sem_openComRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) { } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE); rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
watchdogMode = WITHOUT_WATCHDOG; watchdogMode = WITHOUT_WATCHDOG;
@ -361,7 +365,7 @@ void Tasks::Join() {
* *************************************************************************************/ * *************************************************************************************/
while (true) { while (true) {
Message *msgSend; Message *msgSend;
rt_sem_p(&sem_startRobot, 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);
status = robot.Open(); status = robot.Open();
@ -372,6 +376,8 @@ void Tasks::Join() {
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); // msgSend will be deleted by sendToMon WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); // msgSend will be deleted by sendToMon
} else { } else {
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); // msgSend will be deleted by sendToMon WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); // msgSend will be deleted by sendToMon
rt_sem_p(&sem_startRobot, TM_INFINITE);
rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE); rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
const WatchdogMode mode = watchdogMode; const WatchdogMode mode = watchdogMode;
rt_mutex_release(&mutex_watchdogMode); rt_mutex_release(&mutex_watchdogMode);
@ -430,7 +436,6 @@ void Tasks::Join() {
while (true) { while (true) {
rt_task_wait_period(nullptr); rt_task_wait_period(nullptr);
cout << "Periodic battery level update" << endl;
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
rs = robotStarted; rs = robotStarted;
@ -468,7 +473,6 @@ void Tasks::Join() {
while (true) { while (true) {
rt_task_wait_period(nullptr); rt_task_wait_period(nullptr);
cout << "Periodic movement update" << endl;
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
rs = robotStarted; rs = robotStarted;
rt_mutex_release(&mutex_robotStarted); rt_mutex_release(&mutex_robotStarted);

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

@ -98,6 +98,7 @@ private:
* ********************************************************************/ * ********************************************************************/
RT_SEM sem_barrier; RT_SEM sem_barrier;
RT_SEM sem_serverOk; RT_SEM sem_serverOk;
RT_SEM sem_openComRobot;
RT_SEM sem_startRobot; RT_SEM sem_startRobot;
RT_SEM sem_stopRobot; RT_SEM sem_stopRobot;
RT_SEM sem_stopServer; RT_SEM sem_stopServer;