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

Implement ReceiveFromMonTask

Browse source
This commit is contained in:
Yohan Simard 2021-03-12 15:30:47 +01:00
parent 53f4f7a176
commit 63257cb656
2 changed files with 62 additions and 13 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -21,14 +21,14 @@
using namespace std;
// Déclaration des priorités des taches
#define PRIORITY_TSERVER 30
#define PRIORITY_TOPENCOMROBOT 20
#define PRIORITY_TMOVE 20
#define PRIORITY_TSENDTOMON 22
#define PRIORITY_TRECEIVEFROMMON 25
#define PRIORITY_TSTARTROBOT 20
#define PRIORITY_TCAMERA 21
#define PRIORITY_TBATTERY 23
constexpr int PRIORITY_TSERVER = 30;
constexpr int PRIORITY_TOPENCOMROBOT = 20;
constexpr int PRIORITY_TMOVE = 20;
constexpr int PRIORITY_TSENDTOMON = 22;
constexpr int PRIORITY_TRECEIVEFROMMON = 25;
constexpr int PRIORITY_TSTARTROBOT = 20;
constexpr int PRIORITY_TCAMERA = 21;
constexpr int PRIORITY_TBATTERY = 23;
/*
* Some remarks:
@ -77,6 +77,10 @@ void Tasks::Init() {
cerr << "Error mutex create mutex_move: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
if ((err = rt_mutex_create(&mutex_watchdogMode, nullptr))) {
cerr << "Error mutex create mutex_watchdogMode: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
cout << "Mutexes created successfully" << endl;
/* **************************************************************************************
@ -102,6 +106,14 @@ void Tasks::Init() {
cerr << "Error semaphore create sem_stopRobot: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
if ((err = rt_sem_create(&sem_stopComRobot, nullptr, 0, S_FIFO))) {
cerr << "Error semaphore create sem_stopComRobot: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
if ((err = rt_sem_create(&sem_stopServer, nullptr, 0, S_FIFO))) {
cerr << "Error semaphore create sem_stopServer: " << strerror(-err) << endl;
exit(EXIT_FAILURE);
}
cout << "Semaphores created successfully" << endl;
/* *************************************************************************************
@ -261,7 +273,7 @@ void Tasks::ServerTask(void *arg) {
/**
* @brief Thread receiving data from monitor.
*/
void Tasks::ReceiveFromMonTask(void *arg) {
[[noreturn]] void Tasks::ReceiveFromMonTask(void *arg) {
Message *msgRcv;
cout << "Start " << __PRETTY_FUNCTION__ << endl;
@ -279,11 +291,23 @@ void Tasks::ReceiveFromMonTask(void *arg) {
cout << "Rcv <= " << msgRcv->ToString() << endl;
if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
delete (msgRcv);
exit(-1);
cout << "Monitor connection lost! Stopping robot..." << endl;
// rt_sem_v(&sem_stopCamera); // TODO
rt_sem_v(&sem_stopRobot);
rt_sem_v(&sem_stopComRobot);
rt_sem_v(&sem_stopServer);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
rt_sem_v(&sem_openComRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
watchdogMode = WITHOUT_WATCHDOG;
rt_mutex_release(&mutex_watchdogMode);
rt_sem_v(&sem_startRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
// TODO: gérer la watchdog
rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
watchdogMode = WITH_WATCHDOG;
rt_mutex_release(&mutex_watchdogMode);
rt_sem_v(&sem_startRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
@ -294,6 +318,20 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_acquire(&mutex_move, TM_INFINITE);
move = msgRcv->GetID();
rt_mutex_release(&mutex_move);
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
// TODO
} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) {
// TODO
}
delete (msgRcv); // mus be deleted manually, no consumer
}

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

@ -34,6 +34,12 @@
#include "camera.h"
#include "img.h"
enum WatchdogMode {
WITH_WATCHDOG,
WITHOUT_WATCHDOG,
};
class Tasks {
public:
/**
@ -64,7 +70,8 @@ private:
ComRobot robot;
int robotStarted = 0;
int move = MESSAGE_ROBOT_STOP;
WatchdogMode watchdogMode = WITHOUT_WATCHDOG;
/* *********************************************************************
* Tasks
* ********************************************************************/
@ -83,6 +90,7 @@ private:
RT_MUTEX mutex_robot;
RT_MUTEX mutex_robotStarted;
RT_MUTEX mutex_move;
RT_MUTEX mutex_watchdogMode;
/* *********************************************************************
* Semaphores
@ -92,6 +100,8 @@ private:
RT_SEM sem_serverOk;
RT_SEM sem_startRobot;
RT_SEM sem_stopRobot;
RT_SEM sem_stopComRobot;
RT_SEM sem_stopServer;
/* *********************************************************************
* Message queues
@ -116,7 +126,7 @@ private:
/**
* @brief Thread receiving data from monitor.
*/
void ReceiveFromMonTask(void *arg);
[[noreturn]] void ReceiveFromMonTask(void *arg);
/**
* @brief Thread opening communication with the robot.
@ -159,5 +169,6 @@ private:
};
#endif // __TASKS_H__