INSA/dumber
1
0
Fork 0
mirror of https://github.com/yoboujon/dumber.git synced 2025-06-08 05:40: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_TSENDTOMON 22
#define PRIORITY_TRECEIVEFROMMON 25
#define PRIORITY_TSTARTROBOT 20
// Added Priorities
// 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);
// Calling manageCamera, unblocking the thread.
rt_sem_v(&sem_manageCamera, TM_INFINITE);
rt_sem_v(&sem_manageCamera);
}
else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) {
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
@ -377,7 +378,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_release(&mutex_cameraStatus);
// Calling manageCamera, unblocking the thread.
rt_sem_v(&sem_manageCamera, TM_INFINITE);
rt_sem_v(&sem_manageCamera);
}
// Arena
else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
@ -387,31 +388,25 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_release(&mutex_arenaStatus);
// 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)) {
// Debug purpose only
std::cout << "\n\n\nArena Confirm asked !!!\n\n\n" << std::endl;
rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
if(ArenaStatusEnum::SEARCHED == arenaStatus)
arenaStatus = ArenaStatusEnum::CONFIRM;
rt_mutex_release(&mutex_arenaStatus);
// 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)) {
// Debug purpose only
std::cout << "\n\n\nArena Infirm asked !!!\n\n\n" << std::endl;
rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
if(ArenaStatusEnum::SEARCHED == arenaStatus)
arenaStatus = ArenaStatusEnum::INFIRM;
rt_mutex_release(&mutex_arenaStatus);
// Calling arenaChoice, unblocking the thread.
rt_sem_v(&sem_arenaChoice, TM_INFINITE);
rt_sem_v(&sem_arenaChoice);
}
else if (msgRcv->CompareID(MESSAGE_CAM_IMAGE)) {
//?
@ -614,37 +609,32 @@ void Tasks::ManageCameraTask(void * arg)
// Called when semaphore arenChoice incremented
rt_sem_p(&sem_manageCamera, TM_INFINITE);
// Verify that the robot has started
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
rs = robotStarted;
rt_mutex_release(&mutex_robotStarted);
// Maybe will be deleted -> Camera could be started without the robot
if(rs != 0)
// Check the status of the camera and store it in a local variable
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
cs = cameraStatus;
rt_mutex_release(&mutex_cameraStatus);
// Opening the camera
if(CameraStatusEnum::OPENING == cs)
{
// Check the status of the camera and store it in a local variable
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
cs = cameraStatus;
rt_mutex_release(&mutex_cameraStatus);
// Opening the camera
if(CameraStatusEnum::OPENING == cs)
{
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));
}
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)
{
// Variables
CameraStatusEnum cs = CameraStatusEnum::CLOSED;
Arena a = Arena();
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
@ -661,6 +651,11 @@ void Tasks::ImageCameraTask(void * arg)
const CameraStatusEnum cs = 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
if(CameraStatusEnum::OPENED == cs)
{
@ -670,10 +665,8 @@ void Tasks::ImageCameraTask(void * arg)
rt_mutex_release(&mutex_camera);
// If arena has been found, draw overlay
rt_mutex_acquire(&mutex_arena, TM_INFINITE);
if(!arena.IsEmpty())
img->DrawArena(arena);
rt_mutex_acquire(&mutex_arena, TM_INFINITE);
if(!a.IsEmpty())
img->DrawArena(a);
// Sending the image to the monitor
MessageImg *msgImg = new MessageImg(MESSAGE_CAM_IMAGE, img);
@ -698,6 +691,7 @@ void Tasks::ArenaChoiceTask(void * arg)
{
// Called when semaphore arenChoice incremented
rt_sem_p(&sem_arenaChoice, TM_INFINITE);
// Check the status of the arena
rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
as = arenaStatus;
@ -706,6 +700,11 @@ void Tasks::ArenaChoiceTask(void * arg)
// ASK_ARENA
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);
const CameraStatusEnum cs = cameraStatus;
rt_mutex_release(&mutex_cameraStatus);
@ -729,16 +728,9 @@ void Tasks::ArenaChoiceTask(void * arg)
}
if(a.IsEmpty())
{
// Debug
std::cout << "\n\n\nArena is empty\n\n\n" << std::endl;
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
}
// If arena is not empty show the arena
else {
// Debug
std::cout << "\n\n\nArena detected\n\n\n" << std::endl;
// Adding overlay on the image
img->DrawArena(a);
// Sending it to the writing queue
@ -748,22 +740,14 @@ void Tasks::ArenaChoiceTask(void * arg)
rt_mutex_release(&mutex_monitor);
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
else if(as == ArenaStatusEnum::CONFIRM || as == ArenaStatusEnum::INFIRM)
if((ArenaStatusEnum::CONFIRM == as) || (ArenaStatusEnum::INFIRM == as))
{
// Store arena to internal buffer (only if confirmed)
if(as == ArenaStatusEnum::CONFIRM)
{
rt_mutex_acquire(&mutex_arena, TM_INFINITE);
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
a = Arena();

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

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