1821 lines
53 KiB
C++
1821 lines
53 KiB
C++
/*
|
|
*******************************************************************************
|
|
* 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 <stdlib.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <signal.h>
|
|
#include <fcntl.h>
|
|
#include <errno.h>
|
|
#include <getopt.h>
|
|
#include <unistd.h>
|
|
#include <stdarg.h>
|
|
#include <sys/stat.h>
|
|
#include <pthread.h>
|
|
#include <sys/ioctl.h>
|
|
#include <sys/time.h>
|
|
#include <linux/spi/spidev.h>
|
|
#include <MemBroker/mem_broker.h>
|
|
#include <MsgBroker/msg_broker.h>
|
|
#include <vmf/sync_shared_memory.h>
|
|
#include <vmf/video_encoder_output_srb.h>
|
|
#include <vmf/video_source.h>
|
|
#include <vmf/video_encoder.h>
|
|
#include <vmf/video_bind.h>
|
|
#include <vmf/resize.h>
|
|
#include <comm/frame_info.h>
|
|
#include <vmf/ssm_info.h>
|
|
|
|
#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<sensor_config_file>] [-d<fusion_sensor_config_file>] [-i<I2C device name, default:/dev/i2c-0>]\n"
|
|
"\t [-s<Lepton SPI device name, default:/dev/lepton3.0>] [-C<codec_type>] [-a autosecne_config_file] \n"
|
|
"\t [-r <reset sleep time(0:750 ms, 1: 4 sec)>] [-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;
|
|
}
|