/* ******************************************************************************* * Copyright (c) 2010-2022 VATICS(KNERON) Inc. All rights reserved. * * +-----------------------------------------------------------------+ * | THIS SOFTWARE IS FURNISHED UNDER A LICENSE AND MAY ONLY BE USED | * | AND COPIED IN ACCORDANCE WITH THE TERMS AND CONDITIONS OF SUCH | * | A LICENSE AND WITH THE INCLUSION OF THE THIS COPY RIGHT NOTICE. | * | THIS SOFTWARE OR ANY OTHER COPIES OF THIS SOFTWARE MAY NOT BE | * | PROVIDED OR OTHERWISE MADE AVAILABLE TO ANY OTHER PERSON. THE | * | OWNERSHIP AND TITLE OF THIS SOFTWARE IS NOT TRANSFERRED. | * | | * | THE INFORMATION IN THIS SOFTWARE IS SUBJECT TO CHANGE WITHOUT | * | ANY PRIOR NOTICE AND SHOULD NOT BE CONSTRUED AS A COMMITMENT BY | * | VATICS(KNERON) INC. | * +-----------------------------------------------------------------+ * ******************************************************************************* */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #define MODULE_NAME "vospi" #define VENC_OUTPUT_BUF_NUM 3 #define VENC_VSRC_PIN "vsrc_ssm" //! VMF_VSRC Output pin #define WRITER_PIN "vsrc_ssm_tc" #define VENC_OUTPUT_PIN "venc_srb_1" //! VMF_VideoEnc Output pin #define VENC_TH_OUTPUT_PIN "venc_srb_2" //! VMF_VideoEnc Output pin #define VENC_CMD_FIFO "/tmp/venc/c0/command.fifo" //! communicate with rtsps, vrec, etc. #define VENC_RESOURCE_DIR "./Resource/" //! directory contains ISP, AE, AWB, AutoScene sub directory #define VENC_ENCODE_BUF_SIZE (4*1024*1024) //! 4*1024*1024 #define VENC_ENCODE_WIDTH 320 #define VENC_ENCODE_HEIGHT 240 #define LEPTON_SPI_FPS 26 #define THERMAL_FPS 9 // about LEPTON_SPI_FPS/3 #define COLOR_STEPS 256 #define PACKET_SIZE 164 #define READ_PACKET_NUM 60 #define VENC_TH_ENCODE_BUF_SIZE (4*VENC_ENCODE_WIDTH*VENC_ENCODE_HEIGHT) //! 4*Width*Height //#define DEBUG_WRITE_YUV_FILE 1 #define THERMAL_I2C_DEVICE "/dev/i2c-0" #define I2C_TIMEOUT 0x0702 #define I2C_RETRIES 0x0701 #define I2C_RDWR 0x0707 #define THERMAL_DEVICE "/dev/lepton3.0" struct i2c_msg { unsigned short addr; unsigned short flags; unsigned short len; unsigned char *buf; }; struct i2c_rdwr_ioctl_data { struct i2c_msg *msgs; int nmsgs; }; // YUV Palette const unsigned char SPECTRAL_Y[COLOR_STEPS]= { 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 114, 115, 117, 119, 120, 122, 124, 126, 128, 130, 131, 133, 135, 137, 139, 141, 142, 144, 146, 148, 150, 152, 153, 155, 157, 159, 161, 163, 164, 166, 167, 168, 170, 171, 173, 174, 176, 177, 178, 180, 181, 183, 184, 186, 187, 188, 190, 191, 193, 194, 195, 197, 198, 200, 201, 202, 203, 204, 206, 207, 208, 209, 210, 212, 213, 214, 215, 216, 218, 219, 220, 221, 222, 223, 225, 226, 227, 228, 229, 230, 231, 232, 232, 233, 234, 234, 235, 236, 236, 237, 238, 238, 239, 240, 241, 241, 242, 243, 243, 244, 245, 245, 246, 247, 247, 246, 245, 244, 243, 242, 241, 240, 239, 238, 237, 236, 235, 234, 233, 232, 231, 230, 229, 229, 228, 227, 226, 225, 224, 223, 221, 220, 219, 217, 216, 215, 213, 212, 211, 209, 208, 207, 205, 204, 203, 201, 200, 198, 197, 196, 194, 193, 192, 190, 189, 187, 186, 184, 182, 181, 179, 177, 175, 174, 172, 170, 168, 167, 165, 163, 161, 160, 158, 156, 154, 153, 151, 149, 148, 146, 144, 143, 141, 140, 139, 137, 136, 134, 133, 132, 130, 129, 127, 126, 125, 123, 122, 120, 119, 118, 116, 115, 113, 112, 111, 109, 108, 105, 103, 101, 99, 97, 95, 93, 91, 89, 86, 84, 82, 80, 78, 76, 74, 72, 70, 67, 65, 63, 61, 59, 57, 55 }; const unsigned char SPECTRAL_U[COLOR_STEPS]= { 166, 166, 166, 166, 166, 166, 166, 167, 167, 167, 167, 167, 167, 167, 167, 167, 167, 167, 167, 167, 168, 168, 168, 168, 168, 168, 167, 166, 164, 163, 161, 159, 158, 156, 155, 153, 152, 150, 148, 147, 145, 144, 142, 141, 139, 137, 136, 134, 133, 131, 130, 128, 127, 126, 126, 125, 124, 123, 122, 121, 121, 120, 119, 118, 117, 116, 116, 115, 114, 113, 112, 111, 111, 110, 109, 108, 107, 106, 106, 105, 104, 103, 102, 101, 100, 99, 98, 97, 96, 95, 94, 93, 92, 91, 91, 90, 89, 88, 87, 86, 85, 84, 83, 84, 84, 84, 85, 85, 86, 86, 87, 87, 88, 88, 89, 89, 90, 90, 91, 91, 91, 92, 92, 93, 93, 94, 94, 95, 95, 94, 93, 93, 92, 92, 91, 90, 90, 89, 89, 88, 87, 87, 86, 86, 85, 84, 84, 83, 82, 82, 81, 81, 80, 79, 79, 79, 79, 79, 79, 78, 78, 78, 78, 78, 78, 77, 77, 77, 77, 77, 77, 76, 76, 76, 76, 76, 76, 75, 75, 75, 76, 76, 76, 77, 77, 77, 78, 78, 78, 78, 79, 79, 79, 80, 80, 80, 81, 81, 81, 82, 82, 82, 83, 83, 83, 84, 85, 86, 87, 88, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 129, 130, 131, 132, 133 }; const unsigned char SPECTRAL_V[COLOR_STEPS]= { 128, 126, 124, 122, 120, 118, 116, 115, 113, 111, 109, 107, 105, 103, 101, 99, 98, 96, 94, 92, 90, 88, 86, 84, 83, 81, 80, 80, 80, 80, 80, 80, 81, 81, 81, 81, 81, 81, 81, 82, 82, 82, 82, 82, 82, 82, 83, 83, 83, 83, 83, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 103, 104, 105, 106, 107, 108, 109, 109, 110, 111, 112, 113, 113, 114, 115, 116, 117, 117, 118, 119, 120, 121, 121, 122, 123, 124, 125, 125, 126, 127, 127, 127, 128, 128, 128, 128, 128, 129, 129, 129, 129, 129, 130, 130, 130, 130, 130, 131, 131, 131, 131, 131, 132, 132, 132, 133, 133, 134, 134, 135, 136, 136, 137, 138, 138, 139, 140, 140, 141, 142, 142, 143, 144, 144, 145, 146, 146, 147, 148, 148, 149, 150, 151, 152, 153, 154, 155, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 198, 198, 198, 198, 199, 199, 199, 199, 199, 199, 199, 199, 200, 200, 200, 200, 200, 200, 200, 200, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 200, 200, 200, 200, 200, 200, 200, 200, 200 }; VMF_LAYOUT_T g_tLayout; static unsigned short *g_adwThermalValue = NULL; static int g_bTerminate = 0; static unsigned int g_dwStreamingNum = 0; static unsigned int g_dwStreamingThNum = 0; static unsigned int g_dwLeptonType = 3; static unsigned int g_dwResetSleepType = 1; static char* g_szAutoSceneConfig = NULL; static char* g_szSensorConfig = NULL; static char* g_szSensorConfigFusion = NULL; VMF_VSRC_HANDLE_T* g_ptVsrcHandle = NULL; VMF_BIND_CONTEXT_T* g_ptBind = NULL; VMF_BIND_CONTEXT_T* g_ptThBind = NULL; VMF_VENC_OUT_SRB_T* g_ptVencOutputSRB = NULL; VMF_VENC_OUT_SRB_T* g_ptVencThOutputSRB = NULL; VMF_VENC_HANDLE_T* g_ptVencHandle = NULL; VMF_VENC_HANDLE_T* g_ptVencThHandle = NULL; VMF_RS_HANDLE_T* g_ptResizeHandle = NULL; VMF_VENC_CODEC_TYPE g_eCodecType = VMF_VENC_CODEC_TYPE_H264; static pthread_mutex_t g_tThemalDataMutex = PTHREAD_MUTEX_INITIALIZER; static char *g_ptThermalDevice = NULL; static char *g_ptI2cDevice = NULL; static int g_bInitialized = 0; static unsigned int g_dwPreFrameCount = 0; static unsigned int g_dwCurFrameCount = 0; static unsigned int g_dwThermalWidth = 80; static unsigned int g_dwThermalHeight = 60; static unsigned int g_dwSpiSpeed = 17000000; static int g_dwSpiFd = -1; static VMF_H4E_CONFIG_T g_tH4e_config = { 25, // dwQp 4000000, // dwBitrate 30, // dbFps 60, // dwGop VMF_H4E_PROFILE_HIGH, // eProfile 0, // iSliceQualityStrategy VMF_ADMODE_MEET_FPS, // eAdMode 0, // dwMinQp 0, // dwMaxQp 0, // dwMinFps 0, // dwVirtIFrameInterval 0 // dwPIQ }; static VMF_H5E_CONFIG_T g_tH5e_config = { 25, // dwQp 4000000, // dwBitrate 30, // dwFps 30, // dwGop 0, // iSliceQualityStrategy 0, // dwMinQp 0, // dwMaxQp 0, // Virtual I-frame interval 0, // dwPIQ VMF_ADMODE_MEET_FPS, // eAdMode 0 // Complex map control in VBR mode }; static VMF_JE_CONFIG_T g_tJep_config = { 50, // dwQp 0, // bEnableThumbnail 0, // dwThumbnailQp 0, // bJfifHdr 1024*1024, // dwBitrate 25 }; void print_msg(const char *fmt, ...) { va_list ap; va_start(ap, fmt); fprintf(stderr, "[%s] ", MODULE_NAME); vfprintf(stderr, fmt, ap); va_end(ap); } static void release_bind(void) { VMF_BIND_Release(g_ptBind); } static void release_thbind(void) { VMF_BIND_Release(g_ptThBind); } static void release_video_source(void) { if(g_ptVsrcHandle) { VMF_VSRC_Stop(g_ptVsrcHandle); VMF_VSRC_Release(g_ptVsrcHandle); } } static void vsrc_init_callback(unsigned int width, unsigned int height) { memset(&g_tLayout, 0, sizeof(VMF_LAYOUT_T)); g_tLayout.dwCanvasWidth = width; g_tLayout.dwCanvasHeight = height; g_tLayout.dwVideoPosX = 0; g_tLayout.dwVideoPosY = 0; g_tLayout.dwVideoWidth = width; g_tLayout.dwVideoHeight = height; print_msg("[%s]: width:%d, height:%d \n", __func__, width, height); } static void setup_spec(VMF_VSRC_SPEC_CONFIG_T* ptSpec, VMF_VENC_CODEC_TYPE eCodecType) { ptSpec->bEnableSpec = 1; ptSpec->dwIspMode = VMF_ISP_MODE_FEC_C; if (eCodecType == VMF_VENC_CODEC_TYPE_H265){ ptSpec->tIfpEncSpec.bEncH265 = 1; } else if (eCodecType == VMF_VENC_CODEC_TYPE_H264){ ptSpec->tIfpEncSpec.bEncH264 = 1; } else if (eCodecType == VMF_VENC_CODEC_TYPE_MJPG){ ptSpec->tIfpEncSpec.bEncJPEG = 1; } if (eCodecType == VMF_VENC_CODEC_TYPE_H265){ ptSpec->tIspEncSpec.bEncH265 = 1; } else if (eCodecType == VMF_VENC_CODEC_TYPE_H264){ ptSpec->tIspEncSpec.bEncH264 = 1; } else if (eCodecType == VMF_VENC_CODEC_TYPE_MJPG){ ptSpec->tIspEncSpec.bEncJPEG = 1; } } static void setup_fec(VMF_FEC_P180_CONFIG_T *ptFecConfig) { ptFecConfig->fZoom = 1.5; ptFecConfig->fFocalLength = 0.630; ptFecConfig->eModeType = VMF_FEC_MODE_PANO_180_TWO_DIRECTION; ptFecConfig->fDstOffsetX = 0.0; ptFecConfig->fDstOffsetY = 0.0; ptFecConfig->fDstXYRatio = 1.00; ptFecConfig->fRectCurvature = 0.35; ptFecConfig->fRectSlope = 0.35; ptFecConfig->eLensType = VMF_FEC_LENS_EQUIDISTANT; } static int init_video_source(VMF_VENC_CODEC_TYPE eCodecType) { VMF_VSRC_INITOPT_T tVsrcInitOpt; VMF_VSRC_FRONTEND_CONFIG_T tVsrcFrontendConfig; VMF_FEC_P180_CONFIG_T tFecP180Config; memset(&tVsrcInitOpt, 0, sizeof(VMF_VSRC_INITOPT_T)); memset(&tVsrcFrontendConfig, 0, sizeof(VMF_VSRC_FRONTEND_CONFIG_T)); memset(&tFecP180Config, 0, sizeof(VMF_FEC_P180_CONFIG_T)); setup_fec(&tFecP180Config); tVsrcFrontendConfig.tFecInitConfig.ptFecConfig = &tFecP180Config; tVsrcFrontendConfig.tFecInitConfig.eCoeffMode = VMF_FEC_COEF_MODE_P180; tVsrcFrontendConfig.tFecInitConfig.eFecMethod = VMF_FEC_METHOD_GTR; tVsrcFrontendConfig.tFecInitConfig.eGridSize = VMF_FEC_GRID_8X8; tVsrcFrontendConfig.apszSensorConfig[0] = g_szSensorConfig; tVsrcFrontendConfig.apszSensorConfig[1] = g_szSensorConfigFusion; if(tVsrcFrontendConfig.apszSensorConfig[1] != NULL) { print_msg("[%s] -d detected, start with Fusion mode.\n", __func__); tVsrcFrontendConfig.dwSensorConfigCount = 2; tVsrcInitOpt.eAppMode = VMF_VSRC_APP_MODE_FUSION; } else { print_msg("[%s] Not using -d, start with Normal mode.\n", __func__); tVsrcFrontendConfig.dwSensorConfigCount = 1; tVsrcInitOpt.eAppMode = VMF_VSRC_APP_MODE_NORMAL; } tVsrcInitOpt.dwFrontConfigCount = 1; tVsrcInitOpt.ptFrontConfig = &tVsrcFrontendConfig; tVsrcInitOpt.pszAutoSceneConfig = g_szAutoSceneConfig; tVsrcInitOpt.pszOutPinPrefix = VENC_VSRC_PIN; tVsrcInitOpt.bShared = 1; tVsrcInitOpt.fnInitCallback = vsrc_init_callback; tVsrcInitOpt.pszResourceDir = VENC_RESOURCE_DIR; setup_spec(&tVsrcInitOpt.tSpecConfig, eCodecType); g_ptVsrcHandle = VMF_VSRC_Init(&tVsrcInitOpt); if (!g_ptVsrcHandle) { print_msg("[%s] VMF_VSRC_Init failed!\n", __func__); return -1; } if (VMF_VSRC_Start(g_ptVsrcHandle, NULL) != 0) { print_msg("[%s] VMF_VSRC_Start failed!\n", __func__); release_video_source(); return -1; } return 0; } static int init_bind(void) { VMF_BIND_INITOPT_T tBindOpt; memset(&tBindOpt, 0, sizeof(VMF_BIND_INITOPT_T)); tBindOpt.dwSrcOutputIndex = 0; tBindOpt.ptSrcHandle = g_ptVsrcHandle; tBindOpt.pfnQueryFunc = (VMF_BIND_QUERY_FUNC) VMF_VSRC_GetInfo; tBindOpt.pfnIspFunc = (VMF_BIND_CONFIG_ISP_FUNC) VMF_VSRC_ConfigISP; g_ptBind = VMF_BIND_Init(&tBindOpt); if (!g_ptBind){ print_msg("[%s] VMF_BIND_Init failed!!\n", __func__); release_video_source(); return -1; } return 0; } static int init_thbind(void) { VMF_BIND_INITOPT_T tBindOpt; memset(&tBindOpt, 0, sizeof(VMF_BIND_INITOPT_T)); tBindOpt.dwSrcOutputIndex = 0; tBindOpt.ptSrcHandle = g_ptVsrcHandle; tBindOpt.pfnQueryFunc = (VMF_BIND_QUERY_FUNC) VMF_VSRC_GetInfo; tBindOpt.pfnIspFunc = (VMF_BIND_CONFIG_ISP_FUNC) VMF_VSRC_ConfigISP; g_ptThBind = VMF_BIND_Init(&tBindOpt); if (!g_ptThBind){ print_msg("[%s] VMF_BIND_Init failed!!\n", __func__); release_video_source(); return -1; } return 0; } static int init_resize_hanle(void) { VMF_RS_INITOPT_T init_opt; VMF_RS_CONFIG_T Config_Opt; memset(&init_opt, 0, sizeof(VMF_RS_INITOPT_T)); memset(&Config_Opt, 0, sizeof(VMF_RS_CONFIG_T)); init_opt.dwSrcWidth = g_dwThermalWidth; init_opt.dwSrcHeight = g_dwThermalHeight; init_opt.dwSrcStride = g_dwThermalWidth; init_opt.dwFormatFlag = 0; init_opt.pszParamsDir = "./Resource/ISP/0/"; Config_Opt.dwDstWidth = VENC_ENCODE_WIDTH; Config_Opt.dwDstHeight = VENC_ENCODE_HEIGHT; Config_Opt.dwDstStride = VENC_ENCODE_WIDTH; Config_Opt.dwSharpness = 0; g_ptResizeHandle = VMF_RS_Init(&init_opt,&Config_Opt); if (!g_ptResizeHandle) return -1; return 0; } static void release_output_srb(void) { VMF_VENC_OUT_SRB_Release(&g_ptVencOutputSRB); } static void release_thoutput_srb(void) { VMF_VENC_OUT_SRB_Release(&g_ptVencThOutputSRB); } static void release_video_encoder(void) { if(g_ptVencHandle) VMF_VENC_Release(g_ptVencHandle); if(g_ptVencOutputSRB) VMF_VENC_OUT_SRB_Release(&g_ptVencOutputSRB); } static void release_thvideo_encoder(void) { if(g_ptVencThHandle) VMF_VENC_Release(g_ptVencThHandle); if(g_ptVencThOutputSRB) VMF_VENC_OUT_SRB_Release(&g_ptVencThOutputSRB); } static int init_output_srb(const char* name, unsigned int buf_number, unsigned int buf_size) { VMF_VENC_OUT_SRB_INITOPT_T tSrbInitOpt; memset(&tSrbInitOpt, 0, sizeof(VMF_VENC_OUT_SRB_INITOPT_T)); tSrbInitOpt.pszSrbName = name; tSrbInitOpt.dwSrbNum = buf_number; tSrbInitOpt.dwSrbSize = buf_size; if (0 != VMF_VENC_OUT_SRB_Init(&g_ptVencOutputSRB, &tSrbInitOpt)) { print_msg("[%s] VMF_VENC_OUT_SRB_Init failed!!\n", __func__); release_video_source(); release_bind(); return -1; } return 0; } static int init_thoutput_srb(const char* name, unsigned int buf_number, unsigned int buf_size) { VMF_VENC_OUT_SRB_INITOPT_T tSrbInitOpt; memset(&tSrbInitOpt, 0, sizeof(VMF_VENC_OUT_SRB_INITOPT_T)); tSrbInitOpt.pszSrbName = name; tSrbInitOpt.dwSrbNum = buf_number; tSrbInitOpt.dwSrbSize = buf_size; if (0 != VMF_VENC_OUT_SRB_Init(&g_ptVencThOutputSRB, &tSrbInitOpt)) { print_msg("[%s] Thermal VMF_VENC_OUT_SRB_Init failed!!\n", __func__); release_thbind(); return -1; } return 0; } static int init_video_encoder(VMF_VENC_CODEC_TYPE eCodecType, int dwVideoWidth, int dwVideoHeight) { VMF_VENC_CONFIG_T tVencConfig; memset(&tVencConfig, 0, sizeof(VMF_VENC_CONFIG_T)); tVencConfig.dwEncWidth = dwVideoWidth; tVencConfig.dwEncHeight = dwVideoHeight; if(g_szSensorConfigFusion){ tVencConfig.dwFps = 20; } else { tVencConfig.dwFps = 30; } switch(eCodecType){ case VMF_VENC_CODEC_TYPE_H264: tVencConfig.eCodecType = VMF_VENC_CODEC_TYPE_H264; tVencConfig.pCodecConfig = &g_tH4e_config; break; case VMF_VENC_CODEC_TYPE_H265: tVencConfig.eCodecType = VMF_VENC_CODEC_TYPE_H265; tVencConfig.pCodecConfig = &g_tH5e_config; break; case VMF_VENC_CODEC_TYPE_MJPG: tVencConfig.eCodecType = VMF_VENC_CODEC_TYPE_MJPG; tVencConfig.pCodecConfig = &g_tJep_config; tVencConfig.dwFps = 10; break; default: print_msg("[%s] Invalid Codec Type\n", __func__); return -1; } VMF_VENC_OUT_SRB_Setup_Config(&tVencConfig, tVencConfig.eCodecType, tVencConfig.pCodecConfig, g_ptVencOutputSRB); tVencConfig.fnSrcConnectFunc = (VMF_SRC_CONNECT_FUNC) VMF_BIND_Request; tVencConfig.pBind = g_ptBind; g_ptVencHandle = VMF_VENC_Init(&tVencConfig); if (!g_ptVencHandle) { print_msg("[%s] VMF_VENC_Init() failed\n", __func__); release_video_source(); release_bind(); release_output_srb(); return -1; } /* start the video encoder engine */ VMF_VENC_ProduceStreamHdr(g_ptVencHandle); return 0; } static int Custom_BIND_Request(VMF_BIND_CONTEXT_T* ptContext, unsigned int dwWidth, unsigned int dwHeight, unsigned int dwStride __attribute__((unused)), unsigned int dwFps __attribute__((unused)), VMF_SRC_CONNECT_INFO_T* ptConnectInfo) { if (!ptContext) { return -1; } ptConnectInfo->bConnectIfp = 0; ptConnectInfo->bDisableSharedOsd = 0; ptConnectInfo->bIsSsmShared = 1; ptConnectInfo->bUnregister = 0; ptConnectInfo->bUseResizedSrc = 0; ptConnectInfo->dwCodecType = g_eCodecType; ptConnectInfo->dwDataType = 0; ptConnectInfo->dwSrcWidth = dwWidth; ptConnectInfo->dwSrcHeight = dwHeight; ptConnectInfo->dwSrcYStride = dwWidth; ptConnectInfo->dwSrcUVStride = ptConnectInfo->dwSrcYStride>>1; //memcpy(ptConnectInfo->szSrcPin, READER_PIN, strlen(READER_PIN)); memcpy(ptConnectInfo->szSrcPin, WRITER_PIN, strlen(WRITER_PIN)); return 0; } static int init_thvideo_encoder(VMF_VENC_CODEC_TYPE eCodecType, int dwVideoWidth, int dwVideoHeight) { VMF_VENC_CONFIG_T tVencConfig; memset(&tVencConfig, 0, sizeof(VMF_VENC_CONFIG_T)); tVencConfig.dwEncWidth = dwVideoWidth; tVencConfig.dwEncHeight = dwVideoHeight; switch(eCodecType){ case VMF_VENC_CODEC_TYPE_H264: tVencConfig.eCodecType = VMF_VENC_CODEC_TYPE_H264; tVencConfig.pCodecConfig = &g_tH4e_config; break; case VMF_VENC_CODEC_TYPE_H265: tVencConfig.eCodecType = VMF_VENC_CODEC_TYPE_H265; tVencConfig.pCodecConfig = &g_tH5e_config; break; case VMF_VENC_CODEC_TYPE_MJPG: tVencConfig.eCodecType = VMF_VENC_CODEC_TYPE_MJPG; tVencConfig.pCodecConfig = &g_tJep_config; break; default: print_msg("[%s] Invalid Codec Type\n", __func__); return -1; } tVencConfig.dwFps = THERMAL_FPS; VMF_VENC_OUT_SRB_Setup_Config(&tVencConfig, tVencConfig.eCodecType, tVencConfig.pCodecConfig, g_ptVencThOutputSRB); tVencConfig.fnSrcConnectFunc = (VMF_SRC_CONNECT_FUNC) Custom_BIND_Request; tVencConfig.pBind = g_ptThBind; g_ptVencThHandle = VMF_VENC_Init(&tVencConfig); if (!g_ptVencThHandle) { print_msg("[%s] VMF_VENC_Init() failed\n", __func__); release_thbind(); release_thoutput_srb(); return -1; } /* start the video encoder engine */ VMF_VENC_ProduceStreamHdr(g_ptVencThHandle); return 0; } static void msg_callback(MsgContext* msg_context, void* user_data) { (void) user_data; print_msg("[%s] msg_context->pszHost=%s, msg_context->pszCmd=%s \n", __func__, msg_context->pszHost, msg_context->pszCmd); while(!g_bInitialized) { usleep(1000); } if (!strcasecmp(msg_context->pszHost, "encoder0")) { if (!strcasecmp(msg_context->pszCmd, "start")) { if (++g_dwStreamingNum == 1) { VMF_VENC_Start(g_ptVencHandle); } } else if (!strcasecmp(msg_context->pszCmd, "stop")) { if (g_dwStreamingNum) { if (--g_dwStreamingNum == 0) { VMF_VENC_Stop(g_ptVencHandle); } } } else if (!strcasecmp(msg_context->pszCmd, "forceCI")) { VMF_VENC_ProduceStreamHdr(g_ptVencHandle); } else if (!strcasecmp(msg_context->pszCmd, "forceIntra")) { VMF_VENC_SetIntra(g_ptVencHandle); } } else if (!strcasecmp(msg_context->pszHost, "encoder1")) { if (!strcasecmp(msg_context->pszCmd, "start")) { if (++g_dwStreamingThNum == 1) { VMF_VENC_Start(g_ptVencThHandle); } } else if (!strcasecmp(msg_context->pszCmd, "stop")) { if (g_dwStreamingThNum) { if (--g_dwStreamingThNum == 0) { VMF_VENC_Stop(g_ptVencThHandle); } } } else if (!strcasecmp(msg_context->pszCmd, "forceCI")) { VMF_VENC_ProduceStreamHdr(g_ptVencThHandle); } else if (!strcasecmp(msg_context->pszCmd, "forceIntra")) { VMF_VENC_SetIntra(g_ptVencThHandle); } } else if( !strcasecmp(msg_context->pszHost, SR_MODULE_NAME) ){ if( !strcasecmp(msg_context->pszCmd, SUSPEND_CMD) ) { VMF_VSRC_Suspend(g_ptVsrcHandle); //! suspend VMF_VENC_Suspend(g_ptVencHandle); VMF_VENC_Suspend(g_ptVencThHandle); MsgBroker_SuspendAckMsg(); }else if( !strcasecmp(msg_context->pszCmd, RESUME_CMD) ) { VMF_VSRC_Resume(g_ptVsrcHandle); //! resume VMF_VENC_Resume(g_ptVencHandle); VMF_VENC_Resume(g_ptVencThHandle); } } if (msg_context->bHasResponse) { msg_context->dwDataSize = 0; } } void ssm_clear_header(unsigned char* virt_addr, unsigned int buf_size, void* pUserData) { VMF_VSRC_SSM_OUTPUT_INFO_T* vsrc_ssm_writer_info = (VMF_VSRC_SSM_OUTPUT_INFO_T*) pUserData; if (buf_size > VMF_MAX_SSM_HEADER_SIZE) memset(virt_addr, 0, VMF_MAX_SSM_HEADER_SIZE); VMF_VSRC_SSM_SetInfo(virt_addr, vsrc_ssm_writer_info); } int i2c_write_reg(int slave_2_byte, char *dev, unsigned char *buf, unsigned char slave_address, unsigned int reg_address, int len) { struct i2c_rdwr_ioctl_data work_queue; unsigned char *w_buf = NULL; int ret; int offset; w_buf = (unsigned char *)calloc(1, sizeof(unsigned char)*(len+2)); if(w_buf == NULL){ print_msg("[%s] Allocate memory fail!\n", __func__); return 0; } if(slave_2_byte == 1) { w_buf[0] = (reg_address & 0xFF00) >> 8; w_buf[1] = (reg_address & 0x00FF) >> 0; offset = 2; } else { w_buf[0] = (reg_address & 0x00FF) >> 0; offset = 1; } int fd = open(dev, O_RDWR); if (fd < 0) { print_msg("[%s] Error on opening the device file\n", __func__); if(w_buf){ free(w_buf); w_buf = NULL; } return 0; } work_queue.nmsgs = 1; work_queue.msgs = (struct i2c_msg*)malloc(work_queue.nmsgs *sizeof(struct i2c_msg)); if (!work_queue.msgs) { print_msg("[%s] Memory alloc error\n", __func__); close(fd); if(w_buf){ free(w_buf); w_buf = NULL; } return 0; } ioctl(fd, I2C_TIMEOUT, 2); ioctl(fd, I2C_RETRIES, 1); (work_queue.msgs[0]).len = offset + len; (work_queue.msgs[0]).addr = slave_address; (work_queue.msgs[0]).buf = w_buf; memcpy(w_buf + offset, buf, len); ret = ioctl(fd, I2C_RDWR, (unsigned long) &work_queue); if (ret < 0) { print_msg("[%s] Error during I2C_RDWR ioctl with error code: %d\n", __func__, ret); close(fd); if(w_buf){ free(w_buf); w_buf = NULL; } if(work_queue.msgs){ free(work_queue.msgs); work_queue.msgs = NULL; } return 0; } else { //printf("write salve:%02x reg:%02x\n", slave_address, reg_address); close(fd); if(w_buf){ free(w_buf); w_buf = NULL; } if(work_queue.msgs){ free(work_queue.msgs); work_queue.msgs = NULL; } return len; } } void lepton_reboot_via_i2c() { unsigned char buf[2]; int slave_2_byte = 1; char *dev = strdup(g_ptI2cDevice); unsigned char slave_address = 0x2a; unsigned int reg_address; int value; int len = 2; //i2c /dev/i2c-0 2a 1 0008 0 reg_address = 0x0008; value = 0x0000; buf[1] = (value & 0x00ff) >> 0; buf[0] = (value & 0xff00) >> 8; i2c_write_reg(slave_2_byte, dev, buf, slave_address, reg_address, len); //i2c /dev/i2c-0 2a 1 0004 4842 reg_address = 0x0004; value = 0x4842; buf[1] = (value & 0x00ff) >> 0; buf[0] = (value & 0xff00) >> 8; i2c_write_reg(slave_2_byte, dev, buf, slave_address, reg_address, len); if(dev){ free(dev); dev = NULL; } } int SpiOpenPort( void ) { int status_value = -1; //int *spi_cs_fd; unsigned char spi_mode = SPI_MODE_3; unsigned char spi_bitsPerWord = 8; //----- SET SPI MODE ----- //SPI_MODE_0 (0,0) CPOL=0 (Clock Idle low level), CPHA=0 (SDO transmit/change edge active to idle) //SPI_MODE_1 (0,1) CPOL=0 (Clock Idle low level), CPHA=1 (SDO transmit/change edge idle to active) //SPI_MODE_2 (1,0) CPOL=1 (Clock Idle high level), CPHA=0 (SDO transmit/change edge active to idle) //SPI_MODE_3 (1,1) CPOL=1 (Clock Idle high level), CPHA=1 (SDO transmit/change edge idle to active) spi_mode = SPI_MODE_3; //----- SET BITS PER WORD ----- spi_bitsPerWord = 8; //----- SET SPI BUS SPEED ----- //g_dwSpiSpeed = 17000000; //1000000 = 1MHz (1uS per bit) g_dwSpiFd = open(g_ptThermalDevice, O_RDWR); if (g_dwSpiFd < 0) { printf("Error - Could not open SPI device"); return -1; } status_value = ioctl(g_dwSpiFd, SPI_IOC_WR_MODE, &spi_mode); if(status_value < 0) { printf("Could not set SPIMode (WR)...ioctl fail"); return -1; } status_value = ioctl(g_dwSpiFd, SPI_IOC_RD_MODE, &spi_mode); if(status_value < 0) { printf("Could not set SPIMode (RD)...ioctl fail"); return -1; } status_value = ioctl(g_dwSpiFd, SPI_IOC_WR_BITS_PER_WORD, &spi_bitsPerWord); if(status_value < 0) { printf("Could not set SPI bitsPerWord (WR)...ioctl fail"); return -1; } status_value = ioctl(g_dwSpiFd, SPI_IOC_RD_BITS_PER_WORD, &spi_bitsPerWord); if(status_value < 0) { printf("Could not set SPI bitsPerWord(RD)...ioctl fail"); return -1; } status_value = ioctl(g_dwSpiFd, SPI_IOC_WR_MAX_SPEED_HZ, &g_dwSpiSpeed); if(status_value < 0) { printf("Could not set SPI speed (WR)...ioctl fail"); return -1; } status_value = ioctl(g_dwSpiFd, SPI_IOC_RD_MAX_SPEED_HZ, &g_dwSpiSpeed); if(status_value < 0) { printf("Could not set SPI speed (RD)...ioctl fail"); return -1; } printf("spi_mode %d, SpiSpeed is %d, spi_bitsPerWord %d\n", spi_mode, g_dwSpiSpeed, spi_bitsPerWord); return(status_value); } int SpiClosePort( void ) { int status_value = -1; status_value = close(g_dwSpiFd); if(status_value < 0) { printf("Error - Could not close SPI device"); return -1; } return(status_value); } void *vatics_lepton_thread_lptdrv(void *arg __attribute__((unused))) { int fd = -1; unsigned int i = 0, j = 0, h = 0; unsigned int cur_frame_count = 0; unsigned int fps_frame_count = 0; unsigned int segment_id = 1; unsigned int pre_segment_id = 1; unsigned int az_segment[4] = {0, 0, 0, 0}; unsigned int segment_shift = 0; int good_packet = 0; int fail_frame = 0; int reset_count = 0; int read_return = 0; struct timeval now, prev; struct timeval reset_now, reset_prev; struct timeval timeout; fd_set set; unsigned char achSpiBuff[PACKET_SIZE*READ_PACKET_NUM]; unsigned int dwGetAllSeg = 1; unsigned dwDiffTime = 0; memset(achSpiBuff, 0, sizeof(unsigned char) * PACKET_SIZE * READ_PACKET_NUM); gettimeofday(&prev, NULL); gettimeofday(&reset_prev, NULL); fd = open(g_ptThermalDevice, O_RDWR); if (fd < 0){ print_msg("[%s] can't open device %s", __func__, g_ptThermalDevice); return NULL; } FD_ZERO(&set); /* clear the set */ FD_SET(fd, &set); /* add our file descriptor to the set */ timeout.tv_sec = 1; timeout.tv_usec = 0; while(g_bTerminate == 0) { read_return = select(fd + 1, &set, NULL, NULL, &timeout); if(read_return == -1){ print_msg("select error\n"); // an error accured usleep(3000); continue; } else if(read_return == 0) { print_msg("read timeout\n"); // a timeout occured continue; } else { read_return = read(fd, achSpiBuff, 0); // there was data to read } if(read_return != (int)(PACKET_SIZE*READ_PACKET_NUM) ){ print_msg("[%s] read buffer size Fail, ret: %d\n", __func__, read_return); usleep(3000); continue; } if(g_dwLeptonType != 3){ if(cur_frame_count % 3 != 0){ cur_frame_count++; usleep(3000); continue; } } good_packet = 0; for(i = 0 ; i < READ_PACKET_NUM ; i++) { if(achSpiBuff[i*PACKET_SIZE+1] == i) { good_packet++; } else { break; } } if (good_packet == READ_PACKET_NUM) { cur_frame_count++; fail_frame = 0; reset_count = 0; segment_id = (achSpiBuff[20*PACKET_SIZE]&0xF0)>>4; } else { fail_frame++; //if(fail_frame >= 750){ if(fail_frame >= 50){ // sometimes the device is always send fail frame, and it can't recover from calling reset // need to ask lepton how to reset exactly or make reset from outer circuit //usleep(750000); usleep(200000); fail_frame = 0; reset_count++; print_msg("reset_count: %d\n", reset_count); if(reset_count >= 5){ gettimeofday(&reset_now, NULL); print_msg("[%s] Reset lepton via i2c, Period: %d s !!!\n", __func__, reset_now.tv_sec-reset_prev.tv_sec); reset_count = 0; lepton_reboot_via_i2c(); if(g_dwResetSleepType){ // sleep 4 sec print_msg("Sleep 4 sec\n"); for(int k = 0 ; k < 20 ; k++){ g_dwCurFrameCount++; usleep(200000); } } else { print_msg("Sleep 750 ms\n"); usleep(750000); } reset_prev = reset_now; } //} else { // if(fail_frame % 10 == 0) { // print_msg("Fail Frame: %d\n", fail_frame); // } } continue; } if(g_dwLeptonType == 3){ switch(segment_id) { case 1: az_segment[0] = 1; pre_segment_id = segment_id; break; case 2: case 3: case 4: if(pre_segment_id == segment_id-1){ az_segment[segment_id-1] = 1; pre_segment_id = segment_id; } break; default: usleep(3000); continue; } h = 0; segment_shift = (segment_id-1)*( (g_dwThermalWidth * g_dwThermalHeight) >> 2); pthread_mutex_lock(&g_tThemalDataMutex); for(j = 0 ; j < READ_PACKET_NUM ; j++) { for(i = 4 ; i < PACKET_SIZE ; i = i + 2 ){ g_adwThermalValue[h+segment_shift] = (unsigned short)(achSpiBuff[(j*PACKET_SIZE)+i] << 8) + achSpiBuff[(j*PACKET_SIZE)+i+1]; h++; } } pthread_mutex_unlock(&g_tThemalDataMutex); dwGetAllSeg = 1; for(int j = 0; j < 4 ; j++){ if(az_segment[j] == 0){ dwGetAllSeg = 0; break; } } if(dwGetAllSeg){ memset(&az_segment, 0, sizeof(unsigned int)*4); if(g_dwCurFrameCount % 100 == 99){ gettimeofday(&now, NULL); dwDiffTime = (now.tv_sec * 1000000 + now.tv_usec) - (prev.tv_sec * 1000000 + prev.tv_usec); print_msg("Segment numbers: %d, Thermal fps:\t%.2f\n", cur_frame_count - fps_frame_count, (float)1000/((float)dwDiffTime/100000) ); fps_frame_count = cur_frame_count; prev = now; } g_dwCurFrameCount++; } } else { if(g_dwCurFrameCount % 100 == 99){ gettimeofday(&now, NULL); dwDiffTime = (now.tv_sec * 1000000 + now.tv_usec) - (prev.tv_sec * 1000000 + prev.tv_usec); print_msg("frames: %d, Thermal fps:\t%.2f\n", cur_frame_count - fps_frame_count, (float)1000/((float)dwDiffTime/100000) ); fps_frame_count = cur_frame_count; prev = now; } h = 0; pthread_mutex_lock(&g_tThemalDataMutex); for(j = 0 ; j < g_dwThermalHeight ; j++) { for(i = 4 ; i < PACKET_SIZE ; i = i + 2 ){ g_adwThermalValue[h] = (unsigned short)(achSpiBuff[(j*PACKET_SIZE)+i] << 8) + achSpiBuff[(j*PACKET_SIZE)+i+1]; h++; } } pthread_mutex_unlock(&g_tThemalDataMutex); g_dwCurFrameCount++; } } close(fd); return NULL; } int lepton_Init( void ){ int ret = 0; ret = SpiOpenPort(); if (ret == -1) { printf("Open SPI Port Error\n"); return -1; } return 0; } void * vatics_lepton_thread_spidev(void *arg __attribute__((unused))) { unsigned int i = 0, j = 0, h = 0; unsigned int iNFStartCopy = 0, iNFCopySize = 0; int iNFStartPKID = -99; int iNFCurPKID = 0; unsigned int cur_frame_count = 0; unsigned int fps_frame_count = 0; struct timeval now = {0 , 0}, prev = {0 , 0}; unsigned int segment_id = 1; unsigned int pre_segment_id = 1; unsigned int az_segment[4] = {0, 0, 0, 0}; unsigned int segment_shift = 0; unsigned int dwGetAllSeg = 1; unsigned dwDiffTime = 0; unsigned char achSpiBuffTemp[PACKET_SIZE*READ_PACKET_NUM]; unsigned char achSpiBuffCurr[PACKET_SIZE*READ_PACKET_NUM]; unsigned char achSpiBuffNext[PACKET_SIZE*READ_PACKET_NUM]; if(lepton_Init() != 0){ print_msg("[%s] Err: lepton init ERROR\n", __func__); return NULL; } while(g_bTerminate == 0) { //read data packets from lepton over SPI int resets = 0; h = 0; int isGetAllPacket = 0; int iStartPKID = -99; int iCurPKID = 0; unsigned int iStartCopy = 0, iCopySize = 0; memset(achSpiBuffTemp, 0, sizeof(unsigned char) * PACKET_SIZE * READ_PACKET_NUM); memset(achSpiBuffCurr, 0, sizeof(unsigned char) * PACKET_SIZE * READ_PACKET_NUM); if(iNFStartCopy > 0){ // copy data of next frame to cur frame iStartPKID = iNFCopySize; iCurPKID = iNFCopySize - 1; memcpy(achSpiBuffCurr, achSpiBuffNext, PACKET_SIZE*(iNFCopySize)); } memset(achSpiBuffNext, 0, sizeof(unsigned char) * PACKET_SIZE * READ_PACKET_NUM); iNFStartCopy = 0, iNFCopySize = 0; iNFStartPKID = -99; iNFCurPKID = 0; while (!isGetAllPacket) { int iGetDiscard = 0; int iNFGetDiscard = 0; int iGetError = 0; unsigned int chkidx = 0; int ret = read(g_dwSpiFd, achSpiBuffTemp, sizeof(unsigned char)*PACKET_SIZE * READ_PACKET_NUM); if(ret != PACKET_SIZE * READ_PACKET_NUM ){ printf("SPI Read Error, ret:%d\n", ret); break; } iStartCopy = 0, iCopySize = 0; for(chkidx = 0; chkidx < READ_PACKET_NUM ; chkidx++){ unsigned char *pchPacketID = achSpiBuffTemp + chkidx * PACKET_SIZE; int packetID = (int)pchPacketID[1]; if(isGetAllPacket == 1){ // Get Next frame if (packetID == 0 ) { // id = 0, First packet iNFStartCopy = chkidx; iNFStartPKID = (0-chkidx); iNFCurPKID = 0; iNFCopySize = 1; } else if (packetID == iNFCurPKID + 1) { // id = 1 ~ 59 packet, must follow the sequence iNFCopySize++; if(packetID != READ_PACKET_NUM - 1){ iNFCurPKID++; } } else { if( ((pchPacketID[0] & 0x0F)== 0x0F) && ((pchPacketID[1] & 0xF0)== 0xF0) ){ // buf[0][1] = xFFx, discard packet iNFGetDiscard++; } else { // not follow the id sequence, restart get the first packet and get error, jump the loop iNFCurPKID = 0; iNFCopySize = 0; break; } } } else { // Get First frame if (packetID == 0 ) { // id = 0, First packet iStartCopy = chkidx; iStartPKID = (0-chkidx); iCurPKID = 0; iCopySize = 1; } else if (packetID == iCurPKID + 1) { // id = 1 ~ 59 packet, must follow the sequence if(packetID == READ_PACKET_NUM - 1){ // id = 59 packet, get all packet(one frame) if(isGetAllPacket == 0){ isGetAllPacket = 1; iCopySize++; } } else { iCopySize++; iCurPKID++; } } else { if( ((pchPacketID[0] & 0x0F)== 0x0F) && ((pchPacketID[1] & 0xF0)== 0xF0) ){ // buf[0][1] = xFFx, discard packet iGetDiscard++; } else { // not follow the id sequence, restart get the first packet and get error, jump the loop iGetError = 1; iCurPKID = 0; iStartPKID = -99; break; } } } } if(iGetDiscard == READ_PACKET_NUM || iGetError){ // N packets are all discard packets, sleep resets += 1; usleep(1000); if(resets % 50 == 0){ printf("[%s:%u] iGetDiscard: %u !!!\n", __func__, __LINE__, resets); usleep(200000); } //Note: we've selected 750 resets as an arbitrary limit, since there should never be 750 "null" packets between two valid transmissions at the current poll rate //By polling faster, developers may easily exceed this count, and the down period between frames may then be flagged as a loss of sync if(resets >= 750) { lepton_reboot_via_i2c(); //usleep(750000); usleep(3000000); printf("[%s:%u] Reset !!!\n", __func__, __LINE__); resets = 0; } } else { // get data packets, copy to result if (iStartPKID < 0){ iStartPKID = 0; } if(iStartPKID + iCopySize <= READ_PACKET_NUM){ // id must on 0 ~ 59 memcpy(achSpiBuffCurr+(iStartPKID*PACKET_SIZE), achSpiBuffTemp+(iStartCopy*PACKET_SIZE), PACKET_SIZE*(iCopySize)); iStartPKID = iStartPKID + iCopySize; } if(iNFCopySize > 0){ if (iNFStartPKID < 0){ iNFStartPKID = 0; } memcpy(achSpiBuffNext+(iNFStartPKID*PACKET_SIZE), achSpiBuffTemp+(iNFStartCopy*PACKET_SIZE), PACKET_SIZE*(iNFCopySize)); iNFStartPKID = iNFStartPKID + iNFCopySize; } } } cur_frame_count++; if(g_dwLeptonType == 3){ // Lepton 3.5 segment_id = (achSpiBuffCurr[20*PACKET_SIZE]&0xF0)>>4; switch(segment_id) { case 1: az_segment[0] = 1; pre_segment_id = segment_id; break; case 2: case 3: case 4: if(pre_segment_id == segment_id-1){ az_segment[segment_id-1] = 1; pre_segment_id = segment_id; } if(segment_id == 4) usleep(2000); break; default: usleep(1000); continue; } h = 0; segment_shift = (segment_id-1)*( (g_dwThermalWidth * g_dwThermalHeight) >> 2); pthread_mutex_lock(&g_tThemalDataMutex); for(j = 0 ; j < READ_PACKET_NUM ; j++) { for(i = 4 ; i < PACKET_SIZE ; i = i + 2 ){ g_adwThermalValue[h+segment_shift] = (unsigned short)(achSpiBuffCurr[(j*PACKET_SIZE)+i] << 8) + achSpiBuffCurr[(j*PACKET_SIZE)+i+1]; h++; } } pthread_mutex_unlock(&g_tThemalDataMutex); dwGetAllSeg = 1; for(int j = 0; j < 4 ; j++){ if(az_segment[j] == 0){ dwGetAllSeg = 0; break; } } if(dwGetAllSeg){ memset(&az_segment, 0, sizeof(unsigned int)*4); if(g_dwCurFrameCount % 100 == 99){ gettimeofday(&now, NULL); if(prev.tv_sec == 0 && prev.tv_usec == 0){ prev = now; } else { dwDiffTime = (now.tv_sec * 1000000 + now.tv_usec) - (prev.tv_sec * 1000000 + prev.tv_usec); print_msg("segments: %d, Thermal fps:\t%.2f\n", cur_frame_count - fps_frame_count, (float)1000/((float)dwDiffTime/100000) ); fps_frame_count = cur_frame_count; prev = now; } } g_dwCurFrameCount++; } } else { // Lepton 2.5 if(cur_frame_count % 3 != 0){ usleep(3000); continue; } if(g_dwCurFrameCount % 100 == 99){ gettimeofday(&now, NULL); if(prev.tv_sec == 0 && prev.tv_usec == 0){ prev = now; } else { dwDiffTime = (now.tv_sec * 1000000 + now.tv_usec) - (prev.tv_sec * 1000000 + prev.tv_usec); print_msg("CurFrames: %d, Thermal fps:\t%.2f\n", g_dwCurFrameCount, (float)1000/((float)dwDiffTime/100000) ); fps_frame_count = cur_frame_count; prev = now; } } h = 0; pthread_mutex_lock(&g_tThemalDataMutex); for(j = 0 ; j < g_dwThermalHeight ; j++) { for(i = 4 ; i < PACKET_SIZE ; i = i + 2 ){ g_adwThermalValue[h] = (unsigned short)(achSpiBuffCurr[(j*PACKET_SIZE)+i] << 8) + achSpiBuffCurr[(j*PACKET_SIZE)+i+1]; h++; } } pthread_mutex_unlock(&g_tThemalDataMutex); g_dwCurFrameCount++; } } if(SpiClosePort() != 0){ print_msg("[%s] Err: Cannot close SPI device.\n", __func__); } printf("[%s] exit thread\n", __func__); return NULL; } void *thermal_loop (void *arg __attribute__((unused))) { int iMinValue = -1, iMaxValue = -1; float fValueStep = 0; struct timeval tNowFrameTime, tPreFrameTime = {0, 0}; unsigned int dwSize = VMF_32_ALIGN(VENC_ENCODE_WIDTH)*VMF_16_ALIGN(VENC_ENCODE_HEIGHT); //! init writer SSM_WRITER_INIT_OPTION_T tSsmWriterInit; SSM_HANDLE_T *ptSsmWriterHandle = NULL; SSM_BUFFER_T tOutWriterSsmBuffer; VMF_VIDEO_BUF_T tRsFrameInfo; VMF_VIDEO_BUF_T tRsOutbuf; VMF_VSRC_SSM_OUTPUT_INFO_T tSsmWriterOutInfo; static unsigned char* pBrsRsFrame = NULL; static unsigned char* pOutRsFrame = NULL; int iDiffTime = 0, iSleepTime = 0; struct timespec tFrameTime; unsigned short *ptTermalData = NULL; #ifdef DEBUG_WRITE_YUV_FILE unsigned int iCountTmp = 0; #endif memset(&tOutWriterSsmBuffer, 0, sizeof(SSM_BUFFER_T)); memset(&tSsmWriterInit, 0, sizeof(SSM_WRITER_INIT_OPTION_T)); memset(&tRsFrameInfo, 0, sizeof(VMF_VIDEO_BUF_T)); memset(&tRsOutbuf, 0, sizeof(VMF_VIDEO_BUF_T)); tSsmWriterOutInfo.dwYStride = VMF_32_ALIGN(VENC_ENCODE_WIDTH); tSsmWriterOutInfo.dwYSize = tSsmWriterOutInfo.dwYStride * VENC_ENCODE_HEIGHT; tSsmWriterOutInfo.dwUVSize = tSsmWriterOutInfo.dwYSize >> 2; tSsmWriterOutInfo.dwOffset[0] = VMF_MAX_SSM_HEADER_SIZE; tSsmWriterOutInfo.dwOffset[1] = tSsmWriterOutInfo.dwOffset[0] + tSsmWriterOutInfo.dwYSize; tSsmWriterOutInfo.dwOffset[2] = tSsmWriterOutInfo.dwOffset[1] + tSsmWriterOutInfo.dwUVSize; tSsmWriterOutInfo.dwWidth = VMF_32_ALIGN(VENC_ENCODE_WIDTH); tSsmWriterOutInfo.dwHeight = VMF_16_ALIGN(VENC_ENCODE_HEIGHT); tSsmWriterInit.name = WRITER_PIN; tSsmWriterInit.buf_size = ((dwSize*3)>>1) + VMF_MAX_SSM_HEADER_SIZE; tSsmWriterInit.alignment = VMF_ALIGN_TYPE_DEFAULT; tSsmWriterInit.pshared = 1; tSsmWriterInit.pUserData = &tSsmWriterOutInfo; tSsmWriterInit.fp_setup_buffer = ssm_clear_header; ptSsmWriterHandle = SSM_Writer_Init(&tSsmWriterInit); if (!ptSsmWriterHandle) { print_msg("%s() failed, SSM_Writer_Init failed!\n", __func__); goto RELEASE; } SSM_Writer_SendGetBuff(ptSsmWriterHandle, &tOutWriterSsmBuffer); pBrsRsFrame = (unsigned char *)MemBroker_GetMemory(g_dwThermalWidth * g_dwThermalHeight * 3 >> 1, VMF_ALIGN_TYPE_DEFAULT); if (!pBrsRsFrame) { print_msg("[%s] Allocate resize output frame buffer fail !!\n",__func__); goto RELEASE; } // memset(pBrsRsFrame, 0, sizeof(unsigned char)*g_dwThermalWidth * g_dwThermalHeight * 3 >> 1); pOutRsFrame = (unsigned char *)MemBroker_GetMemory(VENC_ENCODE_WIDTH * VENC_ENCODE_HEIGHT * 3 >> 1, VMF_ALIGN_TYPE_DEFAULT); if (!pOutRsFrame) { print_msg("[%s] Allocate resize output frame buffer fail !!\n",__func__); goto RELEASE; } // memset(pOutRsFrame, 0, sizeof(unsigned char)*VENC_ENCODE_WIDTH * VENC_ENCODE_HEIGHT * 3 >> 1); while(g_bTerminate == 0){ unsigned short Y, U, V; int iOffset = 0; unsigned int i = 0; unsigned int iBrsYOffset = 0; //VMF_MAX_SSM_HEADER_SIZE; unsigned int iBrsUOffset = iBrsYOffset + (g_dwThermalWidth*g_dwThermalHeight); unsigned int iBrsVOffset = iBrsUOffset + (iBrsUOffset >> 2); if( g_dwPreFrameCount != g_dwCurFrameCount){ ptTermalData = g_adwThermalValue; g_dwPreFrameCount = g_dwCurFrameCount; } else { usleep(3000); continue; } memset(pBrsRsFrame, 0, sizeof(unsigned char)*g_dwThermalWidth * g_dwThermalHeight * 3 >> 1); iMinValue = ptTermalData[0]; iMaxValue = ptTermalData[0]; for(unsigned int i = 0 ; i < g_dwThermalWidth*g_dwThermalHeight ; i++){ if(ptTermalData[i] < iMinValue && ptTermalData[i] != 0 ){ iMinValue = ptTermalData[i]; } if(ptTermalData[i] > iMaxValue){ iMaxValue = ptTermalData[i]; } } if( (iMaxValue - iMinValue) > COLOR_STEPS){ fValueStep = (float)(iMaxValue - iMinValue) / COLOR_STEPS; } else { fValueStep = 1; } tSsmWriterOutInfo.dwYStride = VMF_32_ALIGN(VENC_ENCODE_WIDTH); tSsmWriterOutInfo.dwYSize = tSsmWriterOutInfo.dwYStride * VENC_ENCODE_HEIGHT; tSsmWriterOutInfo.dwUVSize = tSsmWriterOutInfo.dwYSize >> 2; tSsmWriterOutInfo.dwOffset[0] = VMF_MAX_SSM_HEADER_SIZE; tSsmWriterOutInfo.dwOffset[1] = tSsmWriterOutInfo.dwOffset[0] + tSsmWriterOutInfo.dwYSize; tSsmWriterOutInfo.dwOffset[2] = tSsmWriterOutInfo.dwOffset[1] + tSsmWriterOutInfo.dwUVSize; i = 0; for(unsigned int h = 0 ; h < g_dwThermalHeight ; h++){ for(unsigned int w = 0 ; w < g_dwThermalWidth ; w++){ int iDiff = 0; iDiff = (ptTermalData[i] - iMinValue) / fValueStep; if(iDiff > COLOR_STEPS - 1){ iDiff = COLOR_STEPS - 1; } Y = SPECTRAL_Y[iDiff]; U = SPECTRAL_U[iDiff]; V = SPECTRAL_V[iDiff]; pBrsRsFrame[iBrsYOffset + i] = Y; if(h % 2 == 0){ if(i % 2 == 0) { pBrsRsFrame[iBrsUOffset + iOffset] = U; pBrsRsFrame[iBrsVOffset + iOffset] = V; iOffset += 1; } } i++; } } MemBroker_CacheCopyBack(pBrsRsFrame, sizeof(unsigned char)*g_dwThermalWidth * g_dwThermalHeight * 3 >> 1); tRsFrameInfo.apbyVirtAddr[0] = pBrsRsFrame; tRsFrameInfo.apbyVirtAddr[1] = tRsFrameInfo.apbyVirtAddr[0] + (g_dwThermalWidth * g_dwThermalHeight); tRsFrameInfo.apbyVirtAddr[2] = tRsFrameInfo.apbyVirtAddr[1] + ((g_dwThermalWidth * g_dwThermalHeight) >> 2); tRsFrameInfo.apbyPhysAddr[0] = (unsigned char *)MemBroker_GetPhysAddr((unsigned char *)tRsFrameInfo.apbyVirtAddr[0]); tRsFrameInfo.apbyPhysAddr[1] = (unsigned char *)MemBroker_GetPhysAddr((unsigned char *)tRsFrameInfo.apbyVirtAddr[1]); tRsFrameInfo.apbyPhysAddr[2] = (unsigned char *)MemBroker_GetPhysAddr((unsigned char *)tRsFrameInfo.apbyVirtAddr[2]); if (tRsFrameInfo.apbyVirtAddr[0] != 0) { tRsOutbuf.apbyVirtAddr[0] = pOutRsFrame; tRsOutbuf.apbyVirtAddr[1] = tRsOutbuf.apbyVirtAddr[0] + (VENC_ENCODE_WIDTH * VENC_ENCODE_HEIGHT); tRsOutbuf.apbyVirtAddr[2] = tRsOutbuf.apbyVirtAddr[1] + ((VENC_ENCODE_WIDTH * VENC_ENCODE_HEIGHT) >> 2); tRsOutbuf.apbyPhysAddr[0] = (unsigned char *)MemBroker_GetPhysAddr(tRsOutbuf.apbyVirtAddr[0]); tRsOutbuf.apbyPhysAddr[1] = (unsigned char *)MemBroker_GetPhysAddr(tRsOutbuf.apbyVirtAddr[1]); tRsOutbuf.apbyPhysAddr[2] = (unsigned char *)MemBroker_GetPhysAddr(tRsOutbuf.apbyVirtAddr[2]); VMF_RS_ProcessOneFrame(g_ptResizeHandle, &tRsOutbuf, &tRsFrameInfo); } memcpy(tOutWriterSsmBuffer.buffer+VMF_MAX_SSM_HEADER_SIZE, pOutRsFrame, sizeof(unsigned char)*(VENC_ENCODE_WIDTH * VENC_ENCODE_HEIGHT * 3) >> 1); MemBroker_CacheCopyBack(tOutWriterSsmBuffer.buffer, sizeof(unsigned char)*tSsmWriterInit.buf_size); #ifdef DEBUG_WRITE_YUV_FILE iCountTmp++; if(iCountTmp >= 600){ iCountTmp = 0; char path[128] = {0}; char isp_output_format[128] = {0}; strcpy(isp_output_format, "out_rs_%dx%d_420.yuv"); FILE *fp = NULL; FILE *brs_fp = NULL; sprintf(path, isp_output_format, VENC_ENCODE_WIDTH, VENC_ENCODE_HEIGHT); print_msg("[%s] write yuv file : %s ...\n", __func__, path); int ys = VENC_ENCODE_WIDTH * VENC_ENCODE_HEIGHT; int uv = ys >> 2; fp = fopen(path, "wb"); if (fp) { fwrite(pOutRsFrame, 1, (ys * 3) >> 1, fp); fclose(fp); } else { print_msg("[%s] open %s fail\n", __func__, path); } sprintf(path, "brs_rs_%dx%d_420.yuv", g_dwThermalWidth, g_dwThermalHeight); ys = g_dwThermalWidth * g_dwThermalHeight; uv = ys >> 2; brs_fp = fopen(path, "wb"); if (brs_fp) { fwrite(pBrsRsFrame, 1, ys * 3 >> 1, brs_fp); fclose(brs_fp); } else { print_msg("[%s] open %s fail\n", __func__, path); } } #endif gettimeofday(&tNowFrameTime, NULL); if(tPreFrameTime.tv_sec != 0){ iDiffTime = (tNowFrameTime.tv_sec - tPreFrameTime.tv_sec) * 1000000 + (tNowFrameTime.tv_usec - tPreFrameTime.tv_usec); iSleepTime = (1000000/THERMAL_FPS) - iDiffTime; if(iSleepTime > 0) { usleep(iSleepTime); } } tPreFrameTime = tNowFrameTime; VMF_FRAME_INFO_T* black_frame_info = (VMF_FRAME_INFO_T*) tOutWriterSsmBuffer.buffer; clock_gettime(CLOCK_MONOTONIC_RAW, &tFrameTime); unsigned int time_gap = (1000000/THERMAL_FPS)*2; if((unsigned int)tFrameTime.tv_nsec/1000 < time_gap) { black_frame_info->dwSec = (unsigned int)tFrameTime.tv_sec - 1; black_frame_info->dwUSec = (unsigned int)(tFrameTime.tv_nsec/1000) + 1000000 - time_gap; } else { black_frame_info->dwSec = (unsigned int)tFrameTime.tv_sec; black_frame_info->dwUSec = (unsigned int)(tFrameTime.tv_nsec/1000) - time_gap; } MemBroker_CacheCopyBack(tOutWriterSsmBuffer.buffer, sizeof(unsigned char)*tSsmWriterInit.buf_size); SSM_Writer_SendGetBuff(ptSsmWriterHandle, &tOutWriterSsmBuffer); } RELEASE: if (pBrsRsFrame){ MemBroker_FreeMemory(pBrsRsFrame); pBrsRsFrame = NULL; } if (pOutRsFrame){ MemBroker_FreeMemory(pOutRsFrame); pOutRsFrame = NULL; } if (ptSsmWriterHandle) { SSM_Release(ptSsmWriterHandle); ptSsmWriterHandle = NULL; } return NULL; } static void sig_kill(int signo) { print_msg("[%s] receive SIGNAL: %d\n",__func__, signo); g_bTerminate = 1; } int main(int argc, char* argv[]) { int ch, ret = 0; pthread_attr_t attr; struct sched_param param; pthread_t lept_pid; pthread_t thermal_pid; char *pStrstrRet = NULL; VMF_VENC_CODEC_TYPE eCodecType = VMF_VENC_CODEC_TYPE_H264; while ((ch = getopt(argc, argv, "c:d:i:s:C:t:r:p:a:")) != -1) { switch(ch) { case 'c': g_szSensorConfig = strdup(optarg); break; case 'd': g_szSensorConfigFusion= strdup(optarg); break; case 'i': g_ptI2cDevice = strdup(optarg); break; case 's': g_ptThermalDevice = strdup(optarg); break; case 'C': eCodecType = (VMF_VENC_CODEC_TYPE) atoi(optarg); break; case 't': g_dwLeptonType = atoi(optarg); break; case 'r': g_dwResetSleepType = atoi(optarg); break; case 'p': g_dwSpiSpeed = atoi(optarg); break; case 'a': g_szAutoSceneConfig = strdup(optarg); break; default: print_msg("Usage: %s [-c] [-d] [-i]\n" "\t [-s] [-C] [-a autosecne_config_file] \n" "\t [-r ] [-t Lepton type(2: Lepton2.5, 3:Lepton 3.5)]\r\n", argv[0]); goto FAILURE; } } g_eCodecType = eCodecType; if(g_ptI2cDevice == NULL){ g_ptI2cDevice = strdup(THERMAL_I2C_DEVICE); } if(g_ptThermalDevice == NULL){ g_ptThermalDevice = strdup(THERMAL_DEVICE); } print_msg("Thermal Device: %s, I2C Device: %s\n", g_ptThermalDevice, g_ptI2cDevice); /* check sensor config */ if (!g_szSensorConfig) { print_msg("[%s] Err: no sensor config\n", __func__); goto FAILURE; } if(g_dwLeptonType == 3){ g_dwThermalWidth = 160; g_dwThermalHeight = 120; } else { g_dwThermalWidth = 80; g_dwThermalHeight = 60; } g_adwThermalValue = (unsigned short *)calloc(1, sizeof(unsigned short) * g_dwThermalWidth * g_dwThermalHeight); if(g_adwThermalValue == NULL){ print_msg("[%s] allocate lepton buffer faild \n", __func__); goto FAILURE; } /* initialized with default attributes */ pthread_attr_init(&attr); /* safe to get existing scheduling param */ pthread_attr_getschedparam (&attr, ¶m); /* set the police and the priority */ pthread_attr_setschedpolicy(&attr, SCHED_RR); param.sched_priority = 50; /* setting the new scheduling param */ pthread_attr_setschedparam(&attr, ¶m); /* it make the new attr working.*/ pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); pthread_mutex_init(&g_tThemalDataMutex, NULL); pStrstrRet = strstr(g_ptThermalDevice, "spidev"); if (pStrstrRet) { print_msg("[%s] use spi device.\n", __func__); if (0 != pthread_create(&lept_pid, &attr, vatics_lepton_thread_spidev, NULL)) { print_msg("[%s] create lepton spidev thread faild \n", __func__); goto FAILURE; } else { pthread_setname_np(lept_pid, "lepton_spi"); } } else { print_msg("[%s] use lepton driver.\n", __func__); if (0 != pthread_create(&lept_pid, &attr, vatics_lepton_thread_lptdrv, NULL)) { print_msg("[%s] create lepton driver thread faild \n", __func__); goto FAILURE; } else { pthread_setname_np(lept_pid, "lepton_drv"); } } //! init thread if (0 != pthread_create(&thermal_pid, NULL, thermal_loop, NULL)) { print_msg("[%s] create thermal thread faild\n", __func__); goto FAILURE; } else { /* set video source thread name */ pthread_setname_np(thermal_pid, "thermal"); } /* register signal */ signal(SIGTERM, sig_kill); signal(SIGINT, sig_kill); if (init_video_source(eCodecType)) { goto FAILURE; } /* initializing the binder associated with the video source */ if (init_bind()) { goto FAILURE; } /* initializing the binder associated with the video source */ if (init_thbind()) { goto FAILURE; } /* initialize the SRB for primary encoder output buffer */ if (init_output_srb(VENC_OUTPUT_PIN, VENC_OUTPUT_BUF_NUM, VENC_ENCODE_BUF_SIZE)) { goto FAILURE; } if (init_thoutput_srb(VENC_TH_OUTPUT_PIN, VENC_OUTPUT_BUF_NUM, VENC_TH_ENCODE_BUF_SIZE)) { goto FAILURE; } if (init_video_encoder(eCodecType, g_tLayout.dwVideoWidth, g_tLayout.dwVideoHeight)) { goto FAILURE; } if (init_thvideo_encoder(eCodecType, VENC_ENCODE_WIDTH, VENC_ENCODE_HEIGHT)) { goto FAILURE; } ret = init_resize_hanle(); if (ret) { print_msg("[%s] Initial resize handle failed !!\n", __func__); goto FAILURE; } g_bInitialized = 1; MsgBroker_RegisterMsg(VENC_CMD_FIFO); MsgBroker_Run(VENC_CMD_FIFO, msg_callback, NULL, &g_bTerminate); MsgBroker_UnRegisterMsg(); if(pthread_join(thermal_pid, NULL)) { print_msg("[%s] Thermal thread join failed !!\n", __func__); goto FAILURE; } if(pthread_join(lept_pid, NULL)) { print_msg("[%s] Lepton SPI thread join failed !!\n", __func__); goto FAILURE; } FAILURE: if (g_ptResizeHandle){ VMF_RS_Release(g_ptResizeHandle); } release_video_encoder(); release_thvideo_encoder(); release_output_srb(); release_thoutput_srb(); release_bind(); release_thbind(); release_video_source(); if(g_szAutoSceneConfig){ free(g_szAutoSceneConfig); g_szAutoSceneConfig = NULL; } if(g_szSensorConfig){ free(g_szSensorConfig); g_szSensorConfig = NULL; } if(g_szSensorConfigFusion){ free(g_szSensorConfigFusion); g_szSensorConfigFusion = NULL; } if(g_ptI2cDevice){ free(g_ptI2cDevice); g_ptI2cDevice = NULL; } if(g_ptThermalDevice){ free(g_ptThermalDevice); g_ptThermalDevice = NULL; } if (g_adwThermalValue){ free(g_adwThermalValue); g_adwThermalValue = NULL; } if(pStrstrRet){ free(pStrstrRet); pStrstrRet = NULL; } print_msg("[%s] terminated successfully!\n", __func__); return 0; }