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

Merge remote-tracking branch 'origin/cmake' into cmake

Browse source
This commit is contained in:
Yohan Simard 2021-03-12 13:50:58 +01:00
commit 2673651834
1 changed files with 217 additions and 216 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -136,7 +136,7 @@ void Tasks::Init() {
/* *************************************************************************************
* Message queues creation
* *************************************************************************************/
if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof(Message * ) * 50, Q_UNLIMITED, Q_FIFO)) < 0) {
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);
}
@ -151,11 +151,11 @@ void Tasks::Run() {
rt_task_set_priority(nullptr, T_LOPRIO);
int err;
if (err = rt_task_start(&th_server, (void(*)(void*)) & Tasks::ServerTask, this)) {
if (err = rt_task_start(&th_server, (void (*)(void *)) &Tasks::ServerTask, this)) {
cerr << "Error task start ServerTask: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
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;
exit(EXIT_FAILURE);
}
@ -218,7 +218,8 @@ void Tasks::ServerTask(void *arg) {
cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl;
if (status < 0) throw std::runtime_error {
if (status < 0)
throw std::runtime_error{
"Unable to start server on port " + std::to_string(SERVER_PORT)
};
monitor.AcceptClient(); // Wait the monitor client
@ -229,7 +230,7 @@ void Tasks::ServerTask(void *arg) {
/**
* @brief Thread sending data to monitor.
*/
[[noreturn]] void Tasks::SendToMonTask(void* arg) {
[[noreturn]] void Tasks::SendToMonTask(void *arg) {
Message *msg;
cout << "Start " << __PRETTY_FUNCTION__ << endl;
@ -272,7 +273,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
cout << "Rcv <= " << msgRcv->ToString() << endl;
if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
delete(msgRcv);
delete (msgRcv);
exit(-1);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
rt_sem_v(&sem_openComRobot);
@ -288,7 +289,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
move = msgRcv->GetID();
rt_mutex_release(&mutex_move);
}
delete(msgRcv); // mus be deleted manually, no consumer
delete (msgRcv); // mus be deleted manually, no consumer
}
}
@ -315,7 +316,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
cout << status;
cout << ")" << endl << flush;
Message * msgSend;
Message *msgSend;
if (status < 0) {
msgSend = new Message(MESSAGE_ANSWER_NACK);
} else {
@ -368,10 +369,10 @@ void Tasks::ReceiveFromMonTask(void *arg) {
/* The task sendBatteryLevel starts here */
/* *************************************************************************************/
rt_task_set_periodic(NULL, TM_NOW, 500000000); // 500 ms //TODO
rt_task_set_periodic(nullptr, TM_NOW, 500000000); // 500 ms //TODO
while (true) {
rt_task_wait_period(NULL);
rt_task_wait_period(nullptr);
cout << "Periodic battery level update" << endl;
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
@ -422,7 +423,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
cout << " move: " << cpMove;
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
robot.Write(new Message((MessageID)cpMove));
robot.Write(new Message((MessageID) cpMove));
rt_mutex_release(&mutex_robot);
}
cout << endl << flush;
@ -436,7 +437,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
*/
void Tasks::WriteInQueue(RT_QUEUE *queue, Message *msg) {
int err;
if ((err = rt_queue_write(queue, (const void *) &msg, sizeof ((const void *) &msg), Q_NORMAL)) < 0) {
if ((err = rt_queue_write(queue, (const void *) &msg, sizeof((const void *) &msg), Q_NORMAL)) < 0) {
cerr << "Write in queue failed: " << strerror(-err) << endl;
throw std::runtime_error{"Error in write in queue"};
}
@ -451,7 +452,7 @@ Message *Tasks::ReadInQueue(RT_QUEUE *queue) {
int err;
Message *msg;
if ((err = rt_queue_read(queue, &msg, sizeof ((void*) &msg), TM_INFINITE)) < 0) {
if ((err = rt_queue_read(queue, &msg, sizeof((void *) &msg), TM_INFINITE)) < 0) {
cout << "Read in queue failed: " << strerror(-err) << endl;
throw std::runtime_error{"Error in read in queue"};
}/** else {