Fully working Camera closing

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

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)) {
             //?
         }
@@ -583,3 +598,36 @@ void Tasks::ImageCamera(void * arg)
         }
     }
 }
+
+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);
+        }
+    }
+}

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);
 
 
     /**********************************************************************/