From c3555e26ba2cb569cee1dc7dae15c8954341c28a Mon Sep 17 00:00:00 2001
From: Arnaud Vergnet <vergnet@etud.insa-toulouse.fr>
Date: Fri, 12 Mar 2021 15:09:42 +0100
Subject: [PATCH] Add move watchdog

---
 .../raspberry/superviseur-robot/tasks.cpp     | 23 +++++++++++++++----
 software/raspberry/superviseur-robot/tasks.h  |  1 +
 2 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp
index af3633d..1b9b517 100644
--- a/software/raspberry/superviseur-robot/tasks.cpp
+++ b/software/raspberry/superviseur-robot/tasks.cpp
@@ -98,6 +98,10 @@ void Tasks::Init() {
 		cerr << "Error semaphore create sem_startRobot: " << strerror(-err) << endl;
 		exit(EXIT_FAILURE);
 	}
+	if ((err = rt_sem_create(&sem_stopRobot, nullptr, 0, S_FIFO))) {
+		cerr << "Error semaphore create sem_stopRobot: " << strerror(-err) << endl;
+		exit(EXIT_FAILURE);
+	}
 	cout << "Semaphores created successfully" << endl;
 
 	/* *************************************************************************************
@@ -400,6 +404,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
 [[noreturn]] void Tasks::MoveTask(void *arg) {
 	int rs;
 	int cpMove;
+	int counter = 0;
 
 	cout << "Start " << __PRETTY_FUNCTION__ << endl;
 	// Synchronization barrier (waiting that all tasks are starting)
@@ -412,7 +417,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
 
 	while (true) {
 		rt_task_wait_period(nullptr);
-		cout << "Periodic movement update";
+		cout << "Periodic movement update" << endl;
 		rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
 		rs = robotStarted;
 		rt_mutex_release(&mutex_robotStarted);
@@ -421,13 +426,23 @@ void Tasks::ReceiveFromMonTask(void *arg) {
 			cpMove = move;
 			rt_mutex_release(&mutex_move);
 
-			cout << " move: " << cpMove;
+			cout << "move: " << cpMove << endl;
 
 			rt_mutex_acquire(&mutex_robot, TM_INFINITE);
-			robot.Write(new Message((MessageID) cpMove));
+			auto msgSend = robot.Write(new Message((MessageID) cpMove));
+			if (msgSend->CompareID(MESSAGE_ANSWER_ACK)) {
+				counter = 0;
+			} else {
+				counter ++;
+			}
+			if (counter == 3) {
+				rt_sem_v(&sem_stopRobot);
+				rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
+				monitor.Write(new Message(MESSAGE_ANSWER_COM_ERROR));
+				rt_mutex_release(&mutex_monitor);
+			}
 			rt_mutex_release(&mutex_robot);
 		}
-		cout << endl;
 	}
 }
 
diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h
index 6ba34df..7b793c5 100644
--- a/software/raspberry/superviseur-robot/tasks.h
+++ b/software/raspberry/superviseur-robot/tasks.h
@@ -91,6 +91,7 @@ private:
     RT_SEM sem_openComRobot;
     RT_SEM sem_serverOk;
     RT_SEM sem_startRobot;
+	RT_SEM sem_stopRobot;
 
     /* *********************************************************************
      * Message queues