From f78ec42f7b0ca06e7da8cded4275b380fd961c96 Mon Sep 17 00:00:00 2001 From: Arnaud Vergnet Date: Wed, 17 Mar 2021 10:30:02 +0100 Subject: [PATCH] separate com and robot management --- .../raspberry/superviseur-robot/tasks.cpp | 128 +++++++++++------- software/raspberry/superviseur-robot/tasks.h | 6 +- 2 files changed, 85 insertions(+), 49 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 59dcf30..4c232b2 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -101,11 +101,15 @@ void Tasks::Init() { cerr << "Error semaphore create sem_openComRobot: " << strerror(-err) << endl; exit(EXIT_FAILURE); } + if ((err = rt_sem_create(&sem_closeComRobot, nullptr, 0, S_FIFO))) { + cerr << "Error semaphore create sem_closeComRobot: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } if ((err = rt_sem_create(&sem_startRobot, nullptr, 0, S_FIFO))) { cerr << "Error semaphore create sem_startRobot: " << strerror(-err) << endl; exit(EXIT_FAILURE); } - if ((err = rt_sem_create(&sem_stopRobot, nullptr, 0, S_FIFO))) { + if ((err = rt_sem_create(&sem_stopRobot, nullptr, 0, S_PULSE))) { cerr << "Error semaphore create sem_stopRobot: " << strerror(-err) << endl; exit(EXIT_FAILURE); } @@ -139,6 +143,10 @@ void Tasks::Init() { cerr << "Error task create th_sendBatteryLevel: " << strerror(-err) << endl; exit(EXIT_FAILURE); } + if ((err = rt_task_create(&th_manageRobotCom, "th_manageRobotCom", 0, PRIORITY_TSTARTROBOT, 0))) { + cerr << "Error task create th_manageRobotCom: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } if ((err = rt_task_create(&th_manageRobot, "th_manageRobot", 0, PRIORITY_TSTARTROBOT, 0))) { cerr << "Error task create th_manageRobot: " << strerror(-err) << endl; exit(EXIT_FAILURE); @@ -176,6 +184,10 @@ void Tasks::Run() { cerr << "Error task start ReceiveFromMonTask: " << strerror(-err) << endl; exit(EXIT_FAILURE); } + if (err = rt_task_start(&th_manageRobotCom, reinterpret_cast(&Tasks::ManageRobotComTask), this)) { + cerr << "Error task start ManageRobotComTask: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } if (err = rt_task_start(&th_manageRobot, reinterpret_cast(&Tasks::ManageRobotTask), this)) { cerr << "Error task start ManageRobotTask: " << strerror(-err) << endl; exit(EXIT_FAILURE); @@ -241,6 +253,7 @@ void Tasks::Join() { // Wait for stopServer signal rt_sem_p(&sem_stopServer, TM_INFINITE); rt_sem_v(&sem_stopRobot); + rt_sem_v(&sem_closeComRobot); rt_mutex_acquire(&mutex_monitor, TM_INFINITE); rt_mutex_acquire(&mutex_monitorConnected, TM_INFINITE); monitorConnected = false; @@ -351,6 +364,39 @@ void Tasks::Join() { } } +/** + * @brief Thread managing the communication with the robot. + */ +[[noreturn]] void Tasks::ManageRobotComTask(void *arg) { + int status; + cout << "Start " << __PRETTY_FUNCTION__ << endl; + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); + + /* ************************************************************************************* + * The task startRobot starts here + * *************************************************************************************/ + while (true) { + rt_sem_p(&sem_openComRobot, TM_INFINITE); + cout << "Open serial com ("; + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + status = robot.Open(); + rt_mutex_release(&mutex_robot); + cout << status; + cout << ")" << endl; + if (status < 0) { + WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); // msgSend will be deleted by sendToMon + } else { + WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); // msgSend will be deleted by sendToMon + + rt_sem_p(&sem_closeComRobot, TM_INFINITE); + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + robot.Close(); + rt_mutex_release(&mutex_robot); + } + } +} + /** * @brief Thread starting the communication with the robot. */ @@ -365,56 +411,44 @@ void Tasks::Join() { * *************************************************************************************/ while (true) { Message *msgSend; - rt_sem_p(&sem_openComRobot, TM_INFINITE); - cout << "Open serial com ("; - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - status = robot.Open(); - rt_mutex_release(&mutex_robot); - cout << status; - cout << ")" << endl; - if (status < 0) { - WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); // msgSend will be deleted by sendToMon + rt_sem_p(&sem_startRobot, TM_INFINITE); + rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE); + const WatchdogMode mode = watchdogMode; + rt_mutex_release(&mutex_watchdogMode); + + if (mode == WITH_WATCHDOG) { + cout << "Start robot with watchdog ("; + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + msgSend = robot.Write(ComRobot::StartWithWD()); + rt_mutex_release(&mutex_robot); + cout << msgSend->GetID(); + cout << ")" << endl; } else { - 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); - const WatchdogMode mode = watchdogMode; - rt_mutex_release(&mutex_watchdogMode); - - if (mode == WITH_WATCHDOG) { - cout << "Start robot with watchdog ("; - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - msgSend = robot.Write(ComRobot::StartWithWD()); - rt_mutex_release(&mutex_robot); - cout << msgSend->GetID(); - cout << ")" << endl; - } else { - cout << "Start robot without watchdog ("; - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - msgSend = robot.Write(ComRobot::StartWithoutWD()); - rt_mutex_release(&mutex_robot); - cout << msgSend->GetID(); - cout << ")" << endl; - } + cout << "Start robot without watchdog ("; + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + msgSend = robot.Write(ComRobot::StartWithoutWD()); + rt_mutex_release(&mutex_robot); + cout << msgSend->GetID(); + cout << ")" << endl; + } - cout << "Movement answer: " << msgSend->ToString() << endl; - WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon + cout << "Start answer: " << msgSend->ToString() << endl; + WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon - if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { - rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); - robotStarted = 1; - rt_mutex_release(&mutex_robotStarted); - rt_sem_p(&sem_stopRobot, TM_INFINITE); - rt_mutex_acquire(&mutex_robot, TM_INFINITE); - robot.Write(new Message(MESSAGE_ROBOT_STOP)); - robot.Close(); - rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); - robotStarted = 0; - rt_mutex_release(&mutex_robotStarted); - rt_mutex_release(&mutex_robot); - } + if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + robotStarted = 1; + cout << "robot started" << endl; + rt_mutex_release(&mutex_robotStarted); + rt_sem_p(&sem_stopRobot, TM_INFINITE); + rt_mutex_acquire(&mutex_robot, TM_INFINITE); + robot.Write(new Message(MESSAGE_ROBOT_STOP)); + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + robotStarted = 0; + cout << "robot stopped" << endl; + rt_mutex_release(&mutex_robotStarted); + rt_mutex_release(&mutex_robot); } } } diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index 5a77929..0bbc459 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -81,6 +81,7 @@ private: RT_TASK th_receiveFromMon; RT_TASK th_move; RT_TASK th_sendBatteryLevel; + RT_TASK th_manageRobotCom; RT_TASK th_manageRobot; /* ********************************************************************* @@ -99,6 +100,7 @@ private: RT_SEM sem_barrier; RT_SEM sem_serverOk; RT_SEM sem_openComRobot; + RT_SEM sem_closeComRobot; RT_SEM sem_startRobot; RT_SEM sem_stopRobot; RT_SEM sem_stopServer; @@ -129,9 +131,9 @@ private: [[noreturn]] void ReceiveFromMonTask(void *arg); /** - * @brief Thread opening communication with the robot. + * @brief Thread managing communication with the robot. */ - [[noreturn]] void OpenComRobot(void *arg); + [[noreturn]] void ManageRobotComTask(void *arg); /** * @brief Thread starting the communication with the robot.