From 63257cb65650ed9744b2724e843c684521253f63 Mon Sep 17 00:00:00 2001 From: Yohan Simard Date: Fri, 12 Mar 2021 15:30:47 +0100 Subject: [PATCH] Implement ReceiveFromMonTask --- .../raspberry/superviseur-robot/tasks.cpp | 60 +++++++++++++++---- software/raspberry/superviseur-robot/tasks.h | 15 ++++- 2 files changed, 62 insertions(+), 13 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 1b9b517..6737d17 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -21,14 +21,14 @@ using namespace std; // Déclaration des priorités des taches -#define PRIORITY_TSERVER 30 -#define PRIORITY_TOPENCOMROBOT 20 -#define PRIORITY_TMOVE 20 -#define PRIORITY_TSENDTOMON 22 -#define PRIORITY_TRECEIVEFROMMON 25 -#define PRIORITY_TSTARTROBOT 20 -#define PRIORITY_TCAMERA 21 -#define PRIORITY_TBATTERY 23 +constexpr int PRIORITY_TSERVER = 30; +constexpr int PRIORITY_TOPENCOMROBOT = 20; +constexpr int PRIORITY_TMOVE = 20; +constexpr int PRIORITY_TSENDTOMON = 22; +constexpr int PRIORITY_TRECEIVEFROMMON = 25; +constexpr int PRIORITY_TSTARTROBOT = 20; +constexpr int PRIORITY_TCAMERA = 21; +constexpr int PRIORITY_TBATTERY = 23; /* * Some remarks: @@ -77,6 +77,10 @@ void Tasks::Init() { cerr << "Error mutex create mutex_move: " << strerror(-err) << endl; exit(EXIT_FAILURE); } + if ((err = rt_mutex_create(&mutex_watchdogMode, nullptr))) { + cerr << "Error mutex create mutex_watchdogMode: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } cout << "Mutexes created successfully" << endl; /* ************************************************************************************** @@ -102,6 +106,14 @@ void Tasks::Init() { cerr << "Error semaphore create sem_stopRobot: " << strerror(-err) << endl; exit(EXIT_FAILURE); } + if ((err = rt_sem_create(&sem_stopComRobot, nullptr, 0, S_FIFO))) { + cerr << "Error semaphore create sem_stopComRobot: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } + if ((err = rt_sem_create(&sem_stopServer, nullptr, 0, S_FIFO))) { + cerr << "Error semaphore create sem_stopServer: " << strerror(-err) << endl; + exit(EXIT_FAILURE); + } cout << "Semaphores created successfully" << endl; /* ************************************************************************************* @@ -261,7 +273,7 @@ void Tasks::ServerTask(void *arg) { /** * @brief Thread receiving data from monitor. */ -void Tasks::ReceiveFromMonTask(void *arg) { +[[noreturn]] void Tasks::ReceiveFromMonTask(void *arg) { Message *msgRcv; cout << "Start " << __PRETTY_FUNCTION__ << endl; @@ -279,11 +291,23 @@ void Tasks::ReceiveFromMonTask(void *arg) { cout << "Rcv <= " << msgRcv->ToString() << endl; if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) { - delete (msgRcv); - exit(-1); + cout << "Monitor connection lost! Stopping robot..." << endl; +// rt_sem_v(&sem_stopCamera); // TODO + rt_sem_v(&sem_stopRobot); + rt_sem_v(&sem_stopComRobot); + rt_sem_v(&sem_stopServer); } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) { rt_sem_v(&sem_openComRobot); } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) { + rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE); + watchdogMode = WITHOUT_WATCHDOG; + rt_mutex_release(&mutex_watchdogMode); + rt_sem_v(&sem_startRobot); + } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) { + // TODO: gérer la watchdog + rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE); + watchdogMode = WITH_WATCHDOG; + rt_mutex_release(&mutex_watchdogMode); rt_sem_v(&sem_startRobot); } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) || msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) || @@ -294,6 +318,20 @@ void Tasks::ReceiveFromMonTask(void *arg) { rt_mutex_acquire(&mutex_move, TM_INFINITE); move = msgRcv->GetID(); rt_mutex_release(&mutex_move); + } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) { + // TODO + } else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) { + // TODO + } else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) { + // TODO + } else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) { + // TODO + } else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) { + // TODO + } else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) { + // TODO + } else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) { + // TODO } delete (msgRcv); // mus be deleted manually, no consumer } diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index 7b793c5..1ef9457 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -34,6 +34,12 @@ #include "camera.h" #include "img.h" +enum WatchdogMode { + WITH_WATCHDOG, + WITHOUT_WATCHDOG, +}; + + class Tasks { public: /** @@ -64,7 +70,8 @@ private: ComRobot robot; int robotStarted = 0; int move = MESSAGE_ROBOT_STOP; - + WatchdogMode watchdogMode = WITHOUT_WATCHDOG; + /* ********************************************************************* * Tasks * ********************************************************************/ @@ -83,6 +90,7 @@ private: RT_MUTEX mutex_robot; RT_MUTEX mutex_robotStarted; RT_MUTEX mutex_move; + RT_MUTEX mutex_watchdogMode; /* ********************************************************************* * Semaphores @@ -92,6 +100,8 @@ private: RT_SEM sem_serverOk; RT_SEM sem_startRobot; RT_SEM sem_stopRobot; + RT_SEM sem_stopComRobot; + RT_SEM sem_stopServer; /* ********************************************************************* * Message queues @@ -116,7 +126,7 @@ private: /** * @brief Thread receiving data from monitor. */ - void ReceiveFromMonTask(void *arg); + [[noreturn]] void ReceiveFromMonTask(void *arg); /** * @brief Thread opening communication with the robot. @@ -159,5 +169,6 @@ private: }; + #endif // __TASKS_H__