Add missing code in previous commit

software/raspberry/superviseur-robot/tasks.cpp

@@ -26,6 +26,7 @@
 #define PRIORITY_TRECEIVEFROMMON 25
 #define PRIORITY_TSTARTROBOT 20
 #define PRIORITY_TCAMERA 21
+#define PRIORITY_TBATTERY 23
 
 /*
  * Some remarks:
@@ -356,6 +357,41 @@ void Tasks::ReceiveFromMonTask(void *arg) {
     }
 }
 
+
+[[noreturn]] void Tasks::SendBatteryLevel(void *arg) {
+	int rs;
+	cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
+	// Synchronization barrier (waiting that all tasks are starting)
+	rt_sem_p(&sem_barrier, TM_INFINITE);
+
+	/* *************************************************************************************/
+	/* The task sendBatteryLevel starts here                                               */
+	/* *************************************************************************************/
+
+	rt_task_set_periodic(NULL, TM_NOW, 500000000); // 500 ms //TODO
+
+	while (true) {
+		rt_task_wait_period(NULL);
+		cout << "Periodic battery level update" << endl;
+
+		rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
+		rs = robotStarted;
+		rt_mutex_release(&mutex_robotStarted);
+		if (rs == 1) {
+			rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+			auto battRequest = new Message(MESSAGE_ROBOT_BATTERY_GET);
+			auto msg = dynamic_cast<MessageBattery *>(robot.Write(battRequest));
+			if (msg == nullptr) {
+				cout << "NULLPTR" << endl;
+			} else {
+				WriteInQueue(&q_messageToMon, msg);
+			}
+			rt_mutex_release(&mutex_robot);
+		}
+	}
+
+}
+
 /**
  * @brief Thread handling control of the robot.
  */

software/raspberry/superviseur-robot/tasks.h

@@ -76,6 +76,7 @@ private:
     RT_TASK th_openComRobot;
     RT_TASK th_startRobot;
     RT_TASK th_move;
+    RT_TASK th_sendBatteryLevel;
 
     /* *********************************************************************
      * Mutex