vergnet/very-dumb
1
1
Fork 0
Code Issues Pull requests Releases Wiki Activity

Add sendBatteryLevel task

Browse source
This commit is contained in:
Yohan Simard 2021-03-12 13:25:52 +01:00
parent a39a47bb97
commit 7f993019bc
2 changed files with 109 additions and 92 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

156
software/raspberry/superviseur-robot/tasks.cpp
View file

@ -106,34 +106,40 @@ void Tasks::Init() {
} }
if ((err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0))) { if ((err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0))) {
cerr << "Error task create th_sendToMon: " << strerror(-err) << endl; cerr << "Error task create th_sendToMon: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if ((err = rt_task_create(&th_receiveFromMon, "th_receiveFromMon", 0, PRIORITY_TRECEIVEFROMMON, 0))) { if ((err = rt_task_create(&th_receiveFromMon, "th_receiveFromMon", 0, PRIORITY_TRECEIVEFROMMON, 0))) {
cerr << "Error task create th_receiveFromMon: " << strerror(-err) << endl; cerr << "Error task create th_receiveFromMon: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if ((err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0))) { if ((err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0))) {
cerr << "Error task create th_openComRobot: " << strerror(-err) << endl; cerr << "Error task create th_openComRobot: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if ((err = rt_task_create(&th_startRobot, "th_startRobot", 0, PRIORITY_TSTARTROBOT, 0))) { if ((err = rt_task_create(&th_startRobot, "th_startRobot", 0, PRIORITY_TSTARTROBOT, 0))) {
cerr << "Error task create th_startRobot: " << strerror(-err) << endl; cerr << "Error task create th_startRobot: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if ((err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0))) { if ((err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0))) {
cerr << "Error task create th_move: " << strerror(-err) << endl; cerr << "Error task create th_move: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
cout << "Tasks created successfully" << endl;
/* ************************************************************************************* if ((err = rt_task_create(&th_sendBatteryLevel, "th_sendBatteryLevel", 0, PRIORITY_TBATTERY, 0))) {
* Message queues creation cerr << "Error task create th_sendBatteryLevel: " << strerror(-err) << endl;
* *************************************************************************************/ exit(EXIT_FAILURE);
if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof (Message*)*50, Q_UNLIMITED, Q_FIFO)) < 0) { }
cerr << "Error msg queue create q_messageToMon: " << strerror(-err) << endl;
exit(EXIT_FAILURE); cout << "Tasks created successfully" << endl;
}
cout << "Queues created successfully" << endl; /* *************************************************************************************
* Message queues creation
* *************************************************************************************/
if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof(Message * ) * 50, Q_UNLIMITED, Q_FIFO)) < 0) {
cerr << "Error msg queue create q_messageToMon: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
cout << "Queues created successfully" << endl;
} }
@ -150,26 +156,30 @@ void Tasks::Run() {
} }
if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) { if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) {
cerr << "Error task start SendToMonTask: " << strerror(-err) << endl; cerr << "Error task start SendToMonTask: " << strerror(-err) << endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_receiveFromMon, (void(*)(void*)) & Tasks::ReceiveFromMonTask, this)) {
cerr << "Error task start ReceiveFromMonTask: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) {
cerr << "Error task start OpenComRobot: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_startRobot, (void(*)(void*)) & Tasks::StartRobotTask, this)) {
cerr << "Error task start StartRobotTask: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_move, (void(*)(void*)) & Tasks::MoveTask, this)) {
cerr << "Error task start MoveTask: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
} }
if (err = rt_task_start(&th_receiveFromMon, (void (*)(void *)) &Tasks::ReceiveFromMonTask, this)) {
cerr << "Error task start ReceiveFromMonTask: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_openComRobot, (void (*)(void *)) &Tasks::OpenComRobot, this)) {
cerr << "Error task start OpenComRobot: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_startRobot, (void (*)(void *)) &Tasks::StartRobotTask, this)) {
cerr << "Error task start StartRobotTask: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_move, (void (*)(void *)) &Tasks::MoveTask, this)) {
cerr << "Error task start MoveTask: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_sendBatteryLevel, (void (*)(void *)) &Tasks::SendBatteryLevel, this)) {
cerr << "Error task start SendBatteryLevel: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
cout << "Tasks launched" << endl; cout << "Tasks launched" << endl;
} }
/** /**
@ -326,23 +336,23 @@ void Tasks::ReceiveFromMonTask(void *arg) {
* The task startRobot starts here * The task startRobot starts here
* *************************************************************************************/ * *************************************************************************************/
while (true) { while (true) {
Message * msgSend; Message *msgSend;
rt_sem_p(&sem_startRobot, TM_INFINITE); rt_sem_p(&sem_startRobot, TM_INFINITE);
cout << "Start robot without watchdog ("; cout << "Start robot without watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = robot.Write(robot.StartWithoutWD()); msgSend = robot.Write(ComRobot::StartWithoutWD());
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
cout << msgSend->GetID(); cout << msgSend->GetID();
cout << ")" << endl; cout << ")" << endl;
cout << "Movement answer: " << msgSend->ToString() << endl; cout << "Movement answer: " << msgSend->ToString() << endl;
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
robotStarted = 1; robotStarted = 1;
rt_mutex_release(&mutex_robotStarted); rt_mutex_release(&mutex_robotStarted);
} }
} }
} }
@ -350,21 +360,21 @@ void Tasks::ReceiveFromMonTask(void *arg) {
* @brief Thread handling control of the robot. * @brief Thread handling control of the robot.
*/ */
[[noreturn]] void Tasks::MoveTask(void *arg) { [[noreturn]] void Tasks::MoveTask(void *arg) {
int rs; int rs;
int cpMove; int cpMove;
cout << "Start " << __PRETTY_FUNCTION__ << endl;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
/* **************************************************************************************
* The task starts here
* *************************************************************************************/
rt_task_set_periodic(nullptr, TM_NOW, 100000000);
while (true) { cout << "Start " << __PRETTY_FUNCTION__ << endl;
rt_task_wait_period(nullptr); // Synchronization barrier (waiting that all tasks are starting)
cout << "Periodic movement update"; rt_sem_p(&sem_barrier, TM_INFINITE);
/* **************************************************************************************
* The task starts here
* *************************************************************************************/
rt_task_set_periodic(nullptr, TM_NOW, 100000000);
while (true) {
rt_task_wait_period(nullptr);
cout << "Periodic movement update";
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
rs = robotStarted; rs = robotStarted;
rt_mutex_release(&mutex_robotStarted); rt_mutex_release(&mutex_robotStarted);

45
software/raspberry/superviseur-robot/tasks.h
View file

@ -123,25 +123,32 @@ private:
*/ */
[[noreturn]] void OpenComRobot(void *arg); [[noreturn]] void OpenComRobot(void *arg);
/** /**
* @brief Thread starting the communication with the robot. * @brief Thread starting the communication with the robot.
*/ */
[[noreturn]] void StartRobotTask(void *arg); [[noreturn]] void StartRobotTask(void *arg);
/** /**
* @brief Thread handling control of the robot. * @brief Thread handling control of the robot.
*/ */
[[noreturn]] void MoveTask(void *arg); [[noreturn]] void MoveTask(void *arg);
/* ********************************************************************* /**
* Queue services * @brief Thread handling battery level sending.
* ********************************************************************/ */
/** [[noreturn]] void SendBatteryLevel(void *arg);
* Write a message in a given queue
* @param queue Queue identifier
* @param msg Message to be stored
*/ /* *********************************************************************
void WriteInQueue(RT_QUEUE *queue, Message *msg); * Queue services
* ********************************************************************/
/**
* Write a message in a given queue
* @param queue Queue identifier
* @param msg Message to be stored
*/
void WriteInQueue(RT_QUEUE *queue, Message *msg);
/** /**
* Read a message from a given queue, block if empty * Read a message from a given queue, block if empty