gentlsrc: add support for FLIR Spinnaker cameras

This also required changing how we read values, as FLIR uses little
endian. Timestamps are handled differently as well.

We're even more overdue for moving to properly parse the XML, just a
matter of if we'll use the GenApi reference implementation or something
else like Aravis.

sys/gentl/gstgentlsrc.c

@@ -64,6 +64,7 @@ initialize_evt_addresses (GstGenTlProducer * producer)
   producer->cti_path =
       g_strdup ("C:\\Program Files\\EVT\\eSDK\\bin\\EmergentGenTL.cti");
   producer->acquisition_mode_value = 0;
+  producer->timestamp_control_latch_value = 2;
   producer->width = 0xA000;
   producer->height = 0xA004;
   producer->pixel_format = 0xA008;
@@ -76,6 +77,7 @@ initialize_evt_addresses (GstGenTlProducer * producer)
   producer->timestamp_control_latch = 0x944;
   producer->timestamp_low = 0x094C;
   producer->timestamp_high = 0x0948;
+  producer->port_endianness = G_BIG_ENDIAN;
 }
 
 static void
@@ -93,8 +95,31 @@ initialize_basler_addresses (GstGenTlProducer * producer)
   producer->acquisition_mode = 0x40004;
   producer->acquisition_start = 0x40024;
   producer->acquisition_stop = 0x40044;
+  producer->port_endianness = G_BIG_ENDIAN;
 }
 
+static void
+initialize_flir_addresses (GstGenTlProducer * producer)
+{
+  memset (producer, 0, sizeof (producer));
+  producer->cti_path =
+      g_strdup
+      ("C:\\Program Files\\FLIR Systems\\Spinnaker\\cti64\\vs2015\\FLIR_GenTL_v140.cti");
+  producer->acquisition_mode_value = 0;
+  producer->timestamp_control_latch_value = 1;
+  producer->width = 0x00081084;
+  producer->height = 0x00081064;
+  producer->pixel_format = 0x00086008;
+  producer->payload_size = 0x20002008;
+  producer->acquisition_mode = 0x000C00C8;
+  producer->acquisition_start = 0x000C0004;
+  producer->acquisition_stop = 0x000C0024;
+  producer->timestamp_control_latch = 0x1F8;
+  producer->timestamp = 0x1F0;
+  producer->port_endianness = G_LITTLE_ENDIAN;
+}
+
+
 #define GST_TYPE_GENTLSRC_PRODUCER (gst_gentlsrc_producer_get_type())
 static GType
 gst_gentlsrc_producer_get_type (void)
@@ -103,6 +128,7 @@ gst_gentlsrc_producer_get_type (void)
   static const GEnumValue gentlsrc_producer[] = {
     {GST_GENTLSRC_PRODUCER_BASLER, "Basler producer", "basler"},
     {GST_GENTLSRC_PRODUCER_EVT, "EVT producer", "evt"},
+    {GST_GENTLSRC_PRODUCER_FLIR, "FLIR producer", "flir"},
     {0, NULL, NULL},
   };
 
@@ -135,6 +161,7 @@ static GstFlowReturn gst_gentlsrc_create (GstPushSrc * src, GstBuffer ** buf);
 
 static gchar *gst_gentlsrc_get_error_string (GstGenTlSrc * src);
 static void gst_gentlsrc_cleanup_tl (GstGenTlSrc * src);
