Прибор ночного видения на базе тепловизионного модуля Flir Lepton 3

#ifndef LEPTON_CONTROL_H
#define LEPTON_CONTROL_H

#include <stdbool.h>
#include <stdio.h>

//исходные размеры изображения (не перевёрнутое)
#define LEPTON_ORIGINAL_IMAGE_WIDTH 160
#define LEPTON_ORIGINAL_IMAGE_HEIGHT 120

//высота кадра VoSPI
#define VOSPI_FRAME_HEIGHT 60
//ширина кадра VoSPI
#define VOSPI_FRAME_WIDTH 80
//размер пакета VoSPI а байтах (164 для RAW14 и 244 для RGB)
#define VOSPI_PACKAGE_SIZE 164
//размер строки пакета VoSPI в байтах
#define VOSPI_PACKAGE_LINE_SIZE 160
//размер сегмента VOSPI в байтах
#define VOSPI_SEGMENT_LINE_AMOUNT 60

void LEPTONCONTROL_Init(void);//инициализация
void LEPTONCONTROL_CalculateCRC(unsigned short *crc,unsigned char byte);//вычислить crc
bool LEPTONCONTROL_PushVoSPI(unsigned char data[VOSPI_PACKAGE_SIZE],bool *first_line);//подать данные одного пакета VoSPI на вход модуля
unsigned short *LEPTONCONTROL_GetRAW14Ptr(void);//получить указатель на данные собранного изображения
#endif

#include "leptoncontrol.h"
#include "stm32f4xx_hal.h"

static unsigned short RAW14Image[LEPTON_ORIGINAL_IMAGE_HEIGHT*LEPTON_ORIGINAL_IMAGE_WIDTH];//собираемое изображение
static unsigned short CRCTable[256];//таблица для расчета CRC16

//время для ресинхронизации в мс
#define RESYNC_TIMEOUT_MS 19
//код: нет сегмента
#define NO_SEGMENT -1
//код: ошибка пакета
#define ERROR_PACKAGE -2

//----------------------------------------------------------------------------------------------------
//инициализация
//----------------------------------------------------------------------------------------------------
void LEPTONCONTROL_Init(void)
{
 //инициалдизируем таблицу для вычисления CRC
 unsigned short code;
 for(long n=0;n<256;n++)
 {
  code=((unsigned short)n)<<8;
  for(unsigned char m=0;m<8;m++)
  {
   if(code&(1<<15)) code=(code<<1)^0x1021;
               else code=code<<1;
  }
  CRCTable[n]=code;
 }
}

//----------------------------------------------------------------------------------------------------
//вычислить crc
//----------------------------------------------------------------------------------------------------
void LEPTONCONTROL_CalculateCRC(unsigned short *crc,unsigned char byte)
{
 *crc=CRCTable[(((*crc)>>8)^byte++)&0xFF]^((*crc)<<8);
}

//----------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------
long LEPTONCONTROL_ReadSegment(unsigned short *raw14_ptr,unsigned char data[VOSPI_PACKAGE_SIZE],bool *first_line)
{
 static long current_package=-1;
 static long segment=-1;
 long n;
 *first_line=false;
 if ((data[0]&0x0F)==0x0F) return(NO_SEGMENT);//отбрасываемый пакет
 unsigned short crc=data[2];
 crc<<=8;
 crc|=data[3];
 //считаем CRC
 unsigned short crc16=0;
 LEPTONCONTROL_CalculateCRC(&crc16,data[0]&0x0F);
 LEPTONCONTROL_CalculateCRC(&crc16,data[1]);
 LEPTONCONTROL_CalculateCRC(&crc16,0);
 LEPTONCONTROL_CalculateCRC(&crc16,0);
 for(n=4;n<VOSPI_PACKAGE_SIZE;n++) LEPTONCONTROL_CalculateCRC(&crc16,data[n]);
 if (crc16!=crc) return(ERROR_PACKAGE);//ошибка CRC

 //определяем номер пакета
 unsigned short package=data[0]&0x0F;
 package<<=8;
 package|=data[1];
 if (package==0)
 {
  *first_line=true;
  current_package=0;
 }
 if (package==20)
 {
  unsigned char ttt=(data[0]&0x70)>>4;//номер кадра бывает только в 20 пакете
  segment=ttt;
 }
 if (current_package<0) return(NO_SEGMENT);
 if (current_package!=package)
 { 
  current_package=-1;
  return(ERROR_PACKAGE);
 }
 unsigned short *raw_ptr=raw14_ptr+current_package*VOSPI_PACKAGE_LINE_SIZE/2;
 for(n=0;n<VOSPI_PACKAGE_LINE_SIZE/2;n++,raw_ptr++)
 {
  //байты заданы в порядке big-endian: старший, младший
  unsigned short value=data[n*sizeof(short)+4];
  value<<=8;
  value|=data[n*sizeof(short)+5];
  *raw_ptr=value;
 }
 current_package++;
 if (current_package!=VOSPI_FRAME_HEIGHT) return(NO_SEGMENT);
 current_package=-1;
 return(segment); 
}

//----------------------------------------------------------------------------------------------------
//подать данные одного пакета VoSPI на вход модуля
//----------------------------------------------------------------------------------------------------
bool LEPTONCONTROL_PushVoSPI(unsigned char data[VOSPI_PACKAGE_SIZE],bool *first_line)
{
 *first_line=false;
 static long waitable_segment=1;
 long segment=LEPTONCONTROL_ReadSegment(RAW14Image+(waitable_segment-1)*VOSPI_FRAME_WIDTH*VOSPI_SEGMENT_LINE_AMOUNT,data,first_line);
 if (segment==ERROR_PACKAGE) HAL_Delay(RESYNC_TIMEOUT_MS);
 if (segment==ERROR_PACKAGE || segment==0) waitable_segment=1;
 if (segment==ERROR_PACKAGE || segment==NO_SEGMENT || segment==0) return(false);
 
 if (segment!=waitable_segment)
 {
  waitable_segment=1;
  if (segment!=1) return(false);
 }
 waitable_segment++;
 if (waitable_segment!=5) return(false);
 waitable_segment=1; 
 return(true);
}
//----------------------------------------------------------------------------------------------------
//получить указатель на данные собранного изображения
//----------------------------------------------------------------------------------------------------
unsigned short *LEPTONCONTROL_GetRAW14Ptr(void)
{
 return(RAW14Image);
}

while(1)
	{
   //ищем начало кадра		
   while(1) 
   {
    HAL_SPI_Receive(&hspi1,buffer,VOSPI_PACKAGE_SIZE,0x1000);			 
    bool first_line=false;
    unsigned char *buffer_ptr=buffer;
    LEPTONCONTROL_PushVoSPI(buffer_ptr,&first_line);
    if (first_line==true) break;
   }
   //читаем остаток пакета от lepton3
   unsigned char *buffer_ptr=buffer;
   HAL_SPI_Receive(&hspi1,buffer_ptr,VOSPI_PACKAGE_SIZE*SPI_READ_VOSPI_AMOUNT,0x1000);
   buffer_ptr=buffer;
   for(long n=0;n<SPI_READ_VOSPI_AMOUNT;n++,buffer_ptr+=VOSPI_PACKAGE_SIZE) 
   {
    bool first_line=false;
    bool res=LEPTONCONTROL_PushVoSPI(buffer_ptr,&first_line);
    if (res==true) CreateImage();//расшифровываем данные и рисуем изображение
   } 
  }