diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 72278f5..1244172 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/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(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. */ diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index 1ac61d0..e62dac4 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -76,7 +76,8 @@ private: RT_TASK th_openComRobot; RT_TASK th_startRobot; RT_TASK th_move; - + RT_TASK th_sendBatteryLevel; + /* ********************************************************************* * Mutex * ********************************************************************/