"en fait la doc avait tord il fallait mettre ça"

This commit is contained in:
Raphaël LACROIX 2023-04-03 14:19:41 +02:00
父節點 e665159ce7
當前提交 6da7c4091b
共有 2 個文件被更改,包括 79 次插入3 次删除

查看文件

@ -54,6 +54,7 @@
* semaphore, etc.)
*/
void Tasks::robotLossCounter(){
int status;
// wait for connexion establishement
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
@ -152,7 +153,7 @@ void Tasks::Init() {
cout << "Mutexes created successfully" << endl << flush;
/**************************************************************************************/
/* Semaphors creation */
/* Semaphores creation */
/**************************************************************************************/
if (err = rt_sem_create(&sem_barrier, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
@ -174,6 +175,10 @@ void Tasks::Init() {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_startRobotWithWatchdog, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_startCamera, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
@ -382,6 +387,8 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_sem_v(&sem_openComRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
rt_sem_v(&sem_startRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
rt_sem_v(&sem_startRobotWithWatchdog);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
@ -451,7 +458,7 @@ void Tasks::StartRobotTask(void *arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
/**************************************************************************************/
/* The task startRobot starts here */
/**************************************************************************************/
@ -477,6 +484,69 @@ void Tasks::StartRobotTask(void *arg) {
}
}
/**
* @brief Thread starting the communication with the robot.
*/
// TODO : refactor with the previous : if (watchdog) {call with; start the watchdog messages tasks} else {call without}
void Tasks::StartRobotWithWatchDogTask(void *arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
/**************************************************************************************/
/* The task startRobot starts here */
/**************************************************************************************/
while (1) {
Message * msgSend;
rt_sem_p(&sem_startRobot, TM_INFINITE);
cout << "Start robot without watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
msgSend = call(robot.Write(robot.StartWithWD()));
rt_mutex_release(&mutex_robot);
cout << msgSend->GetID();
cout << ")" << endl;
cout << "Movement answer: " << msgSend->ToString() << endl << flush;
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
robotStarted = 1;
rt_mutex_release(&mutex_robotStarted);
}
rt_sem_v(&sem_reloadMessages, TM_INFINITE);
}
}
/**
* @brief Thread sending a message every 500 ms to the robot to prevent it from turning off.
*/
void Tasks::RobotReloadMessage(void *arg) {
int status;
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
rt_sem_p(&sem_reloadMessages);
cout << "Starting to monitor packet losses " << endl << flush;
rt_task_set_periodic(NULL, TM_NOW, 500*1000*1000);
while(1) {
rt_task_wait_period(NULL);
status = (robot.Write(robot.reloadWD()));
// send info to monitor
if (state < 0) {
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
} else {
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
}
}
}
/**
* @brief Thread starting the communication with the robot.
*/

查看文件

@ -105,6 +105,7 @@ private:
RT_SEM sem_openComRobot;
RT_SEM sem_serverOk;
RT_SEM sem_startRobot;
RT_SEM sem_startRobotWithWatchdog;
RT_SEM sem_startCamera;
RT_SEM sem_connexionEstablished;
@ -141,9 +142,14 @@ private:
* @brief Thread starting the communication with the robot.
*/
void StartRobotTask(void *arg);
/**
* @brief Thread starting the communication with the robot with a watchdog.
*/
void StartRobotWithWatchDogTask(void *arg);
/**
* @brief Thread starting the communication with the robot.
* @brief Thread starting the communication with the Camera.
*/
void StartCameraTask(void *arg);