Template:Jetson Camera Drivers - Fixed Sensor ID for Argus Capture-Template2

diff --git a/gstnvarguscamerasrc.cpp b/gstnvarguscamerasrc.cpp
index 31d5c4b..e53a227 100644
--- a/gstnvarguscamerasrc.cpp
+++ b/gstnvarguscamerasrc.cpp
@@ -636,10 +636,15 @@ bool StreamConsumer::threadShutdown(GstNvArgusCameraSrc* src) { return true; }
 static bool execute(int32_t cameraIndex, int32_t cameraMode, const Size2D<uint32_t>& streamSize, int32_t secToRun,
                     GstNvArgusCameraSrc* src) {
   gfloat frameRate = 0, duration = 0;
+  uint64_t guid = 0;
+  uint64_t val = 0;
   uint32_t index = 0;
+  uint32_t i = 0, j = 0;
   uint32_t err = 0;
   gint found = 0;
   gint best_match = -1;
+  gint uuidNodeNumBytes = 6;
+  gint argusIndex = -1;
   Range<float> sensorModeAnalogGainRange;
   Range<float> ispDigitalGainRange;
   Range<uint64_t> limitExposureTimeRange;
@@ -650,6 +655,7 @@ static bool execute(int32_t cameraIndex, int32_t cameraMode, const Size2D<uint32
   ISensorMode* iSensorMode_ptr = NULL;
   IAutoControlSettings* iAutoControlSettings = NULL;
   ISourceSettings* requestSourceSettings = NULL;
+  UUID uuid = { 0 };
 
   // Domain for errors to bus
   static GQuark domain = g_quark_from_static_string("NvArgusCameraSrc");
@@ -687,17 +693,46 @@ static bool execute(int32_t cameraIndex, int32_t cameraMode, const Size2D<uint32
     ORIGINATE_ERROR("No cameras available");
   }
 
-  if (static_cast<uint32_t>(cameraIndex) >= cameraDevices.size()) {
+  /* Look for the camera device that has the given sensor-id as GUID.
+
+     The GUID is assigned based on the tegra-camera-platform modules definition
+     order and it's not affected if a camera is disconnected, which was the
+     case for the previous sensor-id implementation. */
+
+  for (i = 0; i < cameraDevices.size(); i++) {
+    camProps = interface_cast<ICameraProperties>(cameraDevices[i]);
+    if (!camProps) {
+      ORIGINATE_ERROR("Failed to get camera properties for camera %d", i);
+    }
+
+    uuid = camProps->getUUID();
+    /* In UUID, clock_seq contains low 16 bits of GUID and node[6] contains
+       the high 48 bits of GUID. */
+    guid = 0;
+    /* Fill the first 48 high bits */
+    for (j = 0; j < uuidNodeNumBytes; j++) {
+      val = uuid.node[j];
+      guid = guid | (val << (j * 8 + 16));
+    }
+    /* Fill the last 16 bits */
+    guid = guid | uuid.clock_seq;
+
+    if (guid == cameraIndex) {
+      argusIndex = i;
+      break;
+    }
+  }
+
+  if (argusIndex == -1) {
     error = g_error_new(domain, ERROR_INVALID_CAMERA_INDEX, "Invalid camera index");
     GstMessage* message = gst_message_new_error(GST_OBJECT(src), error, "Argus Error Status");
     gst_element_post_message(GST_ELEMENT_CAST(src), message);
-    ORIGINATE_ERROR("Invalid camera device specified %d specified, %d max index", cameraIndex,
-                    static_cast<int32_t>(cameraDevices.size()) - 1);
+    ORIGINATE_ERROR("No camera device found for index %d.", cameraIndex);
   }
 
   // Create the capture session using the specified device.
   UniqueObj<CaptureSession> captureSession = UniqueObj<CaptureSession>(
-      iCameraProvider->createCaptureSession(cameraDevices[cameraIndex]));
+      iCameraProvider->createCaptureSession(cameraDevices[argusIndex]));
   ICaptureSession* iCaptureSession = interface_cast<ICaptureSession>(captureSession);
   if (!iCaptureSession) {
     error = g_error_new(domain, ERROR_CAPTURE_SESSION, "Failed to create CaptureSession");
@@ -762,7 +797,7 @@ static bool execute(int32_t cameraIndex, int32_t cameraMode, const Size2D<uint32
     if (!iAutoControlSettings) ORIGINATE_ERROR("Failed to get AutoControlSettings");
     src->iAutoControlSettings_ptr = iAutoControlSettings;
 
-    camProps = interface_cast<ICameraProperties>(cameraDevices[cameraIndex]);
+    camProps = interface_cast<ICameraProperties>(cameraDevices[argusIndex]);
     if (!camProps) ORIGINATE_ERROR("Failed to create camera properties");
     camProps->getAllSensorModes(&modes);
 
@@ -842,11 +877,12 @@ static bool execute(int32_t cameraIndex, int32_t cameraMode, const Size2D<uint32
   GST_ARGUS_PRINT(
       "Running with following settings:\n"
       "   Camera index = %d \n"
+      "   Argus Camera index = %d \n"
       "   Camera mode  = %d \n"
       "   Output Stream W = %d H = %d \n"
       "   seconds to Run    = %d \n"
       "   Frame Rate = %f \n",
-      cameraIndex, cameraMode, iSensorMode_ptr->getResolution().width(), iSensorMode_ptr->getResolution().height(),
+      cameraIndex, argusIndex, cameraMode, iSensorMode_ptr->getResolution().width(), iSensorMode_ptr->getResolution().height(),
       secToRun, (1e9 / (iSensorMode_ptr->getFrameDurationRange().min())));
 
   IDenoiseSettings* denoiseSettings = interface_cast<IDenoiseSettings>(src->request);
@@ -1815,7 +1851,8 @@ static void gst_nv_argus_camera_src_class_init(GstNvArgusCameraSrcClass* klass)
 
   g_object_class_install_property(
       gobject_class, PROP_SENSOR_ID,
-      g_param_spec_int("sensor-id", "Sensor ID", "Set the id of camera sensor to use. Default 0.", 0, G_MAXUINT8,
+      g_param_spec_int("sensor-id", "Sensor ID", "Set the id of camera sensor to use according to the "
+                       "tegra-camera-platform modules definition order in the device tree. Default 0.", 0, G_MAXUINT8,
                        NVARGUSCAM_DEFAULT_SENSOR_ID, (GParamFlags)(G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
 
   g_object_class_install_property(