diff --git a/sys/gentl/gstgentlsrc.c b/sys/gentl/gstgentlsrc.c index ebf84af..5dea8db 100644 --- a/sys/gentl/gstgentlsrc.c +++ b/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 = - GTL_GCReadPort (src->hDevPort, src->producer.payload_size, &val, - &datasize); + payload_size = read_uint32 (src, src->producer.payload_size, &ret); 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) - return 0; + 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; - size_t datasize = 4; - 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); + guint64 tick_frequency = read_uint64 (src, src->producer.tick_frequency_low, + src->producer.tick_frequency_high, &ret); 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; - guint32 val, ts_low, ts_high; + guint64 timestamp_ns; - val = GUINT32_TO_BE (2); - datasize = sizeof (val); - ret = GTL_GCWritePort (src->hDevPort, src->producer.timestamp_control_latch, &val, &datasize); // GevTimestampControlLatch + ret = + write_uint32 (src, src->producer.timestamp_control_latch, + 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 - HANDLE_GTL_WARNING ("Failed to get GevTimestampValueLow"); - ret = GTL_GCReadPort (src->hDevPort, src->producer.timestamp_high, &ts_high, &datasize); // GevTimestampValueHigh - HANDLE_GTL_WARNING ("Failed to get GevTimestampValueHigh"); - guint64 ticks = GUINT64_FROM_BE ((guint64) ts_low << 32 | ts_high); - GST_LOG_OBJECT (src, "Timestamp ticks are %llu", ticks); + if (src->producer.timestamp) { + timestamp_ns = read_uint64_single (src, src->producer.timestamp, &ret); + HANDLE_GTL_WARNING ("Failed to read device timestamp"); + } else { + 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) - (gev_ts * ((double)GST_SECOND / src->tick_frequency)));; + src->gentl_latched_ns = gev_ts; + 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])); - datasize = sizeof (val); - ret = - GTL_GCWritePort (src->hDevPort, strtol (pair[0], NULL, 16), &val, - &datasize); + ret = write_uint32 (src, strtol (pair[0], NULL, 16), atoi (pair[1])); 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; - GstVideoInfo vinfo; + guint32 width, height; 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; - size_t datasize = 4; - ret = GTL_GCReadPort (src->hDevPort, src->producer.width, &val, &datasize); + width = read_uint32 (src, src->producer.width, &ret); HANDLE_GTL_ERROR ("Failed to get width"); - width = GUINT32_FROM_BE (val); - ret = GTL_GCReadPort (src->hDevPort, src->producer.height, &val, &datasize); + height = read_uint32 (src, src->producer.height, &ret); HANDLE_GTL_ERROR ("Failed to get height"); - height = GUINT32_FROM_BE (val); GST_DEBUG_OBJECT (src, "Width and height %dx%d", width, height); - ret = - GTL_GCReadPort (src->hDevPort, src->producer.pixel_format, &val, - &datasize); - HANDLE_GTL_ERROR ("Failed to get height"); + guint32 pixfmt_enum = read_uint32 (src, src->producer.pixel_format, &ret); + HANDLE_GTL_ERROR ("Failed to get pixel format"); 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, - &datasize); + write_uint32 (src, src->producer.acquisition_mode, + src->producer.acquisition_mode_value); HANDLE_GTL_ERROR ("Failed to start device acquisition"); /* send AcquisitionStart command */ - val = GUINT32_TO_BE (1); - datasize = sizeof (val); - ret = - GTL_GCWritePort (src->hDevPort, src->producer.acquisition_start, &val, - &datasize); + ret = write_uint32 (src, src->producer.acquisition_start, 1); 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); - gsize datasize = sizeof (val); - GC_ERROR ret = - GTL_GCWritePort (src->hDevPort, src->producer.acquisition_stop, &val, - &datasize); + GC_ERROR ret; + ret = write_uint32 (src, src->producer.acquisition_stop, 1); 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; } @@ -1647,15 +1777,16 @@ gst_gentlsrc_get_buffer (GstGenTlSrc * src) GTL_DSGetBufferInfo (src->hDS, new_buffer_data.BufferHandle, BUFFER_INFO_TIMESTAMP_NS, &datatype, &buf_timestamp_ns, &datasize); if (ret == GC_ERR_SUCCESS) { - GST_LOG_OBJECT(src, "Buffer GentTL timestamp: %llu ns", buf_timestamp_ns); + GST_LOG_OBJECT (src, "Buffer GentTL timestamp: %llu ns", buf_timestamp_ns); } else { ret = - GTL_DSGetBufferInfo(src->hDS, new_buffer_data.BufferHandle, + GTL_DSGetBufferInfo (src->hDS, new_buffer_data.BufferHandle, BUFFER_INFO_TIMESTAMP, &datatype, &buf_timestamp_ticks, &datasize); - HANDLE_GTL_ERROR("Failed to get buffer timestamp"); + 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); + (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); } 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 * diff --git a/sys/gentl/gstgentlsrc.h b/sys/gentl/gstgentlsrc.h index c9c8a78..8e6934e 100644 --- a/sys/gentl/gstgentlsrc.h +++ b/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_low; - guint64 timestamp_high; + guint64 timestamp_control_latch; // GevTimestampControlLatch + guint64 timestamp; // TimestampLatchValue + 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;