Add manage robot task

software/raspberry/superviseur-robot/tasks.cpp

@@ -22,7 +22,6 @@ using namespace std;
 
 // Déclaration des priorités des taches
 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;
@@ -65,6 +64,10 @@ void Tasks::Init() {
 		cerr << "Error mutex create mutex_monitor: " << strerror(-err) << endl;
 		exit(EXIT_FAILURE);
 	}
+	if ((err = rt_mutex_create(&mutex_monitorConnected, nullptr))) {
+		cerr << "Error mutex create mutex_monitorConnected: " << strerror(-err) << endl;
+		exit(EXIT_FAILURE);
+	}
 	if ((err = rt_mutex_create(&mutex_robot, nullptr))) {
 		cerr << "Error mutex create mutex_robot: " << strerror(-err) << endl;
 		exit(EXIT_FAILURE);
@@ -90,10 +93,6 @@ void Tasks::Init() {
 		cerr << "Error semaphore create sem_barrier: " << strerror(-err) << endl;
 		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_serverOk, nullptr, 0, S_FIFO))) {
 		cerr << "Error semaphore create sem_serverOk: " << strerror(-err) << endl;
 		exit(EXIT_FAILURE);
@@ -106,10 +105,6 @@ 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);
@@ -131,14 +126,6 @@ void Tasks::Init() {
 		cerr << "Error task create th_receiveFromMon: " << strerror(-err) << endl;
 		exit(EXIT_FAILURE);
 	}
-	if ((err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0))) {
-		cerr << "Error task create th_openComRobot: " << strerror(-err) << endl;
-		exit(EXIT_FAILURE);
-	}
-	if ((err = rt_task_create(&th_startRobot, "th_startRobot", 0, PRIORITY_TSTARTROBOT, 0))) {
-		cerr << "Error task create th_startRobot: " << strerror(-err) << endl;
-		exit(EXIT_FAILURE);
-	}
 	if ((err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0))) {
 		cerr << "Error task create th_move: " << strerror(-err) << endl;
 		exit(EXIT_FAILURE);
@@ -148,6 +135,10 @@ void Tasks::Init() {
 		cerr << "Error task create th_sendBatteryLevel: " << 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);
+	}
 
 	cout << "Tasks created successfully" << endl;
 
@@ -181,12 +172,8 @@ void Tasks::Run() {
 		cerr << "Error task start ReceiveFromMonTask: " << strerror(-err) << endl;
 		exit(EXIT_FAILURE);
 	}
-	if (err = rt_task_start(&th_openComRobot, reinterpret_cast<void (*)(void *)>(&Tasks::OpenComRobot), this)) {
+	if (err = rt_task_start(&th_manageRobot, reinterpret_cast<void (*)(void *)>(&Tasks::ManageRobotTask), this)) {
-		cerr << "Error task start OpenComRobot: " << strerror(-err) << endl;
+		cerr << "Error task start ManageRobotTask: " << strerror(-err) << endl;
-		exit(EXIT_FAILURE);
-	}
-	if (err = rt_task_start(&th_startRobot, reinterpret_cast<void (*)(void *)>(&Tasks::StartRobotTask), this)) {
-		cerr << "Error task start StartRobotTask: " << strerror(-err) << endl;
 		exit(EXIT_FAILURE);
 	}
 	if (err = rt_task_start(&th_move, reinterpret_cast<void (*)(void *)>(&Tasks::MoveTask), this)) {
@@ -242,12 +229,19 @@ void Tasks::Join() {
 
 		monitor.AcceptClient(); // Wait the monitor client
 		cout << "Rock'n'Roll baby, client accepted!" << endl;
+		rt_mutex_acquire(&mutex_monitorConnected, TM_INFINITE);
+		monitorConnected = true;
+		rt_mutex_release(&mutex_monitorConnected);
 		rt_sem_broadcast(&sem_serverOk);
 
 		// Wait for stopServer signal
 		rt_sem_p(&sem_stopServer, TM_INFINITE);
+		rt_sem_v(&sem_stopRobot);
 		rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
+		rt_mutex_acquire(&mutex_monitorConnected, TM_INFINITE);
+		monitorConnected = false;
 		monitor.Close();
+		rt_mutex_release(&mutex_monitorConnected);
 		rt_mutex_release(&mutex_monitor);
 	}
 }
@@ -272,7 +266,11 @@ void Tasks::Join() {
 		msg = ReadInQueue(&q_messageToMon);
 		cout << "Send msg to mon: " << msg->ToString() << endl;
 		rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
-		monitor.Write(msg); // The message is deleted with the Write
+		rt_mutex_acquire(&mutex_monitorConnected, TM_INFINITE);
+		if (monitorConnected) {
+			monitor.Write(msg); // The message is deleted with the Write
+		}
+		rt_mutex_release(&mutex_monitorConnected);
 		rt_mutex_release(&mutex_monitor);
 	}
 }
@@ -302,13 +300,11 @@ void Tasks::Join() {
 
 			if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
 				cout << "Monitor connection lost! Stopping robot..." << endl;
-//			rt_sem_v(&sem_stopCamera); // TODO
+//			    rt_sem_v(&sem_stopCamera); // TODO
-				rt_sem_v(&sem_stopRobot);
-				rt_sem_v(&sem_stopComRobot);
 				rt_sem_v(&sem_stopServer);
 				lostConnection = true;
 			} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
-				rt_sem_v(&sem_openComRobot);
+				WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
 			} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
 				rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
 				watchdogMode = WITHOUT_WATCHDOG;
