From b7fae99df43a0a2a3a2fc31a233e2302cd62de0e Mon Sep 17 00:00:00 2001 From: Robin Marin-Muller Date: Thu, 4 Apr 2024 17:27:28 +0200 Subject: [PATCH] Fully working Camera closing --- .../timestamps-10.105.0.140-xenomai-22 | 6 +-- .../raspberry/superviseur-robot/tasks.cpp | 52 ++++++++++++++++++- software/raspberry/superviseur-robot/tasks.h | 5 +- 3 files changed, 57 insertions(+), 6 deletions(-) diff --git a/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.0.140-xenomai-22 b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.0.140-xenomai-22 index 053d279..ce021a0 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.0.140-xenomai-22 +++ b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.0.140-xenomai-22 @@ -1,4 +1,4 @@ -#Thu Apr 04 17:02:01 CEST 2024 +#Thu Apr 04 17:21:41 CEST 2024 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/main.cpp=c1712238648769 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1712238648773 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1712238648790 @@ -8,13 +8,13 @@ /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1712238648775 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/lib/.gitignore=c1712238648721 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1712238648782 -/home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/tasks.h=c1712238648836 +/home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/tasks.h=c1712243914675 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/README.md=c1712238648637 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/project.xml=c1712238648826 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1712238648778 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/superviseur.doxygen=c1712238648829 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/lib/base64/README.md=c1712238648730 -/home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1712242917288 +/home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1712244064711 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1712238648753 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/.dep.inc=c1712238648629 /home/marin-muller/Documents/4A/Robot/dumber/software/raspberry/superviseur-robot/lib/img.h=c1712238648760 diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index ddfb3f2..f904a7c 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -25,8 +25,9 @@ #define PRIORITY_TSENDTOMON 22 #define PRIORITY_TRECEIVEFROMMON 25 #define PRIORITY_TSTARTROBOT 20 -#define PRIORITY_TCAMERA 18 +#define PRIORITY_TCAMERA 21 #define PRIORITY_TBATTERY 19 +#define PRIORITY_TSETCAMERA 18 /* * Some remarks: @@ -142,7 +143,7 @@ void Tasks::Init() { cerr << "Error task create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } - if (err = rt_task_create(&th_cameraOpen, "th_cameraOpen", 0, PRIORITY_TCAMERA, 0)) { + if (err = rt_task_create(&th_cameraOpen, "th_cameraOpen", 0, PRIORITY_TSETCAMERA, 0)) { cerr << "Error task create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } @@ -150,6 +151,10 @@ void Tasks::Init() { cerr << "Error task create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } + if (err = rt_task_create(&th_cameraClose, "th_cameraClose", 0, PRIORITY_TSETCAMERA, 0)) { + cerr << "Error task create: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } cout << "Tasks created successfully" << endl << flush; /**************************************************************************************/ @@ -207,6 +212,10 @@ void Tasks::Run() { cerr << "Error task start: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } + if (err = rt_task_start(&th_cameraClose, (void(*)(void*)) & Tasks::CloseCamera, this)) { + cerr << "Error task start: " << strerror(-err) << endl << flush; + exit(EXIT_FAILURE); + } cout << "Tasks launched" << endl << flush; } @@ -328,6 +337,12 @@ void Tasks::ReceiveFromMonTask(void *arg) { cameraStatus = CameraStatusEnum::OPENING; rt_mutex_release(&mutex_cameraStatus); } + else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) { + rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); + if(cameraStatus == CameraStatusEnum::OPENED) + cameraStatus = CameraStatusEnum::CLOSING; + rt_mutex_release(&mutex_cameraStatus); + } else if (msgRcv->CompareID(MESSAGE_CAM_IMAGE)) { //? } @@ -582,4 +597,37 @@ void Tasks::ImageCamera(void * arg) rt_mutex_release(&mutex_monitor); } } +} + +void Tasks::CloseCamera(void * arg) +{ + // Variables + CameraStatusEnum cs(CameraStatusEnum::CLOSED); + + cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + // Synchronization barrier (waiting that all tasks are starting) + rt_sem_p(&sem_barrier, TM_INFINITE); + + rt_task_set_periodic(NULL, TM_NOW, 500000000); + + while(1) + { + rt_task_wait_period(NULL); + + // Check the status of the camera + rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); + cs = cameraStatus; + rt_mutex_release(&mutex_cameraStatus); + if(cs == CameraStatusEnum::CLOSING) + { + rt_mutex_acquire(&mutex_camera, TM_INFINITE); + cam->Close(); + delete cam; + rt_mutex_release(&mutex_camera); + rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); + cameraStatus = CameraStatusEnum::CLOSED; + std::cout << "Camera Closed" << std::endl; + rt_mutex_release(&mutex_cameraStatus); + } + } } \ No newline at end of file diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index 0c38b2b..dd4b700 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -39,7 +39,8 @@ using namespace std; enum class CameraStatusEnum { CLOSED, OPENING, - OPENED + OPENED, + CLOSING }; class Tasks { @@ -88,6 +89,7 @@ private: RT_TASK th_battery; RT_TASK th_cameraOpen; RT_TASK th_cameraImage; + RT_TASK th_cameraClose; /**********************************************************************/ /* Mutex */ @@ -153,6 +155,7 @@ private: void BatteryStatusTask(void * arg); void OpenCamera(void * arg); void ImageCamera(void * arg); + void CloseCamera(void * arg); /**********************************************************************/