Remove need for Y16 firmware with runtime AGC/pseudocolor switching

Inc/lepton.h

@@ -5,13 +5,6 @@
 
 #define FRAME_HEADER_LENGTH (2)
 #define FRAME_LINE_LENGTH (80)
-#ifdef Y16
-#define FRAME_TOTAL_LENGTH (FRAME_HEADER_LENGTH + FRAME_LINE_LENGTH)
-#else
-#define FRAME_TOTAL_LENGTH (FRAME_HEADER_LENGTH + FRAME_LINE_LENGTH + 40)
-#endif
-#define FRAME_TOTAL_SIZE_Y16 (FRAME_TOTAL_LENGTH * sizeof(uint16_t) * 1)
-#define FRAME_TOTAL_SIZE_RGB (FRAME_TOTAL_LENGTH * sizeof(uint8_t) * 3)
 #define IMAGE_NUM_LINES (60)
 #define TELEMETRY_MAX_LINES (3)
 #define IMAGE_OFFSET_LINES (0)
@@ -20,6 +13,7 @@
 extern volatile uint8_t g_uvc_stream_status;
 extern volatile uint8_t g_lepton_type_3;
 extern volatile uint8_t g_telemetry_num_lines;
+extern volatile uint8_t g_format_y16;
 
 typedef enum {
   LEPTON_STATUS_OK = 0,
@@ -111,6 +105,7 @@ typedef struct _lepton_buffer {
   } lines;
   uint8_t number;
   lepton_status status;
+  uint8_t segment;
 } lepton_buffer;
 
 lepton_status complete_lepton_transfer(lepton_buffer *);

Inc/lepton_i2c.h

@@ -11,11 +11,13 @@ HAL_StatusTypeDef read_data();
 HAL_StatusTypeDef lepton_read_data(uint8_t * data);
 
 HAL_StatusTypeDef init_lepton_command_interface(void);
+HAL_StatusTypeDef disable_lepton_agc();
 HAL_StatusTypeDef enable_lepton_agc();
-HAL_StatusTypeDef disable_telemetry_and_radiometry(void);
+HAL_StatusTypeDef disable_telemetry(void);
 HAL_StatusTypeDef enable_telemetry(void);
 HAL_StatusTypeDef get_scene_stats(uint16_t *min, uint16_t *max, uint16_t *avg);
 HAL_StatusTypeDef enable_rgb888(LEP_PCOLOR_LUT_E pcolor_lut);
+HAL_StatusTypeDef enable_raw14();
 
 HAL_StatusTypeDef lepton_low_power();
 HAL_StatusTypeDef lepton_power_on();

Inc/project_config.h

@@ -6,10 +6,8 @@
 #define SPLASHSCREEN_OVERLAY
 // #define Y16
 
-#ifndef Y16
 // Values from LEP_PCOLOR_LUT_E in Middlewares/lepton_sdk/Inc/LEPTON_VID.h
 #define PSUEDOCOLOR_LUT LEP_VID_FUSION_LUT
-#endif
 
 #ifdef USART_DEBUG // this happens by 'make USART_DEBUG=1' or when USART_DEBUG is enabled above
 #undef THERMAL_DATA_UART

Middlewares/ST/STM32_USB_Device_Library/Class/video/Inc/uvc_desc_vs_l2.h

@@ -58,7 +58,6 @@
     .uvc_vs_frame  = UVC_FRAME_FORMAT(1, GREY, 80, 60),
     .uvc_vs_color  = UVC_COLOR_MATCHING_DESCRIPTOR(),
   },
-#ifndef Y16
   {
     .uvc_vs_format = UVC_FORMAT_UNCOMPRESSED_DESCRIPTOR(RGB565, 1),
     .uvc_vs_frame  = UVC_FRAME_FORMAT(1, RGB565, 80, 60),
@@ -69,5 +68,4 @@
     .uvc_vs_frame  = UVC_FRAME_FORMAT(1, BGR3, 80, 60),
     .uvc_vs_color  = UVC_COLOR_MATCHING_DESCRIPTOR(),
   },