@@ -320,6 +316,8 @@ void Tasks::Join() {
 				watchdogMode = WITH_WATCHDOG;
 				rt_mutex_release(&mutex_watchdogMode);
 				rt_sem_v(&sem_startRobot);
+			} else if (msgRcv->CompareID(MESSAGE_ROBOT_RESET)) {
+				rt_sem_v(&sem_stopRobot);
 			} else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
 			           msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
 			           msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
@@ -349,43 +347,11 @@ void Tasks::Join() {
 	}
 }
 
-/**
- * @brief Thread opening communication with the robot.
- */
-[[noreturn]] void Tasks::OpenComRobot(void *arg) {
-	int status;
-	int err;
-
-	cout << "Start " << __PRETTY_FUNCTION__ << endl;
-	// Synchronization barrier (waiting that all tasks are starting)
-	rt_sem_p(&sem_barrier, TM_INFINITE);
-
-	/* *************************************************************************************
-	 * The task openComRobot 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;
-
-		Message *msgSend;
-		if (status < 0) {
-			msgSend = new Message(MESSAGE_ANSWER_NACK);
-		} else {
-			msgSend = new Message(MESSAGE_ANSWER_ACK);
-		}
-		WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
-	}
-}
-
 /**
  * @brief Thread starting the communication with the robot.
  */
-[[noreturn]] void Tasks::StartRobotTask(void *arg) {
+[[noreturn]] void Tasks::ManageRobotTask(void *arg) {
+	int status;
 	cout << "Start " << __PRETTY_FUNCTION__ << endl;
 	// Synchronization barrier (waiting that all tasks are starting)
 	rt_sem_p(&sem_barrier, TM_INFINITE);
@@ -396,25 +362,60 @@ void Tasks::Join() {
 	while (true) {
 		Message *msgSend;
 		rt_sem_p(&sem_startRobot, TM_INFINITE);
-		cout << "Start robot without watchdog (";
+		cout << "Open serial com (";
 		rt_mutex_acquire(&mutex_robot, TM_INFINITE);
-		msgSend = robot.Write(ComRobot::StartWithoutWD());
+		status = robot.Open();
 		rt_mutex_release(&mutex_robot);
-		cout << msgSend->GetID();
+		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_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
+			const WatchdogMode mode = watchdogMode;
+			rt_mutex_release(&mutex_watchdogMode);
 
-		cout << "Movement answer: " << msgSend->ToString() << endl;
+			if (mode == WITH_WATCHDOG) {
-		WriteInQueue(&q_messageToMon, msgSend);  // msgSend will be deleted by sendToMon
+				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;
+			}
 
-		if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
+
-			rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
+			cout << "Movement answer: " << msgSend->ToString() << endl;
-			robotStarted = 1;
+			WriteInQueue(&q_messageToMon, msgSend);  // msgSend will be deleted by sendToMon
-			rt_mutex_release(&mutex_robotStarted);
+
+			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);
+			}
 		}
 	}
 }
 
-
+/**
+ * @brief Thread seding the battery level.
+ */
 [[noreturn]] void Tasks::SendBatteryLevel(void *arg) {
 	int rs;
 	cout << "Start " << __PRETTY_FUNCTION__ << endl;
@@ -487,9 +488,7 @@ void Tasks::Join() {
 			}
 			if (counter == 3) {
 				rt_sem_v(&sem_stopRobot);
-				rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
+				WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_COM_ERROR));
-				monitor.Write(new Message(MESSAGE_ANSWER_COM_ERROR));
-				rt_mutex_release(&mutex_monitor);
 			}
 			rt_mutex_release(&mutex_robot);
 		}

software/raspberry/superviseur-robot/tasks.h

@@ -70,6 +70,7 @@ private:
     ComRobot robot;
     int robotStarted = 0;
     int move = MESSAGE_ROBOT_STOP;
+    bool monitorConnected = false;
     WatchdogMode watchdogMode = WITHOUT_WATCHDOG;
 
     /* *********************************************************************
@@ -78,15 +79,15 @@ private:
     RT_TASK th_server;
     RT_TASK th_sendToMon;
     RT_TASK th_receiveFromMon;
-    RT_TASK th_openComRobot;
-    RT_TASK th_startRobot;
     RT_TASK th_move;
     RT_TASK th_sendBatteryLevel;
+    RT_TASK th_manageRobot;
 
     /* *********************************************************************
      * Mutex
      * ********************************************************************/
     RT_MUTEX mutex_monitor;
+    RT_MUTEX mutex_monitorConnected;
     RT_MUTEX mutex_robot;
     RT_MUTEX mutex_robotStarted;
     RT_MUTEX mutex_move;
@@ -96,11 +97,9 @@ private:
      * Semaphores
      * ********************************************************************/
     RT_SEM sem_barrier;
-    RT_SEM sem_openComRobot;
     RT_SEM sem_serverOk;
     RT_SEM sem_startRobot;
 	RT_SEM sem_stopRobot;
-	RT_SEM sem_stopComRobot;
 	RT_SEM sem_stopServer;
 
     /* *********************************************************************
@@ -136,7 +135,7 @@ private:
 	/**
 	 * @brief Thread starting the communication with the robot.
 	 */
-	[[noreturn]] void StartRobotTask(void *arg);
+	[[noreturn]] void ManageRobotTask(void *arg);
 
 	/**
 	 * @brief Thread handling control of the robot.
@@ -166,7 +165,6 @@ private:
      * @return Message read
      */
     static Message *ReadInQueue(RT_QUEUE *queue);
-
 };