lai/gk-sdk
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
master
gk-sdk/sample/msensor/sample_msensor.c
T

1406 lines
40 KiB
C
Raw Permalink Blame History

/*
* Copyright (c) XMEDIA. All rights reserved.
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <linux/spi/spidev.h>
#include <pthread.h>
#include <signal.h>
#include "xmedia_msensor.h"
#include "xmedia_sys.h"
#include "xmedia_vi.h"
#include "xmedia_venc.h"
#include "xmedia_mmz.h"
#include "sample_comm.h"
#include "sample_comm_sys.h"
#include "sample_comm_vi.h"
#include "sample_comm_vpss.h"
#include "sample_comm_vgs.h"
#include "sample_comm_venc.h"
#include "sample_comm_isp.h"
#include "sample_msensor.h"
#define NUMS_IN_BUF 4000 /* max numbers of gyro data or acc data in buffer */
#define GYRO_BUF_SIZE (sizeof(xmedia_msensor_sample_data) * NUMS_IN_BUF)
#define ACC_BUF_SIZE (sizeof(xmedia_msensor_sample_data) * NUMS_IN_BUF)
#define GYRO_DATA_RECORDER_RANGE 1000
#define GYRO_DATA_DV_RANGE 1000
#define GYRO_DATA_ODR 800
#define ACC_DATA_RANGE 16
#define ACC_DATA_ODR 800
#define INT_MAX 0x7fffffff /* max value for an int */
#define GYRO_BIN_FILE "./gyro_data.bin"
#define GYRO_TXT_FILE "./gyro_data.txt"
#define GYRO_PARSE_FILE "./gyro_parse_data.txt"
typedef struct msensor_get_data_info_ {
xmedia_bool thread_start;
xmedia_msensor_data_type data_type;
xmedia_u32 time_intval;
xmedia_s32 msensor_dev;
xmedia_s32 venc_chn;
} msensor_get_data_info;
typedef struct msensor_data_info_ {
xmedia_s32 msensor_dev;
xmedia_s32 user_id;
FILE *gyro_bin_fp;
FILE *gyro_txt_fp;
} msensor_data_info;
static xmedia_bool g_force_exit = XMEDIA_FALSE;
xmedia_s32 g_msensor_dev_fd = -1;
xmedia_msensor_buf_attr g_msensor_attr[XMEDIA_MSENSOR_MAX_DEV_NUM] = { 0 };
xmedia_s32 g_gyro_data_range = GYRO_DATA_DV_RANGE;
static pthread_t g_msensor_pid[XMEDIA_MSENSOR_MAX_DEV_NUM];
static msensor_get_data_info g_get_data_info[XMEDIA_MSENSOR_MAX_DEV_NUM] = { 0 };
static sample_comm_sensor_type sample_msensor_sensor_type[MAX_SENSOR_NUM] = {
SENSOR0_TYPE,SENSOR1_TYPE,SENSOR2_TYPE,SENSOR3_TYPE,SENSOR4_TYPE
};
xmedia_void sample_msensor_handle_sig(xmedia_s32 signo)
{
if (SIGINT == signo || SIGTSTP == signo || SIGTERM == signo) {
g_force_exit = XMEDIA_TRUE;
}
}
xmedia_s32 sample_msensor_sys_init(sample_sys_config *sys_config)
{
xmedia_s32 ret = 0;
ret = sample_comm_sys_init(sys_config);
if(ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("sample_comm_sys_init failed!\n");
return ret;
}
//in online-online mode,vi and vpss must be reset at the same time
ret = sample_comm_vi_init();
if(ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("sample_comm_vi_init failed!\n");
return ret;
}
ret = sample_comm_vpss_init();
if(ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("sample_comm_vpss_init failed!\n");
return ret;
}
ret = sample_comm_venc_init();
if(ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("sample_comm_venc_init failed!\n");
return ret;
}
return XMEDIA_SUCCESS;
}
xmedia_void sample_msensor_sys_exit(void)
{
sample_comm_venc_exit();
sample_comm_vpss_exit();
sample_comm_vi_exit();
sample_comm_sys_exit();
}
xmedia_void sample_msensor_usage(xmedia_char* args)
{
printf("Usage1 : %s [index] \n", args);
printf("index:\n");
printf("\t 0) h265 + msensor.\n");
printf("\t 1) parse msensor data.\n");
return;
}
xmedia_u32 h265_encode_sei(xmedia_void *dest_buf, xmedia_void *src_buf, xmedia_u32 in_len_bytes,
xmedia_u32 *out_len_bytes)
{
xmedia_u8 *dest = (xmedia_u8 *)dest_buf;
xmedia_u8 *src = (xmedia_u8 *)src_buf;
xmedia_s32 tmp_len = 0;
xmedia_u32 size;
xmedia_u32 tmp_code;
xmedia_u32 byte_cnt = 0;
xmedia_s32 zero_cnt = 0;
xmedia_s32 k;
size = in_len_bytes;
dest[byte_cnt++] = 0x00;
dest[byte_cnt++] = 0x00;
dest[byte_cnt++] = 0x00;
dest[byte_cnt++] = 0x1;
dest[byte_cnt++] = 0x4e;
dest[byte_cnt++] = 0x1;
dest[byte_cnt++] = 0xF0;
while (size >= 255) {
size -= 255;
dest[byte_cnt++] = 0xFF;
}
tmp_code = size;
dest[byte_cnt++] = tmp_code;
tmp_len = byte_cnt;
if (size == 0) {
zero_cnt = 1;
}
for (k = 0; k < (xmedia_s32)in_len_bytes; k++) {
/* true when 0x000000|0x000001|0x000002|0x000003 */
if ((zero_cnt == 2) && !(src[k] & 0xFC)) {
dest[byte_cnt++] = 0x03;
zero_cnt = 0;
}
dest[byte_cnt++] = src[k];
if (src[k] == 0x00) {
zero_cnt++;
} else {
zero_cnt = 0;
}
}
dest[byte_cnt++] = 0x80;
*out_len_bytes = byte_cnt;
return tmp_len;
}
xmedia_u32 h264_encode_sei(xmedia_void *dest_buf, xmedia_void *src_buf, xmedia_u32 in_len_bytes,
xmedia_u32 *out_len_bytes)
{
xmedia_u8 *dest = (xmedia_u8 *)dest_buf;
xmedia_u8 *src = (xmedia_u8 *)src_buf;
xmedia_s32 tmp_len = 0;
xmedia_u32 size;
xmedia_u32 tmp_code;
xmedia_u32 byte_cnt = 0;
xmedia_s32 zero_cnt = 0;
xmedia_s32 k;
size = in_len_bytes;
dest[byte_cnt++] = 0x00;
dest[byte_cnt++] = 0x00;
dest[byte_cnt++] = 0x00;
dest[byte_cnt++] = 0x1;
dest[byte_cnt++] = 0x6;
dest[byte_cnt++] = 0xF0;
while (size >= 255) {
size -= 255;
dest[byte_cnt++] = 0xFF;
}
tmp_code = size;
dest[byte_cnt++] = tmp_code;
tmp_len = byte_cnt;
if (size == 0) {
zero_cnt = 1;
}
for (k = 0; k < (xmedia_s32)in_len_bytes; k++) {
/* true when 0x000000|0x000001|0x000002|0x000003 */
if ((zero_cnt == 2) && !(src[k] & 0xFC)) {
dest[byte_cnt++] = 0x03;
zero_cnt = 0;
}
dest[byte_cnt++] = src[k];
if (src[k] == 0x00) {
zero_cnt++;
} else {
zero_cnt = 0;
}
}
dest[byte_cnt++] = 0x80;
*out_len_bytes = byte_cnt;
return tmp_len;
}
static void save_gyro_data(xmedia_u8 *data, xmedia_s32 datalen, FILE *pfile)
{
xmedia_s32 x;
xmedia_s32 y;
xmedia_s32 z;
xmedia_s32 temp;
xmedia_u64 pts;
xmedia_u8 * temp_ptr = data;
xmedia_s32 tmplen = 0;
xmedia_char buf[128] = {0};
if (datalen % 24 != 0) {
return ;
}
while (tmplen + 24 <= datalen) {
x = *(xmedia_s32 *)temp_ptr;
temp_ptr += sizeof(xmedia_s32);
y = *(xmedia_s32 *)temp_ptr;
temp_ptr += sizeof(xmedia_s32);
z = *(xmedia_s32 *)temp_ptr;
temp_ptr += sizeof(xmedia_s32);
temp = *(xmedia_s32 *)temp_ptr;
temp_ptr += sizeof(xmedia_s32);
pts = *(xmedia_u64 *)temp_ptr;
temp_ptr += sizeof(xmedia_u64);
tmplen += 24;
sprintf(buf, "%12d,%12d,%12d,%12d,%22llu, \n", x, y, z, temp, pts);
fwrite(buf, strlen(buf), 1, pfile);
}
}
// 查找4字节起始码 0x00000001
int find_4byte_start_code(const xmedia_u8* data, int size, int start_pos)
{
int i;
for (i = start_pos; i < size - 4; i++) {
if (data[i] == 0x00 && data[i+1] == 0x00 &&
data[i+2] == 0x00 && data[i+3] == 0x01) {
return i;
}
}
return -1;
}
// 防竞争字节处理 - 移除0x03字节
// 在H.264/H.265中，0x000000, 0x000001, 0x000002, 0x000003前面会插入0x03防止竞争
xmedia_u8* remove_emulation_prevention_bytes(const xmedia_u8* data, int size, int* new_size)
{
xmedia_u8* output;
int output_index = 0;
int zero_count = 0;
if (size <= 0) {
*new_size = 0;
return NULL;
}
// 分配足够大的缓冲区
output = (xmedia_u8*)malloc(size);
if (!output) {
*new_size = 0;
return NULL;
}
for (int i = 0; i < size; i++) {
if (zero_count == 2 && data[i] == 0x03) {
// 找到防竞争字节0x03，跳过它
zero_count = 0;
continue;
}
if (data[i] == 0x00) {
zero_count++;
} else {
zero_count = 0;
}
output[output_index++] = data[i];
}
*new_size = output_index;
return output;
}
// 解析H.264 SEI NAL单元（带防竞争处理）
int parse_h264_sei_nal(const xmedia_u8* nal_data, int nal_size, FILE *file)
{
int rbsp_size;
xmedia_u8* rbsp_data;
int payload_type = 0;
xmedia_u8 byte;
int pos = 1; // 跳过NAL头（1字节）
if (nal_size < 2) {
return 0;
}
// 首先进行防竞争字节处理
rbsp_data = remove_emulation_prevention_bytes(nal_data, nal_size, &rbsp_size);
if (!rbsp_data) {
return 0;
}
pos = 1; // 跳过NAL头（1字节）
while (pos < rbsp_size) {
// 解析payload_type（使用类似UTF-8的变长编码）
do {
if (pos >= rbsp_size) {
break;
}
byte = rbsp_data[pos++];
payload_type += byte;
} while (byte == 0xFF);
if (pos >= rbsp_size) {
break;
}
// 解析payload_size（使用类似UTF-8的变长编码）
int payload_size = 0;
do {
if (pos >= rbsp_size) {
break;
}
byte = rbsp_data[pos++];
payload_size += byte;
} while (byte == 0xFF);
if (pos + payload_size > rbsp_size) {
printf("Warning: SEI payload size exceeds RBSP size\n");
break;
}
save_gyro_data(rbsp_data+pos, payload_size, file);
pos += payload_size;
// 跳过rbsp_trailing_bits（如果有）
if (pos < rbsp_size) {
// rbsp_trailing_bits以1开始，后面跟0
if (rbsp_data[pos] == 0x80) {
pos++;
}
}
}
free(rbsp_data);
return 0;
}
// 解析H.265 SEI NAL单元（带防竞争处理）
int parse_h265_sei_nal(const xmedia_u8* nal_data, int nal_size, FILE *file)
{
int rbsp_size;
xmedia_u8* rbsp_data;
int payload_type = 0;
xmedia_u8 byte;
int pos = 2; // 跳过NAL头（1字节）
if (nal_size < 3) {
return 0;
}
// 首先进行防竞争字节处理
rbsp_data = remove_emulation_prevention_bytes(nal_data, nal_size, &rbsp_size);
if (!rbsp_data) {
return 0;
}
pos = 2; // 跳过NAL头（2字节）
while (pos < rbsp_size) {
// 解析payload_type（使用类似UTF-8的变长编码）
do {
if (pos >= rbsp_size) {
break;
}
byte = rbsp_data[pos++];
payload_type += byte;
} while (byte == 0xFF);
if (pos >= rbsp_size) {
break;
}
// 解析payload_size（使用类似UTF-8的变长编码）
int payload_size = 0;
do {
if (pos >= rbsp_size) {
break;
}
byte = rbsp_data[pos++];
payload_size += byte;
} while (byte == 0xFF);
if (pos + payload_size > rbsp_size) {
printf("Warning: SEI payload size exceeds RBSP size\n");
break;
}
save_gyro_data(rbsp_data+pos, payload_size, file);
pos += payload_size;
// 跳过rbsp_trailing_bits（如果有）
if (pos < rbsp_size) {
// rbsp_trailing_bits以1开始，后面跟0
if (rbsp_data[pos] == 0x80) {
pos++;
}
}
}
free(rbsp_data);
return 0;
}
// 从码流中解析SEI数据（带防竞争处理）
void parse_stream_sei_with_emulation_prevention(const xmedia_u8* stream_data, int stream_size, int is_h265, FILE *file)
{
int pos = 0;
int next_start;
int nal_size;
xmedia_u8 nal_header;
xmedia_u8 nal_unit_type;
int is_sei_nal = 0;
int start_code_pos;
while (pos < stream_size - 4) {
// 查找4字节起始码
start_code_pos = find_4byte_start_code(stream_data, stream_size, pos);
if (start_code_pos == -1) {
break;
}
pos = start_code_pos + 4; // 移动到起始码后
if (pos >= stream_size) {
break;
}
// 查找下一个起始码以确定NAL单元大小
next_start = find_4byte_start_code(stream_data, stream_size, pos);
if (next_start == -1) {
nal_size = stream_size - pos;
} else {
nal_size = next_start - pos;
}
if (nal_size <= 0) {
pos += 1;
continue;
}
// 获取NAL单元类型
nal_header = stream_data[pos];
if (is_h265) {
nal_unit_type = (nal_header >> 1) & 0x3F; // H.265: 6位类型
} else {
nal_unit_type = nal_header & 0x1F; // H.264: 5位类型
}
// 检查是否为SEI NAL单元
if (is_h265) {
// H.265: 前缀SEI(39)和后缀SEI(40)
is_sei_nal = (nal_unit_type == 39 || nal_unit_type == 40);
} else {
// H.264: SEI类型为6
is_sei_nal = (nal_unit_type == 6);
}
//printf(" NAL size: %d bytes, Type: %d\n", nal_size, nal_unit_type);
if (is_sei_nal) {
if (is_h265) {
parse_h265_sei_nal(stream_data + pos, nal_size, file);
} else {
parse_h264_sei_nal(stream_data + pos, nal_size, file);
}
}
pos += nal_size;
}
}
xmedia_s32 msensor_spi_init(void)
{
xmedia_s32 fd, ret;
xmedia_s32 mode = SPI_MODE_3; /* | SPI_LSB_FIRST | SPI_LOOP | SPI_CS_HIGH */
xmedia_s32 bits = 8;
xmedia_u64 speed = 10000000;
xmedia_char *dev = "/dev/spidev2.0";
fd = open(dev, O_RDWR);
if (fd < 0) {
SAMPLE_PRT("open spi def failed\n");
return XMEDIA_FAILURE;
}
/* set spi mode */
ret = ioctl(fd, SPI_IOC_WR_MODE, &mode); /* SPI_IOC_WR_MODE32 */
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("can't set spi mode\n");
goto end;
}
ret = ioctl(fd, SPI_IOC_RD_MODE, &mode); /* SPI_IOC_RD_MODE32 */
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("can't get spi mode\n");
goto end;
}
/*
* bits per word
*/
ret = ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &bits);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("can't set bits per word\n");
goto end;
}
ret = ioctl(fd, SPI_IOC_RD_BITS_PER_WORD, &bits);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("can't get bits per word\n");
goto end;
}
/* set spi max speed */
ret = ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("can't set bits max speed HZ\n");
goto end;
}
ret = ioctl(fd, SPI_IOC_RD_MAX_SPEED_HZ, &speed);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("can't set bits max speed HZ\n");
goto end;
}
printf("spi mode: 0x%x\n", mode);
printf("bits per word: %d\n", bits);
printf("max speed: %lld Hz (%lld KHz)\n", speed, speed / 1000); /* speed with precision of 1000 */
end:
close(fd);
return ret;
}
xmedia_s32 msensor_init(xmedia_s32 dev)
{
xmedia_s32 ret;
xmedia_u32 buf_size = GYRO_BUF_SIZE + ACC_BUF_SIZE;
xmedia_char buf_name[20] = { 0 };
xmedia_msensor_param param;
snprintf(buf_name, sizeof(buf_name), "msensor_dev%d", dev);
g_msensor_attr[dev].phys_addr = xmedia_mmz_alloc(NULL, buf_name, buf_size);
if (g_msensor_attr[dev].phys_addr == 0) {
SAMPLE_PRT("alloc mmz for Msensor failed\n");
return XMEDIA_FAILURE;
}
g_msensor_attr[dev].virt_addr = xmedia_mmz_map(g_msensor_attr[dev].phys_addr, buf_size, XMEDIA_FALSE);
if (g_msensor_attr[dev].virt_addr == 0) {
SAMPLE_PRT("msensor malloc mmz map failed\n");
xmedia_mmz_free(g_msensor_attr[dev].phys_addr);
return XMEDIA_FAILURE;
}
(xmedia_void) memset(g_msensor_attr[dev].virt_addr, 0, buf_size);
g_msensor_attr[dev].buf_len = buf_size;
/* set device work mode */
param.attr.device_mask = XMEDIA_MSENSOR_DEVICE_GYRO | XMEDIA_MSENSOR_DEVICE_ACC;
param.attr.temperature_mask = XMEDIA_MSENSOR_TEMP_GYRO | XMEDIA_MSENSOR_TEMP_ACC;
/* set gyro samplerate and full scale range */
param.config.gyro_config.odr = GYRO_DATA_ODR * XMEDIA_MSENSOR_GRADIENT;
param.config.gyro_config.fsr = g_gyro_data_range * XMEDIA_MSENSOR_GRADIENT;
/* set accel samplerate and full scale range */
param.config.acc_config.odr = ACC_DATA_ODR * XMEDIA_MSENSOR_GRADIENT;
param.config.acc_config.fsr = ACC_DATA_RANGE * XMEDIA_MSENSOR_GRADIENT;
(xmedia_void) memcpy(&param.buf_attr, &g_msensor_attr[dev], sizeof(xmedia_msensor_buf_attr));
ret = xmedia_msensor_create_dev(dev, &param);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("msensor_create failed\n");
return XMEDIA_FAILURE;
}
return ret;
}
xmedia_void msensor_deinit(xmedia_s32 dev)
{
xmedia_s32 ret;
ret = xmedia_msensor_destroy_dev(dev);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("msensor deinit failed , ret:0x%x !\n", ret);
}
ret = xmedia_mmz_unmap(g_msensor_attr[dev].virt_addr);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("msensor mmz map failed, ret:0x%x !\n", ret);
}
ret = xmedia_mmz_free(g_msensor_attr[dev].phys_addr);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("msensor mmz free failed, ret:0x%x !\n", ret);
}
g_msensor_attr[dev].phys_addr = 0;
g_msensor_attr[dev].virt_addr = NULL;
return;
}
xmedia_s32 msensor_start(xmedia_s32 dev)
{
xmedia_s32 ret;
printf("spi init\n");
ret = msensor_spi_init();
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("init spi fail.ret:0x%x !\n", ret);
return ret;
}
printf("spi init ok\n");
ret = msensor_init(dev);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("init gyro fail.ret:0x%x !\n", ret);
return ret;
}
ret = xmedia_msensor_start_dev(dev);;
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("start gyro fail.ret:0x%x !\n", ret);
goto msensor_start_fail;
}
printf("motion sensor start ok\n");
return XMEDIA_SUCCESS;
msensor_start_fail:
msensor_deinit(dev);
return ret;
}
xmedia_void msensor_stop(xmedia_s32 dev)
{
xmedia_msensor_stop_dev(dev);;
msensor_deinit(dev);
return;
}
xmedia_venc_stream *msensor_mix_gyro_stream(xmedia_payload_type payload_type, FILE* pFd, xmedia_venc_stream* pstStream, xmedia_u32 stream_idex, msensor_data_info *pstdata_info)
{
xmedia_s32 i;
xmedia_msensor_data_info gyro_data;
static xmedia_venc_stream *last_stream = XMEDIA_NULL;
static xmedia_u32 delta_pts = 0;
xmedia_venc_stream *temp_stream;
xmedia_s32 addr_offset;
xmedia_s32 x;
xmedia_s32 y;
xmedia_s32 z;
xmedia_s32 temp;
xmedia_u64 pts;
static xmedia_char *gyro_data_buf = XMEDIA_NULL;
static xmedia_char *sei_data_buf = XMEDIA_NULL;
xmedia_u32 gyro_data_len = 0;
xmedia_u32 sei_data_len = 0;
xmedia_char * txt_buf;
xmedia_char txt_tmp[128] = {0};
xmedia_u32 txt_pos = 0;
xmedia_char * temp_ptr;
int cyclic;
xmedia_s32 ret;
xmedia_s32 msensor_dev;
xmedia_s32 user_id;
if (payload_type != PT_H264 && payload_type != PT_H265) {
return pstStream;
}
if (stream_idex == 0) {
last_stream = pstStream;
if (gyro_data_buf == XMEDIA_NULL) {
gyro_data_buf = malloc(4*1024);
}
if (sei_data_buf == XMEDIA_NULL) {
sei_data_buf = malloc(6*1024);
}
return XMEDIA_NULL;
}
if ((gyro_data_buf == XMEDIA_NULL) || (sei_data_buf == XMEDIA_NULL)) {
return pstStream;
}
msensor_dev = pstdata_info->msensor_dev;
user_id = pstdata_info->user_id;
gyro_data.id = user_id;
gyro_data.data_type = XMEDIA_MSENSOR_DATA_TYPE_GYRO;
if (stream_idex != 0xFFFFFFFF) {
delta_pts = pstStream->pack[0].pts - last_stream->pack[0].pts;
gyro_data.begin_pts = last_stream->pack[0].pts;
gyro_data.end_pts = pstStream->pack[0].pts;
} else {
gyro_data.begin_pts = last_stream->pack[0].pts;
gyro_data.end_pts = gyro_data.begin_pts + delta_pts;
}
ret = xmedia_msensor_get_data(msensor_dev, &gyro_data);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("msensor get data failed, pts[%lld]-pts[%lld] ret:0x%x !\n", gyro_data.begin_pts, gyro_data.end_pts, ret);
temp_stream = last_stream;
last_stream = pstStream;
if (stream_idex == 0xFFFFFFFF) {
temp_stream = last_stream;
last_stream = XMEDIA_NULL;
if (gyro_data_buf) {
free(gyro_data_buf);
gyro_data_buf = XMEDIA_NULL;
}
if (sei_data_buf) {
free(sei_data_buf);
sei_data_buf = XMEDIA_NULL;
}
}
return temp_stream;
}
//SAMPLE_PRT("get gyro data start pts %llu, end pts %llu, diff %llu, num %d %d\n", gyro_data.begin_pts,
// gyro_data.end_pts, gyro_data.end_pts - gyro_data.begin_pts, gyro_data.data[0].num,
// gyro_data.data[1].num);usleep(100000);
gyro_data_len = (gyro_data.data[0].num + gyro_data.data[1].num) * 24;
temp_ptr = gyro_data_buf;
addr_offset = g_msensor_attr[msensor_dev].phys_addr - (xmedia_u64)(xmedia_uintptr_t)g_msensor_attr[msensor_dev].virt_addr;
txt_buf = (xmedia_char *)malloc(1024*1024);
txt_pos = 0;
memset(txt_tmp, 0, sizeof(txt_tmp));
for (cyclic = 0; cyclic < 2; cyclic++) {
for (i = 0; i < gyro_data.data[cyclic].num; i++) {
x = *(xmedia_s32 *)(xmedia_uintptr_t)((xmedia_u64)(xmedia_uintptr_t)(gyro_data.data[cyclic].x_phys_addr + i) -
addr_offset);
y = *(xmedia_s32 *)(xmedia_uintptr_t)((xmedia_u64)(xmedia_uintptr_t)(gyro_data.data[cyclic].y_phys_addr + i) -
addr_offset);
z = *(xmedia_s32 *)(xmedia_uintptr_t)((xmedia_u64)(xmedia_uintptr_t)(gyro_data.data[cyclic].z_phys_addr + i) -
addr_offset);
temp = *(xmedia_s32 *)(xmedia_uintptr_t)((xmedia_u64)(xmedia_uintptr_t)(gyro_data.data[cyclic].temperature_phys_addr + i) -
addr_offset);
pts =
*(xmedia_u64 *)(xmedia_uintptr_t)((xmedia_u64)(xmedia_uintptr_t)(gyro_data.data[cyclic].pts_phys_addr + i) -
addr_offset);
*(xmedia_s32 *)temp_ptr = x;
temp_ptr += sizeof(xmedia_s32);
*(xmedia_s32 *)temp_ptr = y;
temp_ptr += sizeof(xmedia_s32);
*(xmedia_s32 *)temp_ptr = z;
temp_ptr += sizeof(xmedia_s32);
*(xmedia_s32 *)temp_ptr = temp;
temp_ptr += sizeof(xmedia_s32);
*(xmedia_u64 *)temp_ptr = pts;
temp_ptr += sizeof(xmedia_u64);
if (txt_buf) {
sprintf(txt_tmp, "%12d,%12d,%12d,%12d,%22llu, \n", x, y, z, temp, pts);
memcpy(txt_buf+txt_pos, txt_tmp, strlen(txt_tmp));
txt_pos += strlen(txt_tmp);
}
}
}
if (txt_buf) {
fwrite(txt_buf, txt_pos, 1, pstdata_info->gyro_txt_fp);
free(txt_buf);
txt_buf = XMEDIA_NULL;
}
if (payload_type == PT_H264) {
h264_encode_sei(sei_data_buf, gyro_data_buf, gyro_data_len, &sei_data_len);
fwrite(sei_data_buf, sei_data_len, 1, pFd);
if (pstdata_info->gyro_bin_fp) {
fwrite(sei_data_buf, sei_data_len, 1, pstdata_info->gyro_bin_fp);
}
} else if (payload_type == PT_H265) {
h265_encode_sei(sei_data_buf, gyro_data_buf, gyro_data_len, &sei_data_len);
fwrite(sei_data_buf, sei_data_len, 1, pFd);
if (pstdata_info->gyro_bin_fp) {
fwrite(sei_data_buf, sei_data_len, 1, pstdata_info->gyro_bin_fp);
}
}
for (i = 0; i < last_stream->pack_count; i++) {
fwrite(last_stream->pack[i].vir_addr + last_stream->pack[i].offset,
last_stream->pack[i].len - last_stream->pack[i].offset, 1, pFd);
}
if (stream_idex == 0xFFFFFFFF) {
temp_stream = last_stream;
last_stream = XMEDIA_NULL;
if (gyro_data_buf) {
free(gyro_data_buf);
gyro_data_buf = XMEDIA_NULL;
}
if (sei_data_buf) {
free(sei_data_buf);
sei_data_buf = XMEDIA_NULL;
}
} else {
temp_stream = last_stream;
last_stream = pstStream;
}
return temp_stream;
}
xmedia_void *msensor_get_data_thread(xmedia_void *args)
{
xmedia_s32 ret;
xmedia_s32 msensor_dev;
xmedia_s32 user_id;
msensor_data_info msensor_info;
msensor_get_data_info *get_data_info = (msensor_get_data_info *)args;
FILE *gyro_bin_fp = XMEDIA_NULL;
FILE *gyro_txt_fp = XMEDIA_NULL;
xmedia_u32 stream_index = 0;
//xmedia_u16 temp_pts = 0;
xmedia_venc_chn_attr venc_chn_attr;
xmedia_payload_type payload_type;
xmedia_char file_name[512];
xmedia_char *file_path="./";
FILE *file_ptr;
struct timeval time_out;
xmedia_venc_stream *venc_stream;
xmedia_venc_chn_status stat;
xmedia_u32 venc_mask = 0;
xmedia_venc_stream *temp_stream;
printf("start dump gyro data\n");
gyro_bin_fp = fopen(GYRO_BIN_FILE, "w+");
if (gyro_bin_fp == XMEDIA_NULL) {
SAMPLE_PRT("Error: cannot open imu data\n");
return XMEDIA_NULL;
}
gyro_txt_fp = fopen(GYRO_TXT_FILE, "w+");
if (gyro_txt_fp == XMEDIA_NULL) {
fclose(gyro_bin_fp);
SAMPLE_PRT("Error: cannot open imu data\n");
return XMEDIA_NULL;
}
ret = xmedia_venc_get_chn_attr(get_data_info->venc_chn, &venc_chn_attr);
if (XMEDIA_SUCCESS != ret) {
SAMPLE_PRT("xmedia_venc_get_chn_attr chn[%d] failed with %#x!\n", get_data_info->venc_chn, ret);
return XMEDIA_NULL;
}
payload_type = venc_chn_attr.venc_attr.en_type;
if (payload_type != PT_H264 && payload_type != PT_H265) {
goto exit;
}
if (payload_type == PT_H264) {
snprintf(file_name, 512, "%s/stream_chn%d%s", file_path, get_data_info->venc_chn, ".h264");
} else {
snprintf(file_name, 512, "%s/stream_chn%d%s", file_path, get_data_info->venc_chn, ".h265");
}
file_ptr = fopen(file_name, "wb");
if (!file_ptr) {
SAMPLE_PRT("open file %s failed!\n", file_name);
return XMEDIA_NULL;
}
chmod(file_name, S_IRUSR|S_IWUSR|S_IRGRP|S_IROTH);
venc_mask |= 1 << get_data_info->venc_chn;
msensor_dev = get_data_info->msensor_dev;
ret = xmedia_msensor_add_user(msensor_dev, &user_id);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("msensor add user failed, ret:0x%x !\n", ret);
goto exit;
}
msensor_info.msensor_dev = msensor_dev;
msensor_info.user_id = user_id;
msensor_info.gyro_txt_fp = gyro_txt_fp;
msensor_info.gyro_bin_fp = gyro_bin_fp;
while (get_data_info->thread_start) {
time_out.tv_sec = 2;
time_out.tv_usec = 0;
ret = xmedia_venc_select(venc_mask, &time_out);
if (ret == XMEDIA_ERRCODE_INVALID_PARAM || ret == XMEDIA_FAILURE) {
SAMPLE_PRT("select err\n");
break;
} else if (ret == XMEDIA_ERRCODE_TIMEOUT) {
usleep(100 * 1000);
continue;
}
ret = xmedia_venc_query_status(get_data_info->venc_chn, &stat);
if (XMEDIA_SUCCESS != ret) {
SAMPLE_PRT("xmedia_venc_query_status chn[%d] failed with %#x!\n", get_data_info->venc_chn, ret);
break;
}
if (0 == stat.cur_packs) {
continue;
}
venc_stream = (xmedia_venc_stream *)malloc(sizeof(xmedia_venc_stream));
if (XMEDIA_NULL == venc_stream) {
SAMPLE_PRT("malloc stream pack failed!\n");
break;
}
memset(venc_stream, 0, sizeof(xmedia_venc_stream));
venc_stream->pack = (xmedia_venc_pack*)malloc(sizeof(xmedia_venc_pack) * stat.cur_packs);
if (XMEDIA_NULL == venc_stream->pack) {
SAMPLE_PRT("malloc stream pack failed!\n");
free(venc_stream);
break;
}
venc_stream->pack_count = stat.cur_packs;
ret = xmedia_venc_get_stream(get_data_info->venc_chn, venc_stream, -1);
if (XMEDIA_SUCCESS != ret) {
free(venc_stream->pack);
venc_stream->pack = XMEDIA_NULL;
free(venc_stream);
SAMPLE_PRT("xmedia_venc_get_stream failed with %#x!\n", ret);
break;
}
if (PT_JPEG != payload_type) {
temp_stream = msensor_mix_gyro_stream(payload_type, file_ptr, venc_stream, stream_index, &msensor_info);
stream_index++;
if (XMEDIA_NULL != temp_stream) {
xmedia_venc_release_stream(get_data_info->venc_chn, temp_stream);
free(temp_stream->pack);
temp_stream->pack = XMEDIA_NULL;
free(temp_stream);
}
}
}
temp_stream = msensor_mix_gyro_stream(payload_type, file_ptr, XMEDIA_NULL, 0xFFFFFFFF, &msensor_info);
if (XMEDIA_NULL != temp_stream) {
xmedia_venc_release_stream(get_data_info->venc_chn, temp_stream);
free(temp_stream->pack);
temp_stream->pack = XMEDIA_NULL;
free(temp_stream);
}
ret = xmedia_msensor_delete_user(msensor_dev, user_id);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("msensor delete user failed, ret:0x%x !\n", ret);
}
exit:
fclose(gyro_bin_fp);
fclose(gyro_txt_fp);
return XMEDIA_NULL;
}
xmedia_s32 sample_msensor_parse_data(xmedia_void)
{
FILE *file = fopen(GYRO_BIN_FILE, "rb");
FILE *gyro_parse_fp = XMEDIA_NULL;
gyro_parse_fp = fopen(GYRO_PARSE_FILE, "w+");
if (gyro_parse_fp == XMEDIA_NULL) {
SAMPLE_PRT("Error: cannot open imu data\n");
return XMEDIA_NULL;
}
if (file == NULL) {
fclose(gyro_parse_fp);
perror("无法打开文件");
return 1;
}
// 获取文件大小
fseek(file, 0, SEEK_END);
long file_size = ftell(file);
fseek(file, 0, SEEK_SET);
// 分配内存
unsigned char *buffer = (unsigned char*)malloc(file_size + 1);
if (buffer == NULL) {
perror("内存分配失败");
fclose(file);
return 1;
}
// 读取整个文件
size_t bytes_read = fread(buffer, 1, file_size, file);
buffer[bytes_read] = '\0'; // 添加字符串结束符
fclose(file);
parse_stream_sei_with_emulation_prevention(buffer, bytes_read, 1, gyro_parse_fp);
fclose(gyro_parse_fp);
// 清理
free(buffer);
return 0;
}
xmedia_s32 sample_msensor_start_dump_data(xmedia_s32 dev)
{
memset(&g_get_data_info[dev], 0, sizeof(msensor_get_data_info));
g_get_data_info[dev].thread_start = XMEDIA_TRUE;
g_get_data_info[dev].data_type = XMEDIA_MSENSOR_DATA_TYPE_GYRO;
g_get_data_info[dev].time_intval = 4;
g_get_data_info[dev].msensor_dev = dev;
pthread_create(&g_msensor_pid[dev], 0, msensor_get_data_thread, (xmedia_void *)&g_get_data_info[dev]);
return XMEDIA_SUCCESS;
}
xmedia_s32 sample_msensor_stop_dump_data(xmedia_s32 dev)
{
if (XMEDIA_TRUE == g_get_data_info[dev].thread_start) {
g_get_data_info[dev].thread_start = XMEDIA_FALSE;
if (g_msensor_pid[dev] > 0) {
pthread_join(g_msensor_pid[dev], 0);
}
}
return XMEDIA_SUCCESS;
}
xmedia_s32 sample_msensor_init(xmedia_s32 dev_cnt, xmedia_s32 *dev)
{
xmedia_s32 i;
xmedia_s32 ret = XMEDIA_SUCCESS;
xmedia_msensor_init();
for (i = 0; i < dev_cnt; i++) {
ret = msensor_start(dev[i]);
}
return ret;
}
xmedia_void sample_msensor_exit(xmedia_s32 dev_cnt, xmedia_s32 *dev)
{
xmedia_s32 i;
for (i = 0; i < dev_cnt; i++) {
msensor_stop(dev[i]);
}
xmedia_msensor_exit();
}
xmedia_s32 sample_venc_h265_msensor(xmedia_bool set_roi)
{
xmedia_s32 ret = XMEDIA_SUCCESS;
xmedia_s32 blk_size = 0;
xmedia_video_size pic_size = { 0 };
xmedia_video_size sub_pic_size = { 0 };
sample_comm_video_param video_param = { 0 };
sample_sys_config sys_config = { 0 };
xmedia_s32 msensor_dev = 0;
xmedia_s32 vi_dev = 0;
xmedia_s32 vi_pipe = 0;
xmedia_s32 vi_chn = 0;
sample_vi_config vi_config = { 0 };
xmedia_s32 sensor_type = sample_msensor_sensor_type[0];
xmedia_u32 framerate = 0;
vi_sensor_info sensor_info = { 0 };
sample_isp_param isp_param = { 0 };
xmedia_isp_config isp_config = { 0 };
xmedia_bool mirror = XMEDIA_FALSE;
xmedia_bool flip = XMEDIA_FALSE;
xmedia_s32 vpss_chnl = 0;
xmedia_s32 vpss_pipe = 0;
xmedia_s32 vpss_ichn = 0;
xmedia_s32 vpss_ochn[1] = { 0 };
sample_vpss_config vpss_config = { 0 };
xmedia_video_size vpss_ochn_size[VPSS_MAX_OCHN_NUM] = { 0 };
xmedia_s32 venc_chnl = 0;
xmedia_s32 venc_chn[1] = { 0 };
xmedia_s32 venc_chn_cnt = 1;
sample_venc_config venc_config = { 0 };
xmedia_venc_gop_mode gop_mode;
video_param.video_fmt = XMEDIA_VIDEO_FMT_LINEAR;
video_param.pixel_fmt = XMEDIA_VIDEO_PIXEL_FMT_YVU_SEMIPLANAR_420;
video_param.data_width = XMEDIA_VIDEO_DATA_WIDTH_8;
video_param.compress_mode = XMEDIA_VIDEO_COMPRESS_MODE_NONE;
sample_comm_vi_get_sensor_info(sensor_type, &sensor_info);
sample_comm_vi_get_framerate_by_sensor(sensor_type, &framerate);
gop_mode = VENC_GOPMODE_NORMALP;
// sys init
sys_config.sys_conf.pipe_mode[vi_pipe].vicap_viproc_mode = XMEDIA_WORK_MODE_OFFLINE;
sys_config.sys_conf.pipe_mode[vi_pipe].viproc_vpss_mode = XMEDIA_WORK_MODE_OFFLINE;
sys_config.sys_conf.pipe_mode[vi_pipe].gdc_vpss_mode = XMEDIA_WORK_MODE_OFFLINE;
sys_config.vb_conf.max_pool_cnt = 25;
pic_size.width = sensor_info.width;
pic_size.height = sensor_info.height;
blk_size = sample_comm_sys_get_buffer_size(pic_size, video_param.video_fmt, sensor_info.pixel_format,
sensor_info.bit_width, video_param.compress_mode);
sys_config.vb_conf.common_pool[0].block_size = blk_size;
sys_config.vb_conf.common_pool[0].block_cnt = 2;
blk_size = sample_comm_sys_get_buffer_size(pic_size, video_param.video_fmt, video_param.pixel_fmt,
video_param.data_width, video_param.compress_mode);
sys_config.vb_conf.common_pool[1].block_size = blk_size;
sys_config.vb_conf.common_pool[1].block_cnt = 4;
sub_pic_size.width = 640;
sub_pic_size.height = 480;
blk_size = sample_comm_sys_get_buffer_size(sub_pic_size, video_param.video_fmt, video_param.pixel_fmt,
video_param.data_width, video_param.compress_mode);
sys_config.vb_conf.common_pool[2].block_size = blk_size;
sys_config.vb_conf.common_pool[2].block_cnt = 2;
sys_config.vb_conf.supplement_config = 1;
ret = sample_msensor_sys_init(&sys_config);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("sys init failed !\n");
return ret;
}
sample_msensor_init(1, &msensor_dev);
vi_config.dev_info[vi_dev].dev_en = XMEDIA_TRUE;
vi_config.dev_info[vi_dev].dev_no = vi_dev;
vi_config.dev_info[vi_dev].sensor_type = sensor_type;
vi_config.pipe_info[vi_pipe].pipe_en = XMEDIA_TRUE;
vi_config.pipe_info[vi_pipe].pipe_no = vi_pipe;
vi_config.pipe_info[vi_pipe].chn_info[vi_chn].chn_en = XMEDIA_TRUE;
vi_config.pipe_info[vi_pipe].chn_info[vi_chn].chn_no = vi_chn;
vi_config.dev_bind_pipe[vi_dev].pipe[0] = vi_pipe;
vi_config.dev_bind_pipe[vi_dev].pipe[1] = -1;
isp_config.fps = framerate;
isp_config.mode_config.work_mode = XMEDIA_ISP_WORK_MODE_MASTER;
isp_config.mode_config.master_mode.blend_stat_enable = XMEDIA_FALSE;
isp_config.mode_config.master_mode.slave_num = 0;
isp_config.pixel_fmt = sensor_info.pixel_format;
isp_config.size.height = sensor_info.height;
isp_config.size.width = sensor_info.width;
isp_config.wdr_mode = sensor_info.wdr_mode;
// isp pipe init
isp_param.pipe[vi_pipe] = vi_pipe;
isp_param.isp_info[vi_pipe].isp_pipe_en = XMEDIA_TRUE;
isp_param.isp_info[vi_pipe].isp_sensor_en = XMEDIA_TRUE;
isp_param.isp_info[vi_pipe].sensor_type = sensor_type;
isp_param.isp_info[vi_pipe].mirror = mirror;
isp_param.isp_info[vi_pipe].flip = flip;
memcpy(&(isp_param.isp_info[vi_pipe].isp_config), &isp_config, sizeof(xmedia_isp_config));
// isp init
ret = sample_comm_isp_init(&isp_param, &vi_config);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("isp init failed!\n");
goto exit0;
}
// vi start
ret = sample_comm_vi_start(&vi_config, &video_param);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("start vi failed!\n");
goto exit1;
}
// isp start
ret = sample_comm_isp_start(&isp_param);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("start isp failed!\n");
goto exit2;
}
// vpss config
ret = sample_comm_vpss_get_default_pipe_cfg(&vpss_config.pipe_info[vpss_pipe].pipe_config, pic_size, &video_param);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("get default pipe cfg failed !\n");
goto exit2;
}
vpss_ochn_size[0].width = sensor_info.width;
vpss_ochn_size[0].height = sensor_info.height;
vpss_config.pipe_info[vpss_pipe].pipe_en = XMEDIA_TRUE;
vpss_config.pipe_info[vpss_pipe].pipe_no = vpss_pipe;
vpss_chnl = vpss_ochn[0];
vpss_config.pipe_info[vpss_pipe].chn_info[vpss_chnl].chn_en = XMEDIA_TRUE;
vpss_config.pipe_info[vpss_pipe].chn_info[vpss_chnl].chn_no = vpss_ochn[0];
ret = sample_comm_vpss_get_default_ochn_cfg(&vpss_config.pipe_info[vpss_pipe].chn_info[vpss_chnl].chn_config,
vpss_ochn_size[vpss_chnl], &video_param);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("get default pipe cfg failed !\n");
goto exit2;
}
vpss_config.pipe_info[vpss_pipe].chn_info[vpss_chnl].chn_config.depth = 0;
// vpss start
ret = sample_comm_vpss_start(&vpss_config);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("vpss start failed !\n");
goto exit2;
}
ret = sample_comm_sys_vi_bind_vpss(vi_pipe, vi_chn, vpss_pipe, vpss_ichn);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("vi bind vpss failed !\n");
goto exit3;
}
venc_chnl = venc_chn[0];
venc_config.chn_info[venc_chnl].venc_en = XMEDIA_TRUE;
venc_config.chn_info[venc_chnl].venc_chn = venc_chnl;
venc_config.chn_info[venc_chnl].payload_type = PT_H265;
venc_config.chn_info[venc_chnl].rc_mode = VENC_RC_MODE_H265CBR;
venc_config.chn_info[venc_chnl].venc_gop_attr.gop_mode = gop_mode;
sample_comm_venc_get_default_chn_info(vpss_ochn_size[0], framerate, &venc_config.chn_info[venc_chnl]);
ret = sample_comm_venc_start(&venc_config);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("venc start failed !\n");
goto exit4;
}
if (set_roi == XMEDIA_TRUE) {
ret = sample_comm_venc_set_roi(venc_chn, venc_chn_cnt);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("venc start get stream failed !\n");
goto exit4;
}
}
ret = sample_comm_sys_vpss_bind_venc(vpss_pipe, vpss_ochn[0], venc_chn[0]);
if (ret != XMEDIA_SUCCESS) {
SAMPLE_PRT("vpss bind venc failed !\n");
goto exit5;
}
sample_msensor_start_dump_data(msensor_dev);
PAUSE(g_force_exit);
sample_msensor_stop_dump_data(msensor_dev);
sample_comm_sys_vpss_unbind_venc(vpss_pipe, vpss_ochn[0], venc_chn[0]);
exit5:
sample_comm_venc_stop(&venc_config);
exit4:
sample_comm_sys_vi_unbind_vpss(vi_pipe, vi_chn, vpss_pipe, vpss_ichn);
exit3:
sample_comm_vpss_stop(&vpss_config);
exit2:
sample_comm_isp_stop(&isp_param);
exit1:
sample_comm_vi_stop(&vi_config);
exit0:
sample_comm_isp_exit(&isp_param);
sample_msensor_exit(1, &msensor_dev);
sample_msensor_sys_exit();
return ret;
}
int main(int argc,char **argv)
{
xmedia_s32 ret = XMEDIA_SUCCESS;
xmedia_s32 index = 0;
xmedia_bool set_roi = XMEDIA_FALSE;
signal(SIGINT, sample_msensor_handle_sig);
signal(SIGTERM, sample_msensor_handle_sig);
if (argc < 2) {
sample_msensor_usage(argv[0]);
return 0;
}
index = atoi(argv[1]);
if (argc == 2) {
switch (index) {
case 0:
ret = sample_venc_h265_msensor(set_roi);
break;
case 1:
sample_msensor_parse_data();
break;
default:
sample_msensor_usage(argv[0]);
return 0;
}
}
if (XMEDIA_SUCCESS == ret) {
SAMPLE_PRT("program exit normally!\n");
} else {
SAMPLE_PRT("program exit abnormally!\n");
}
return 0;
}
Reference in New Issue View Git Blame Copy Permalink