INSA/dumber
1
0
Fork 0
mirror of https://github.com/yoboujon/dumber.git synced 2025-06-08 13:50:49 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Fixed mutex issues with arena. Fully working arena.

Browse source
This commit is contained in:
Robin Marin-Muller 2024-05-15 17:32:53 +02:00
parent fc2a05ab13
commit 5b9656c67a
2 changed files with 42 additions and 59 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -24,6 +24,7 @@
#define PRIORITY_TMOVE 20 #define PRIORITY_TMOVE 20
#define PRIORITY_TSENDTOMON 22 #define PRIORITY_TSENDTOMON 22
#define PRIORITY_TRECEIVEFROMMON 25 #define PRIORITY_TRECEIVEFROMMON 25
#define PRIORITY_TSTARTROBOT 20
// Added Priorities // Added Priorities
// Camera is the most important as it has the most demanding timing request (10ms) // Camera is the most important as it has the most demanding timing request (10ms)
@ -368,7 +369,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_release(&mutex_cameraStatus); rt_mutex_release(&mutex_cameraStatus);
// Calling manageCamera, unblocking the thread. // Calling manageCamera, unblocking the thread.
rt_sem_v(&sem_manageCamera, TM_INFINITE); rt_sem_v(&sem_manageCamera);
} }
else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) { else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) {
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
@ -377,7 +378,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_release(&mutex_cameraStatus); rt_mutex_release(&mutex_cameraStatus);
// Calling manageCamera, unblocking the thread. // Calling manageCamera, unblocking the thread.
rt_sem_v(&sem_manageCamera, TM_INFINITE); rt_sem_v(&sem_manageCamera);
} }
// Arena // Arena
else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) { else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
@ -387,31 +388,25 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_release(&mutex_arenaStatus); rt_mutex_release(&mutex_arenaStatus);
// Calling arenaChoice, unblocking the thread. // Calling arenaChoice, unblocking the thread.
rt_sem_v(&sem_arenaChoice, TM_INFINITE); rt_sem_v(&sem_arenaChoice);
} }
else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) { else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
// Debug purpose only
std::cout << "\n\n\nArena Confirm asked !!!\n\n\n" << std::endl;
rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE); rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
if(ArenaStatusEnum::SEARCHED == arenaStatus) if(ArenaStatusEnum::SEARCHED == arenaStatus)
arenaStatus = ArenaStatusEnum::CONFIRM; arenaStatus = ArenaStatusEnum::CONFIRM;
rt_mutex_release(&mutex_arenaStatus); rt_mutex_release(&mutex_arenaStatus);
// Calling arenaChoice, unblocking the thread. // Calling arenaChoice, unblocking the thread.
rt_sem_v(&sem_arenaChoice, TM_INFINITE); rt_sem_v(&sem_arenaChoice);
} }
else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) { else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) {
// Debug purpose only
std::cout << "\n\n\nArena Infirm asked !!!\n\n\n" << std::endl;
rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE); rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
if(ArenaStatusEnum::SEARCHED == arenaStatus) if(ArenaStatusEnum::SEARCHED == arenaStatus)
arenaStatus = ArenaStatusEnum::INFIRM; arenaStatus = ArenaStatusEnum::INFIRM;
rt_mutex_release(&mutex_arenaStatus); rt_mutex_release(&mutex_arenaStatus);
// Calling arenaChoice, unblocking the thread. // Calling arenaChoice, unblocking the thread.
rt_sem_v(&sem_arenaChoice, TM_INFINITE); rt_sem_v(&sem_arenaChoice);
} }
else if (msgRcv->CompareID(MESSAGE_CAM_IMAGE)) { else if (msgRcv->CompareID(MESSAGE_CAM_IMAGE)) {
//? //?
@ -614,37 +609,32 @@ void Tasks::ManageCameraTask(void * arg)
// Called when semaphore arenChoice incremented // Called when semaphore arenChoice incremented
rt_sem_p(&sem_manageCamera, TM_INFINITE); rt_sem_p(&sem_manageCamera, TM_INFINITE);
// Verify that the robot has started // Check the status of the camera and store it in a local variable
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
rs = robotStarted; cs = cameraStatus;
rt_mutex_release(&mutex_robotStarted); rt_mutex_release(&mutex_cameraStatus);
// Maybe will be deleted -> Camera could be started without the robot // Opening the camera
if(rs != 0) if(CameraStatusEnum::OPENING == cs)
{ {
// Check the status of the camera and store it in a local variable const MessageID tempMessage = this->OpenCamera();
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); WriteInQueue(&q_messageToMon, new Message(tempMessage));
cs = cameraStatus; }
rt_mutex_release(&mutex_cameraStatus); // Closing the camera
if(CameraStatusEnum::CLOSING == cs)
// Opening the camera {
if(CameraStatusEnum::OPENING == cs) this->CloseCamera();
{ WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
const MessageID tempMessage = this->OpenCamera();
WriteInQueue(&q_messageToMon, new Message(tempMessage));
}
// Closing the camera
if(CameraStatusEnum::CLOSING == cs)
{
this->CloseCamera();
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
}
} }
} }
} }
void Tasks::ImageCameraTask(void * arg) void Tasks::ImageCameraTask(void * arg)
{ {
// Variables
CameraStatusEnum cs = CameraStatusEnum::CLOSED;
Arena a = Arena();
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting) // Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
@ -661,6 +651,11 @@ void Tasks::ImageCameraTask(void * arg)
const CameraStatusEnum cs = cameraStatus; const CameraStatusEnum cs = cameraStatus;
rt_mutex_release(&mutex_cameraStatus); rt_mutex_release(&mutex_cameraStatus);
// Gathering arena
rt_mutex_acquire(&mutex_arena, TM_INFINITE);
a = arena;
rt_mutex_release(&mutex_arena);
// Only gather image if opened // Only gather image if opened
if(CameraStatusEnum::OPENED == cs) if(CameraStatusEnum::OPENED == cs)
{ {
@ -670,10 +665,8 @@ void Tasks::ImageCameraTask(void * arg)
rt_mutex_release(&mutex_camera); rt_mutex_release(&mutex_camera);
// If arena has been found, draw overlay // If arena has been found, draw overlay
rt_mutex_acquire(&mutex_arena, TM_INFINITE); if(!a.IsEmpty())
if(!arena.IsEmpty()) img->DrawArena(a);
img->DrawArena(arena);
rt_mutex_acquire(&mutex_arena, TM_INFINITE);
// Sending the image to the monitor // Sending the image to the monitor
MessageImg *msgImg = new MessageImg(MESSAGE_CAM_IMAGE, img); MessageImg *msgImg = new MessageImg(MESSAGE_CAM_IMAGE, img);
@ -698,6 +691,7 @@ void Tasks::ArenaChoiceTask(void * arg)
{ {
// Called when semaphore arenChoice incremented // Called when semaphore arenChoice incremented
rt_sem_p(&sem_arenaChoice, TM_INFINITE); rt_sem_p(&sem_arenaChoice, TM_INFINITE);
// Check the status of the arena // Check the status of the arena
rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE); rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
as = arenaStatus; as = arenaStatus;
@ -706,6 +700,11 @@ void Tasks::ArenaChoiceTask(void * arg)
// ASK_ARENA // ASK_ARENA
if(as == ArenaStatusEnum::SEARCHING) if(as == ArenaStatusEnum::SEARCHING)
{ {
// Change status for next command
rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
arenaStatus = ArenaStatusEnum::SEARCHED;
rt_mutex_release(&mutex_arenaStatus);
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
const CameraStatusEnum cs = cameraStatus; const CameraStatusEnum cs = cameraStatus;
rt_mutex_release(&mutex_cameraStatus); rt_mutex_release(&mutex_cameraStatus);
@ -729,16 +728,9 @@ void Tasks::ArenaChoiceTask(void * arg)
} }
if(a.IsEmpty()) if(a.IsEmpty())
{
// Debug
std::cout << "\n\n\nArena is empty\n\n\n" << std::endl;
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
}
// If arena is not empty show the arena // If arena is not empty show the arena
else { else {
// Debug
std::cout << "\n\n\nArena detected\n\n\n" << std::endl;
// Adding overlay on the image // Adding overlay on the image
img->DrawArena(a); img->DrawArena(a);
// Sending it to the writing queue // Sending it to the writing queue
@ -748,22 +740,14 @@ void Tasks::ArenaChoiceTask(void * arg)
rt_mutex_release(&mutex_monitor); rt_mutex_release(&mutex_monitor);
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
} }
// Finished searching for an arena
rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
arenaStatus = ArenaStatusEnum::SEARCHED;
rt_mutex_release(&mutex_arenaStatus);
} }
// ARENA_CONFIRM / INFIRM // ARENA_CONFIRM / INFIRM
else if(as == ArenaStatusEnum::CONFIRM || as == ArenaStatusEnum::INFIRM) if((ArenaStatusEnum::CONFIRM == as) || (ArenaStatusEnum::INFIRM == as))
{ {
// Store arena to internal buffer (only if confirmed) rt_mutex_acquire(&mutex_arena, TM_INFINITE);
if(as == ArenaStatusEnum::CONFIRM) arena = a;
{ rt_mutex_release(&mutex_arena);
rt_mutex_acquire(&mutex_arena, TM_INFINITE);
arena = a;
rt_mutex_release(&mutex_arena);
}
// empty the temporary arena // empty the temporary arena
a = Arena(); a = Arena();

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

@ -60,7 +60,6 @@ enum class ArenaStatusEnum {
SEARCHING, SEARCHING,
SEARCHED, SEARCHED,
CONFIRM, CONFIRM,
CONFIRMED,
INFIRM INFIRM
}; };