Implement ReceiveFromMonTask

software/raspberry/superviseur-robot/tasks.cpp

@@ -21,14 +21,14 @@
 using namespace std;
 
 // Déclaration des priorités des taches
-#define PRIORITY_TSERVER 30
+constexpr int PRIORITY_TSERVER = 30;
-#define PRIORITY_TOPENCOMROBOT 20
+constexpr int PRIORITY_TOPENCOMROBOT = 20;
-#define PRIORITY_TMOVE 20
+constexpr int PRIORITY_TMOVE = 20;
-#define PRIORITY_TSENDTOMON 22
+constexpr int PRIORITY_TSENDTOMON = 22;
-#define PRIORITY_TRECEIVEFROMMON 25
+constexpr int PRIORITY_TRECEIVEFROMMON = 25;
-#define PRIORITY_TSTARTROBOT 20
+constexpr int PRIORITY_TSTARTROBOT = 20;
-#define PRIORITY_TCAMERA 21
+constexpr int PRIORITY_TCAMERA = 21;
-#define PRIORITY_TBATTERY 23
+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);
+			cout << "Monitor connection lost! Stopping robot..." << endl;
-			exit(-1);
+//			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
 	}

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__