+static gboolean gst_gentlsrc_src_latch_timestamps (GstGenTlSrc * src);
 
 enum
 {
@@ -775,6 +802,111 @@ gst_gentl_print_device_info (GstGenTlSrc * src, uint32_t index)
 //}
 
 
+static guint32
+read_uint32 (GstGenTlSrc * src, guint64 addr, GC_ERROR * ret)
+{
+  guint32 value;
+  size_t datasize = 4;
+
+  *ret = GTL_GCReadPort (src->hDevPort, addr, &value, &datasize);
+  if (*ret != GC_ERR_SUCCESS) {
+    GST_ELEMENT_ERROR (src, LIBRARY, FAILED,
+        ("Failed to read address: %s", gst_gentlsrc_get_error_string (src)),
+        (NULL));
+    goto error;
+  }
+
+  if (src->producer.port_endianness == G_BIG_ENDIAN)
+    value = GUINT32_FROM_BE (value);
+  else
+    value = GUINT32_FROM_LE (value);
+
+  return value;
+
+error:
+  return 0;
+}
+
+static gboolean
+write_uint32 (GstGenTlSrc * src, guint64 addr, guint32 value)
+{
+  GC_ERROR ret;
+  size_t datasize = 4;
+
+  if (src->producer.port_endianness == G_BIG_ENDIAN)
+    value = GUINT32_TO_BE (value);
+  else
+    value = GUINT32_TO_LE (value);
+
+  ret = GTL_GCWritePort (src->hDevPort, addr, &value, &datasize);
+  HANDLE_GTL_ERROR ("Failed to write address");
+
+  return ret;
+
+error:
+  return ret;
+}
+
+static guint64
+read_uint64_single (GstGenTlSrc * src, guint64 addr, GC_ERROR * ret)
+{
+  guint64 value;
+  size_t datasize = 8;
+
+  *ret = GTL_GCReadPort (src->hDevPort, addr, &value, &datasize);
+  if (*ret != GC_ERR_SUCCESS) {
+    GST_ELEMENT_ERROR (src, LIBRARY, FAILED,
+        ("Failed to read address: %s", gst_gentlsrc_get_error_string (src)),
+        (NULL));
+    goto error;
+  }
+
+  if (src->producer.port_endianness == G_BIG_ENDIAN)
+    value = GUINT64_FROM_BE (value);
+  else
+    value = GUINT64_FROM_LE (value);
+
+  return value;
+
+error:
+  return 0;
+}
+
+static guint64
+read_uint64 (GstGenTlSrc * src, guint64 low_addr, guint64 high_addr,
+    GC_ERROR * ret)
+{
+  guint32 low, high;
+  size_t datasize = 4;
+  guint64 value;
+
+  *ret = GTL_GCReadPort (src->hDevPort, low_addr, &low, &datasize);
+  if (*ret != GC_ERR_SUCCESS) {
+    GST_ELEMENT_ERROR (src, LIBRARY, FAILED,
+        ("Failed to read lower address: %s",
+            gst_gentlsrc_get_error_string (src)), (NULL));
+    goto error;
+  }
+  *ret = GTL_GCReadPort (src->hDevPort, high_addr, &high, &datasize);
+  if (*ret != GC_ERR_SUCCESS) {
+    GST_ELEMENT_ERROR (src, LIBRARY, FAILED,
+        ("Failed to read upper address: %s",
+            gst_gentlsrc_get_error_string (src)), (NULL));
+    goto error;
+  }
+
+  if (src->producer.port_endianness == G_BIG_ENDIAN)
+    value = GUINT64_FROM_BE ((guint64) low << 32 | high);
+  else
+    value = GUINT64_FROM_LE ((guint64) low << 32 | high);
+
+  return value;
+
+error:
+  return 0;
+}
+
+
 static size_t
 gst_gentlsrc_get_payload_size (GstGenTlSrc * src)
 {
@@ -798,13 +930,9 @@ gst_gentlsrc_get_payload_size (GstGenTlSrc * src)
         payload_size);
   } else {
     guint32 val = 0;
-    size_t datasize = 4;
     // TODO: use node map
-    ret =
+    payload_size = read_uint32 (src, src->producer.payload_size, &ret);
-        GTL_GCReadPort (src->hDevPort, src->producer.payload_size, &val,
-        &datasize);
     HANDLE_GTL_ERROR ("Failed to get payload size");
-    payload_size = GUINT32_FROM_BE (val);
     GST_DEBUG_OBJECT (src, "Payload size defined by node map: %d",
         payload_size);
 
@@ -860,66 +988,79 @@ gst_gentlsrc_get_gev_tick_frequency (GstGenTlSrc * src)
 {
   GC_ERROR ret;
 
-  if (!src->producer.tick_frequency_high || !src->producer.tick_frequency_low)
+  if (!src->producer.tick_frequency_high || !src->producer.tick_frequency_low) {
+    // latch timestamps once
+    if (gst_gentlsrc_src_latch_timestamps (src)) {
+      GST_DEBUG_OBJECT (src, "Assuming timestamps are in nanoseconds");
+      return GST_SECOND;
+    } else {
+      GST_ERROR_OBJECT (src, "Tick frequency addresses aren't defined");
       return 0;
+    }
+  }
 
-  guint32 freq_low, freq_high;
+  guint64 tick_frequency = read_uint64 (src, src->producer.tick_frequency_low,
-  size_t datasize = 4;
+      src->producer.tick_frequency_high, &ret);
-  ret = GTL_GCReadPort (src->hDevPort, src->producer.tick_frequency_low, &freq_low, &datasize); // GevTimestampTickFrequencyLow
-  HANDLE_GTL_ERROR ("Failed to get GevTimestampTickFrequencyLow");
-  ret = GTL_GCReadPort (src->hDevPort, src->producer.tick_frequency_high, &freq_high, &datasize);       // GevTimestampTickFrequencyHigh
-  HANDLE_GTL_ERROR ("Failed to get GevTimestampTickFrequencyHigh");
-
-  guint64 tick_frequency =
-      GUINT64_FROM_BE ((guint64) freq_low << 32 | freq_high);
   GST_DEBUG_OBJECT (src, "GEV Timestamp tick frequency is %llu",
       tick_frequency);
 
   return tick_frequency;
-
-error:
-  return 0;
 }
 
 static guint64
-gst_gentlsrc_get_gev_timestamp_ticks (GstGenTlSrc * src)
+gst_gentlsrc_get_gev_timestamp_ns (GstGenTlSrc * src)
 {
   GC_ERROR ret;
-  size_t datasize = 4;
+  guint64 timestamp_ns;
-  guint32 val, ts_low, ts_high;
 
-  val = GUINT32_TO_BE (2);
+  ret =
-  datasize = sizeof (val);
+      write_uint32 (src, src->producer.timestamp_control_latch,
-  ret = GTL_GCWritePort (src->hDevPort, src->producer.timestamp_control_latch, &val, &datasize);        // GevTimestampControlLatch
+      src->producer.timestamp_control_latch_value);
   HANDLE_GTL_WARNING ("Failed to latch timestamp GevTimestampControlLatch");
 
-  ret = GTL_GCReadPort (src->hDevPort, src->producer.timestamp_low, &ts_low, &datasize);        // GevTimestampValueLow
+  if (src->producer.timestamp) {
-  HANDLE_GTL_WARNING ("Failed to get GevTimestampValueLow");
+    timestamp_ns = read_uint64_single (src, src->producer.timestamp, &ret);
-  ret = GTL_GCReadPort (src->hDevPort, src->producer.timestamp_high, &ts_high, &datasize);      // GevTimestampValueHigh
+    HANDLE_GTL_WARNING ("Failed to read device timestamp");
-  HANDLE_GTL_WARNING ("Failed to get GevTimestampValueHigh");
+  } else {
-  guint64 ticks = GUINT64_FROM_BE ((guint64) ts_low << 32 | ts_high);
+    guint64 ticks = read_uint64 (src, src->producer.timestamp_low,
+        src->producer.timestamp_high, &ret);
+    HANDLE_GTL_WARNING ("Failed to read timestamp ticks");
     GST_LOG_OBJECT (src, "Timestamp ticks are %llu", ticks);
 
-  return ticks;
+    if (src->tick_frequency == 0) {
+      GST_WARNING_OBJECT (src,
+          "Tick frequency undefined, can't timestamp accurately");
+      goto error;
+    }
+    timestamp_ns = ((guint64)
+        (ticks * ((double) GST_SECOND / src->tick_frequency)));;
+  }
+
+  GST_LOG_OBJECT (src, "Device timestamp in ns is %llu", timestamp_ns);
+
+  return timestamp_ns;
 
 error:
   return 0;
 }
 
-static void
+static gboolean
 gst_gentlsrc_src_latch_timestamps (GstGenTlSrc * src)
 {
   guint64 unix_ts, gev_ts;
 
   unix_ts = get_unix_ns ();
-  gev_ts = gst_gentlsrc_get_gev_timestamp_ticks (src);
+  gev_ts = gst_gentlsrc_get_gev_timestamp_ns (src);
 
   if (gev_ts != 0) {
     src->unix_latched_ns = unix_ts;
-    src->gentl_latched_ns = ((gint64)
+    src->gentl_latched_ns = gev_ts;
-      (gev_ts * ((double)GST_SECOND / src->tick_frequency)));;
+    GST_LOG_OBJECT (src, "Latched system time: %llu", src->unix_latched_ns);
+    GST_LOG_OBJECT (src, "Latched GenTL time : %llu", src->gentl_latched_ns);
+    return TRUE;
   } else {
     GST_WARNING_OBJECT (src, "Failed to latch GEV time, using old latch value");
+    return FALSE;
   }
 }
 
@@ -928,8 +1069,6 @@ gst_gentlsrc_set_attributes (GstGenTlSrc * src)
 {
   gchar **pairs;
   int i;
-  guint32 val;
-  size_t datasize;
   GC_ERROR ret;
 
   if (!src->attributes || src->attributes == 0) {
@@ -956,11 +1095,7 @@ gst_gentlsrc_set_attributes (GstGenTlSrc * src)
 
     GST_DEBUG_OBJECT (src, "Setting attribute, '%s'='%s'", pair[0], pair[1]);
 
-    val = GUINT32_TO_BE (atoi (pair[1]));
+    ret = write_uint32 (src, strtol (pair[0], NULL, 16), atoi (pair[1]));
-    datasize = sizeof (val);
-    ret =
-        GTL_GCWritePort (src->hDevPort, strtol (pair[0], NULL, 16), &val,
-        &datasize);
     if (ret != GC_ERR_SUCCESS) {
       GST_WARNING_OBJECT (src, "Failed to set attribute: %s",
           gst_gentlsrc_get_error_string (src));
@@ -1067,8 +1202,7 @@ gst_gentlsrc_start (GstBaseSrc * bsrc)
   GstGenTlSrcClass *klass = GST_GENTL_SRC_GET_CLASS (src);
   GC_ERROR ret;
   uint32_t i, num_devs;
-  guint32 width, height, stride;
+  guint32 width, height;
-  GstVideoInfo vinfo;
 
   GST_DEBUG_OBJECT (src, "start");
 
@@ -1076,6 +1210,8 @@ gst_gentlsrc_start (GstBaseSrc * bsrc)
     initialize_basler_addresses (&src->producer);
   } else if (src->producer_prop == GST_GENTLSRC_PRODUCER_EVT) {
     initialize_evt_addresses (&src->producer);
+  } else if (src->producer_prop == GST_GENTLSRC_PRODUCER_FLIR) {
+    initialize_flir_addresses (&src->producer);
   } else {
     g_assert_not_reached ();
   }
@@ -1309,28 +1445,19 @@ gst_gentlsrc_start (GstBaseSrc * bsrc)
     }
   }
 
-  src->tick_frequency = gst_gentlsrc_get_gev_tick_frequency (src);
-
   gst_gentlsrc_set_attributes (src);
 
   {
     // TODO: use GenTl node map for this
-    guint32 val = 0;
+    width = read_uint32 (src, src->producer.width, &ret);
-    size_t datasize = 4;
-    ret = GTL_GCReadPort (src->hDevPort, src->producer.width, &val, &datasize);
     HANDLE_GTL_ERROR ("Failed to get width");
-    width = GUINT32_FROM_BE (val);
+    height = read_uint32 (src, src->producer.height, &ret);
-    ret = GTL_GCReadPort (src->hDevPort, src->producer.height, &val, &datasize);
     HANDLE_GTL_ERROR ("Failed to get height");
-    height = GUINT32_FROM_BE (val);
     GST_DEBUG_OBJECT (src, "Width and height %dx%d", width, height);
 
-    ret =
+    guint32 pixfmt_enum = read_uint32 (src, src->producer.pixel_format, &ret);
-        GTL_GCReadPort (src->hDevPort, src->producer.pixel_format, &val,
+    HANDLE_GTL_ERROR ("Failed to get pixel format");
-        &datasize);
-    HANDLE_GTL_ERROR ("Failed to get height");
     const char *genicam_pixfmt;
-    guint32 pixfmt_enum = GUINT32_FROM_BE (val);
     switch (pixfmt_enum) {
       case 0x1:                // Basler Ace
       case 0x01080001:
@@ -1407,25 +1534,17 @@ gst_gentlsrc_start (GstBaseSrc * bsrc)
 
   {
     // TODO: use GenTl node map for this
-    guint32 val;
-    size_t datasize;
 
     /* set AcquisitionMode to Continuous */
     // TODO: "Continuous" value can have different integer values, we need
     // to look it up in the node map (EVT is 0, Basler is 2)
-    val = GUINT32_TO_BE (src->producer.acquisition_mode_value);
-    datasize = sizeof (val);
     ret =
-        GTL_GCWritePort (src->hDevPort, src->producer.acquisition_mode, &val,
+        write_uint32 (src, src->producer.acquisition_mode,
-        &datasize);
+        src->producer.acquisition_mode_value);
     HANDLE_GTL_ERROR ("Failed to start device acquisition");
 
     /* send AcquisitionStart command */
-    val = GUINT32_TO_BE (1);
+    ret = write_uint32 (src, src->producer.acquisition_start, 1);
-    datasize = sizeof (val);
-    ret =
-        GTL_GCWritePort (src->hDevPort, src->producer.acquisition_start, &val,
-        &datasize);
     HANDLE_GTL_ERROR ("Failed to start device acquisition");
   }
 
@@ -1436,6 +1555,8 @@ gst_gentlsrc_start (GstBaseSrc * bsrc)
   src->acq_start_time =
       gst_clock_get_time (gst_element_get_clock (GST_ELEMENT (src)));
 
+  src->tick_frequency = gst_gentlsrc_get_gev_tick_frequency (src);
+
   return TRUE;
 
 error:
@@ -1490,11 +1611,8 @@ gst_gentlsrc_stop (GstBaseSrc * bsrc)
 
   if (src->hDS) {
     /* command AcquisitionStop */
-    guint32 val = GUINT32_TO_BE (1);
+    GC_ERROR ret;
-    gsize datasize = sizeof (val);
+    ret = write_uint32 (src, src->producer.acquisition_stop, 1);
-    GC_ERROR ret =
-        GTL_GCWritePort (src->hDevPort, src->producer.acquisition_stop, &val,
-        &datasize);
 
     GTL_DSStopAcquisition (src->hDS, ACQ_STOP_FLAGS_DEFAULT);
     GTL_DSFlushQueue (src->hDS, ACQ_QUEUE_INPUT_TO_OUTPUT);
@@ -1557,10 +1675,22 @@ gst_gentlsrc_set_caps (GstBaseSrc * bsrc, GstCaps * caps)
 
   GST_DEBUG_OBJECT (src, "The caps being set are %" GST_PTR_FORMAT, caps);
 
+
+  GST_ERROR ("Stride is %d", src->gst_stride);
   gst_video_info_from_caps (&vinfo, caps);
 
   if (GST_VIDEO_INFO_FORMAT (&vinfo) != GST_VIDEO_FORMAT_UNKNOWN) {
-    src->gst_stride = GST_VIDEO_INFO_COMP_STRIDE (&vinfo, 0);
+    int endianness;
+    const char *genicam_pixfmt =
+        gst_genicam_pixel_format_from_caps (caps, &endianness);
+    GST_ERROR ("Format is %s, Stride is %d", genicam_pixfmt, src->gst_stride);
+    if (genicam_pixfmt)
+      src->gst_stride =
+          gst_genicam_pixel_format_get_stride (genicam_pixfmt, G_LITTLE_ENDIAN,
+          vinfo.width);
+    else
+      goto unsupported_caps;
+    GST_ERROR ("Format is %s, Stride is %d", genicam_pixfmt, src->gst_stride);
   } else {
     goto unsupported_caps;
   }
@@ -1655,7 +1785,8 @@ gst_gentlsrc_get_buffer (GstGenTlSrc * src)
     HANDLE_GTL_ERROR ("Failed to get buffer timestamp");
     buf_timestamp_ns = (gint64)
         (buf_timestamp_ticks * ((double) GST_SECOND / src->tick_frequency));
-    GST_LOG_OBJECT(src, "Buffer GentTL timestamp: %llu ticks, %llu ns", buf_timestamp_ticks, buf_timestamp_ns);
+    GST_LOG_OBJECT (src, "Buffer GentTL timestamp: %llu ticks, %llu ns",
+        buf_timestamp_ticks, buf_timestamp_ns);
   }
 
   datasize = sizeof (frame_id);
@@ -1787,9 +1918,6 @@ gst_gentlsrc_create (GstPushSrc * psrc, GstBuffer ** buf)
   }
 
   return GST_FLOW_OK;
-
-error:
-  return GST_FLOW_ERROR;
 }
 
 gchar *

sys/gentl/gstgentlsrc.h

@@ -45,6 +45,7 @@ struct _GstGenTlProducer
 {
   gchar* cti_path;
   guint32 acquisition_mode_value;
+  guint32 timestamp_control_latch_value;
 
   guint64 width;
   guint64 height;
@@ -55,9 +56,11 @@ struct _GstGenTlProducer
   guint64 acquisition_stop;
   guint64 tick_frequency_low;
   guint64 tick_frequency_high;
-  guint64 timestamp_control_latch;
+  guint64 timestamp_control_latch;  // GevTimestampControlLatch
-  guint64 timestamp_low;
+  guint64 timestamp;                // TimestampLatchValue
-  guint64 timestamp_high;
+  guint64 timestamp_low;            // GevTimestampValueLow
+  guint64 timestamp_high;           // GevTimestampValueHigh
+  guint16 port_endianness;
 };
 
 
@@ -71,6 +74,7 @@ struct _GstGenTlProducer
 typedef enum {
   GST_GENTLSRC_PRODUCER_BASLER,
   GST_GENTLSRC_PRODUCER_EVT,
+  GST_GENTLSRC_PRODUCER_FLIR,
 } GstGenTlSrcProducer;