-#endif
 },

Middlewares/ST/STM32_USB_Device_Library/Class/video/Inc/uvc_desc_vs_l3.h

@@ -58,7 +58,6 @@
     .uvc_vs_frame  = UVC_FRAME_FORMAT(1, GREY, 160, 120),
     .uvc_vs_color  = UVC_COLOR_MATCHING_DESCRIPTOR(),
   },
-#ifndef Y16
   {
     .uvc_vs_format = UVC_FORMAT_UNCOMPRESSED_DESCRIPTOR(RGB565, 1),
     .uvc_vs_frame  = UVC_FRAME_FORMAT(1, RGB565, 160, 120),
@@ -69,5 +68,4 @@
     .uvc_vs_frame  = UVC_FRAME_FORMAT(1, BGR3, 160, 120),
     .uvc_vs_color  = UVC_COLOR_MATCHING_DESCRIPTOR(),
   },
-#endif
 },

Middlewares/ST/STM32_USB_Device_Library/Class/video/Src/usbd_uvc.c

@@ -75,6 +75,7 @@
 volatile uint8_t g_uvc_stream_status = 0;
 volatile uint8_t g_lepton_type_3 = 0;
 volatile uint8_t g_telemetry_num_lines = 0;
+volatile uint8_t g_format_y16 = 0;
 
 /** @addtogroup STM32_USB_DEVICE_LIBRARY
   * @{

Src/lepton.c

@@ -42,7 +42,9 @@ void lepton_transfer(lepton_buffer *buf, int nlines)
 
   // DEBUG_PRINTF("Transfer starting: %p@%p\r\n", buf, packet);
 
-  status = setup_lepton_spi_rx(&hspi2, buf->lines.data, FRAME_TOTAL_LENGTH * nlines);
+  int packet_size = FRAME_HEADER_LENGTH +
+		  ((g_format_y16 ? sizeof(uint16_t) : sizeof(rgb_t)) * FRAME_LINE_LENGTH) / sizeof(uint16_t);
+  status = setup_lepton_spi_rx(&hspi2, buf->lines.data, packet_size * nlines);
 
   if (status != HAL_OK)
   {

Src/lepton_i2c.c

@@ -226,58 +226,52 @@ HAL_StatusTypeDef enable_lepton_agc()
   return HAL_OK;
 }
 
-HAL_StatusTypeDef disable_telemetry_and_radiometry(void)
+HAL_StatusTypeDef disable_lepton_agc()
 {
   LEP_RESULT result;
 
-  result = LEP_SetSysTelemetryEnableState(&hport_desc, LEP_TELEMETRY_DISABLED);
+  result = LEP_SetAgcEnableState(&hport_desc, LEP_AGC_DISABLE);
   if (result != LEP_OK) {
-    DEBUG_PRINTF("Could not disable telemetry %d\r\n", result);
+    DEBUG_PRINTF("Could not enable AGC\r\n");
     return HAL_ERROR;
   }
 
-  result = LEP_SetRadEnableState(&hport_desc, LEP_RAD_DISABLE);
+  return HAL_OK;
+}
+
+HAL_StatusTypeDef disable_telemetry(void)
+{
+  LEP_RESULT result;
+
+  result = LEP_SetSysTelemetryEnableState(&hport_desc, LEP_TELEMETRY_DISABLED);
   if (result != LEP_OK) {
-    DEBUG_PRINTF("Could not disable radiometry %d\r\n", result);
+    DEBUG_PRINTF("Could not disable telemetry %d\r\n", result);
     return HAL_ERROR;
   }
 
+  g_telemetry_num_lines = 0;
+
   return HAL_OK;
 }
 
 HAL_StatusTypeDef enable_telemetry(void)
 {
   LEP_RESULT result;
 
-  if (g_lepton_type_3)
-  {
-    // can't handle telemetry on L3 as segments are handled now
-
-    result = LEP_SetSysTelemetryEnableState(&hport_desc, LEP_TELEMETRY_DISABLED);
-    if (result != LEP_OK) {
-      DEBUG_PRINTF("Could not disable telemetry %d\r\n", result);
-      return HAL_ERROR;
-    }
-
-    g_telemetry_num_lines = 0;
+  result = LEP_SetSysTelemetryLocation(&hport_desc, LEP_TELEMETRY_LOCATION_FOOTER);
+  if (result != LEP_OK) {
+    DEBUG_PRINTF("Could not set telemetry location %d\r\n", result);
+    return HAL_ERROR;
   }
-  else
-  {
-    result = LEP_SetSysTelemetryLocation(&hport_desc, LEP_TELEMETRY_LOCATION_FOOTER);
-    if (result != LEP_OK) {
-      DEBUG_PRINTF("Could not set telemetry location %d\r\n", result);
-      return HAL_ERROR;
-    }
-
-    result = LEP_SetSysTelemetryEnableState(&hport_desc, LEP_TELEMETRY_ENABLED);
-    if (result != LEP_OK) {
-      DEBUG_PRINTF("Could not enable telemetry %d\r\n", result);
-      return HAL_ERROR;
-    }
-
-    g_telemetry_num_lines = 3;
+
+  result = LEP_SetSysTelemetryEnableState(&hport_desc, LEP_TELEMETRY_ENABLED);
+  if (result != LEP_OK) {
+    DEBUG_PRINTF("Could not enable telemetry %d\r\n", result);
+    return HAL_ERROR;
   }
 
+  g_telemetry_num_lines = 3;
+
   return HAL_OK;
 }
 
@@ -289,6 +283,12 @@ HAL_StatusTypeDef enable_rgb888(LEP_PCOLOR_LUT_E pcolor_lut)
   LEP_GetOemVideoOutputFormat(&hport_desc, &fmt);
   DEBUG_PRINTF("Current format: %d\r\n", fmt);
 
+  result = LEP_SetRadEnableState(&hport_desc, LEP_RAD_DISABLE);
+  if (result != LEP_OK) {
+    DEBUG_PRINTF("Could not disable radiometry %d\r\n", result);
+    return HAL_ERROR;
+  }
+
   result = LEP_SetOemVideoOutputFormat(&hport_desc, LEP_VIDEO_OUTPUT_FORMAT_RGB888);
   if (result != LEP_OK) {
     DEBUG_PRINTF("Could not set output format %d\r\n", result);
@@ -307,6 +307,32 @@ HAL_StatusTypeDef enable_rgb888(LEP_PCOLOR_LUT_E pcolor_lut)
   return HAL_OK;
 }
 
+HAL_StatusTypeDef enable_raw14()
+{
+  LEP_RESULT result;
+  LEP_OEM_VIDEO_OUTPUT_FORMAT_E fmt;
+
+  LEP_GetOemVideoOutputFormat(&hport_desc, &fmt);
+  DEBUG_PRINTF("Current format: %d\r\n", fmt);
+
+  result = LEP_SetRadEnableState(&hport_desc, LEP_RAD_ENABLE);
+  if (result != LEP_OK) {
+    DEBUG_PRINTF("Could not disable radiometry %d\r\n", result);
+    return HAL_ERROR;
+  }
+
+  result = LEP_SetOemVideoOutputFormat(&hport_desc, LEP_VIDEO_OUTPUT_FORMAT_RAW14);
+  if (result != LEP_OK) {
+    DEBUG_PRINTF("Could not set output format %d\r\n", result);
+    return HAL_ERROR;
+  }
+
+  LEP_GetOemVideoOutputFormat(&hport_desc, &fmt);
+  DEBUG_PRINTF("New format: %d\r\n", fmt);
+
+  return HAL_OK;
+}
+
 // HAL_OK       = 0x00,
 //  HAL_ERROR    = 0x01,
 //  HAL_BUSY     = 0x02,
@@ -349,7 +375,6 @@ HAL_StatusTypeDef init_lepton_command_interface(void)
 HAL_StatusTypeDef lepton_low_power()
 {
   LEP_RESULT result;
-  LEP_OEM_VIDEO_OUTPUT_FORMAT_E fmt;
 
   result = LEP_RunOemLowPowerMode2( &hport_desc );
   if (result != LEP_OK) {
@@ -363,7 +388,6 @@ HAL_StatusTypeDef lepton_low_power()
 HAL_StatusTypeDef lepton_power_on()
 {
   LEP_RESULT result;
-  LEP_OEM_VIDEO_OUTPUT_FORMAT_E fmt;
 
   result = LEP_RunOemPowerOn( &hport_desc );
   if (result != LEP_OK) {

Src/lepton_task.c

@@ -16,6 +16,7 @@
 #include "project_config.h"
 
 extern volatile uint8_t g_lepton_type_3;
+extern struct uvc_streaming_control videoCommitControl;
 
 lepton_buffer *completed_buffer;
 uint32_t completed_frame_count;
@@ -107,6 +108,11 @@ PT_THREAD( lepton_task(struct pt *pt))
 	static uint8_t last_end_line = 0;
 	curtick = last_tick = HAL_GetTick();
 
+#ifdef THERMAL_DATA_UART
+	enable_telemetry();
+	enable_raw14();
+#endif
+
 	while (1)
 	{
 #ifndef THERMAL_DATA_UART
@@ -120,7 +126,25 @@ PT_THREAD( lepton_task(struct pt *pt))
 				HAL_GPIO_TogglePin(SYSTEM_LED_GPIO_Port, SYSTEM_LED_Pin);
 
 				transferring_timer = HAL_GetTick();
-				PT_YIELD_UNTIL(pt, (HAL_GetTick() - transferring_timer) > 500);
+				PT_YIELD_UNTIL(pt, g_uvc_stream_status != 0 || (HAL_GetTick() - transferring_timer) > 500);
+			}
+
+			g_format_y16 = (videoCommitControl.bFormatIndex == VS_FMT_INDEX(Y16));
+
+			if (g_format_y16)
+			{
+				if (g_lepton_type_3)
+					disable_telemetry();
+				else
+					enable_telemetry();
+				disable_lepton_agc();
+				enable_raw14();
+			}
+			else
+			{
+				disable_telemetry();
+				enable_lepton_agc();
+				enable_rgb888(PSUEDOCOLOR_LUT);
 			}
 
 			// flush out any old data
@@ -151,13 +175,18 @@ PT_THREAD( lepton_task(struct pt *pt))
 
 		current_frame_count++;
 
-#ifdef Y16
-		current_segment = ((current_buffer->lines.y16[IMAGE_OFFSET_LINES + 20].header[0] & 0x7000) >> 12);
-		last_end_line = (current_buffer->lines.y16[IMAGE_OFFSET_LINES + IMAGE_NUM_LINES - 1].header[0] & 0x00ff);
-#else
-		current_segment = ((current_buffer->lines.rgb[IMAGE_OFFSET_LINES + 20].header[0] & 0x7000) >> 12);
-		last_end_line = (current_buffer->lines.rgb[IMAGE_OFFSET_LINES + IMAGE_NUM_LINES - 1].header[0] & 0x00ff);
-#endif
+		if (g_format_y16)
+		{
+			current_segment = ((current_buffer->lines.y16[IMAGE_OFFSET_LINES + 20].header[0] & 0x7000) >> 12);
+			last_end_line = (current_buffer->lines.y16[IMAGE_OFFSET_LINES + IMAGE_NUM_LINES - 1].header[0] & 0x00ff);
+		}
+		else
+		{
+			current_segment = ((current_buffer->lines.rgb[IMAGE_OFFSET_LINES + 20].header[0] & 0x7000) >> 12);
+			last_end_line = (current_buffer->lines.rgb[IMAGE_OFFSET_LINES + IMAGE_NUM_LINES - 1].header[0] & 0x00ff);
+		}
+
+		current_buffer->segment = current_segment;
 
 		if (last_end_line != (IMAGE_NUM_LINES - 1))
 		{
@@ -166,6 +195,8 @@ PT_THREAD( lepton_task(struct pt *pt))
 
 			if (current_frame_count > 2)
 			{
+				uint16_t last_header;
+
 				DEBUG_PRINTF("Synchronization lost, status: %d, last end line %d\r\n",
 					current_buffer->status, last_end_line);
 
@@ -178,11 +209,12 @@ PT_THREAD( lepton_task(struct pt *pt))
 
 					transferring_timer = HAL_GetTick();
 					PT_YIELD_UNTIL(pt, current_buffer->status != LEPTON_STATUS_TRANSFERRING || ((HAL_GetTick() - transferring_timer) > 200));
-#ifdef Y16
-				} while (current_buffer->status == LEPTON_STATUS_OK && (current_buffer->lines.y16[0].header[0] & 0x0f00) == 0x0f00);
-#else
-				} while (current_buffer->status == LEPTON_STATUS_OK && (current_buffer->lines.rgb[0].header[0] & 0x0f00) == 0x0f00);
-#endif
+
+					last_header = (g_format_y16 ?
+							current_buffer->lines.y16[0].header[0] :
+							current_buffer->lines.rgb[0].header[0]);
+
+				} while (current_buffer->status == LEPTON_STATUS_OK && (last_header & 0x0f00) == 0x0f00);
 
 				// we picked up the start of a new packet, so read the rest of it in
 				lepton_transfer(current_buffer, IMAGE_NUM_LINES - 1);
@@ -211,13 +243,14 @@ PT_THREAD( lepton_task(struct pt *pt))
 			);
 #endif
 
-#ifdef Y16
-			if (g_telemetry_num_lines > 0)
-				print_telemetry_temps(&current_buffer->lines.y16[TELEMETRY_OFFSET_LINES].data.telemetry_data);
-#else
+
 			if (g_telemetry_num_lines > 0)
-				print_telemetry_temps(&current_buffer->lines.rgb[TELEMETRY_OFFSET_LINES].data.telemetry_data);
-#endif
+			{
+				if (g_format_y16)
+					print_telemetry_temps(&current_buffer->lines.y16[TELEMETRY_OFFSET_LINES].data.telemetry_data);
+				else
+					print_telemetry_temps(&current_buffer->lines.rgb[TELEMETRY_OFFSET_LINES].data.telemetry_data);
+			}
 
 			read_tmp007_regs();
 
@@ -235,18 +268,19 @@ PT_THREAD( lepton_task(struct pt *pt))
 
 			HAL_GPIO_TogglePin(SYSTEM_LED_GPIO_Port, SYSTEM_LED_Pin);
 
-#ifndef Y16
-			for (row = 0; row < IMAGE_NUM_LINES; row++)
+			if (!g_format_y16)
 			{
-				uint16_t* lineptr = (uint16_t*)completed_buffer->lines.rgb[IMAGE_OFFSET_LINES + row].data.image_data;
-				while (lineptr < (uint16_t*)&completed_buffer->lines.rgb[IMAGE_OFFSET_LINES + row].data.image_data[FRAME_LINE_LENGTH])
+				for (row = 0; row < IMAGE_NUM_LINES; row++)
 				{
-				  uint8_t* bytes = (uint8_t*)lineptr;
-				  *lineptr++ = bytes[0] << 8 | bytes[1];
+					uint16_t* lineptr = (uint16_t*)completed_buffer->lines.rgb[IMAGE_OFFSET_LINES + row].data.image_data;
+					while (lineptr < (uint16_t*)&completed_buffer->lines.rgb[IMAGE_OFFSET_LINES + row].data.image_data[FRAME_LINE_LENGTH])
+					{
+					  uint8_t* bytes = (uint8_t*)lineptr;
+					  *lineptr++ = bytes[0] << 8 | bytes[1];
+					}
+					PT_YIELD(pt);
 				}
-				PT_YIELD(pt);
 			}
-#endif
 
 			if (!full(CIRC_BUF_HANDLE(completed_frames_buf)))
 				push(CIRC_BUF_HANDLE(completed_frames_buf), completed_buffer);