@ -26,7 +26,7 @@ extern ExitHandler do_exit;
const size_t FRAME_WIDTH = 1928 ;
const size_t FRAME_HEIGHT = 1208 ;
const size_t FRAME_STRIDE = 2416 ; // for 10 bit output
const size_t FRAME_STRIDE = 2896 ; // for 12 bit output. 1928 * 12 / 8 + 4 (alignment)
const int MIPI_SETTLE_CNT = 33 ; // Calculated by camera_freqs.py
@ -56,7 +56,7 @@ const int EXPOSURE_TIME_MIN = 2; // with HDR, fastest ss
const int EXPOSURE_TIME_MAX = 1904 ; // with HDR, slowest ss
// ************** low level camera helpers ****************
int cam_control ( int fd , int op_code , void * handle , int size ) {
int do_ cam_control( int fd , int op_code , void * handle , int size ) {
struct cam_control camcontrol = { 0 } ;
camcontrol . op_code = op_code ;
camcontrol . handle = ( uint64_t ) handle ;
@ -83,7 +83,7 @@ std::optional<int32_t> device_acquire(int fd, int32_t session_handle, void *data
. num_resources = ( uint32_t ) ( data ? 1 : 0 ) ,
. resource_hdl = ( uint64_t ) data ,
} ;
int err = cam_control ( fd , CAM_ACQUIRE_DEV , & cmd , sizeof ( cmd ) ) ;
int err = do_ cam_control( fd , CAM_ACQUIRE_DEV , & cmd , sizeof ( cmd ) ) ;
return err = = 0 ? std : : make_optional ( cmd . dev_handle ) : std : : nullopt ;
} ;
@ -93,13 +93,13 @@ int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t p
. dev_handle = dev_handle ,
. packet_handle = packet_handle ,
} ;
return cam_control ( fd , CAM_CONFIG_DEV , & cmd , sizeof ( cmd ) ) ;
return do_ cam_control( fd , CAM_CONFIG_DEV , & cmd , sizeof ( cmd ) ) ;
}
int device_control ( int fd , int op_code , int session_handle , int dev_handle ) {
// start stop and release are all the same
struct cam_start_stop_dev_cmd cmd { . session_handle = session_handle , . dev_handle = dev_handle } ;
return cam_control ( fd , op_code , & cmd , sizeof ( cmd ) ) ;
return do_ cam_control( fd , op_code , & cmd , sizeof ( cmd ) ) ;
}
void * alloc_w_mmu_hdl ( int video0_fd , int len , uint32_t * handle , int align = 8 , int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE ,
@ -118,7 +118,7 @@ void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, i
mem_mgr_alloc_cmd . num_hdl + + ;
}
cam_control ( video0_fd , CAM_REQ_MGR_ALLOC_BUF , & mem_mgr_alloc_cmd , sizeof ( mem_mgr_alloc_cmd ) ) ;
do_ cam_control( video0_fd , CAM_REQ_MGR_ALLOC_BUF , & mem_mgr_alloc_cmd , sizeof ( mem_mgr_alloc_cmd ) ) ;
* handle = mem_mgr_alloc_cmd . out . buf_handle ;
void * ptr = NULL ;
@ -137,7 +137,7 @@ void release(int video0_fd, uint32_t handle) {
struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = { 0 } ;
mem_mgr_release_cmd . buf_handle = handle ;
ret = cam_control ( video0_fd , CAM_REQ_MGR_RELEASE_BUF , & mem_mgr_release_cmd , sizeof ( mem_mgr_release_cmd ) ) ;
ret = do_ cam_control( video0_fd , CAM_REQ_MGR_RELEASE_BUF , & mem_mgr_release_cmd , sizeof ( mem_mgr_release_cmd ) ) ;
assert ( ret = = 0 ) ;
}
@ -153,34 +153,39 @@ void clear_req_queue(int fd, int32_t session_hdl, int32_t link_hdl) {
req_mgr_flush_request . link_hdl = link_hdl ;
req_mgr_flush_request . flush_type = CAM_REQ_MGR_FLUSH_TYPE_ALL ;
int ret ;
ret = cam_control ( fd , CAM_REQ_MGR_FLUSH_REQ , & req_mgr_flush_request , sizeof ( req_mgr_flush_request ) ) ;
ret = do_ cam_control( fd , CAM_REQ_MGR_FLUSH_REQ , & req_mgr_flush_request , sizeof ( req_mgr_flush_request ) ) ;
// LOGD("flushed all req: %d", ret);
}
// ************** high level camera helpers ****************
void sensors_poke ( struct CameraState * s , int request_id ) {
void CameraState : : sensors_start ( ) {
int start_reg_len = sizeof ( start_reg_array ) / sizeof ( struct i2c_random_wr_payload ) ;
sensors_i2c ( start_reg_array , start_reg_len , CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG ) ;
}
void CameraState : : sensors_poke ( int request_id ) {
uint32_t cam_packet_handle = 0 ;
int size = sizeof ( struct cam_packet ) ;
struct cam_packet * pkt = ( struct cam_packet * ) alloc_w_mmu_hdl ( s - > multi_cam_state - > video0_fd , size , & cam_packet_handle ) ;
struct cam_packet * pkt = ( struct cam_packet * ) alloc_w_mmu_hdl ( multi_cam_state - > video0_fd , size , & cam_packet_handle ) ;
pkt - > num_cmd_buf = 0 ;
pkt - > kmd_cmd_buf_index = - 1 ;
pkt - > header . size = size ;
pkt - > header . op_code = 0x7f ;
pkt - > header . request_id = request_id ;
int ret = device_config ( s - > s ensor_fd, s - > s ession_handle, s - > sensor_dev_handle , cam_packet_handle ) ;
int ret = device_config ( sensor_fd , session_handle , sensor_dev_handle , cam_packet_handle ) ;
assert ( ret = = 0 ) ;
munmap ( pkt , size ) ;
release_fd ( s - > multi_cam_state - > video0_fd , cam_packet_handle ) ;
release_fd ( multi_cam_state - > video0_fd , cam_packet_handle ) ;
}
void sensors_i2c ( struct CameraState * s , struct i2c_random_wr_payload * dat , int len , int op_code ) {
void CameraState : : sensors_i2c ( struct i2c_random_wr_payload * dat , int len , int op_code ) {
// LOGD("sensors_i2c: %d", len);
uint32_t cam_packet_handle = 0 ;
int size = sizeof ( struct cam_packet ) + sizeof ( struct cam_cmd_buf_desc ) * 1 ;
struct cam_packet * pkt = ( struct cam_packet * ) alloc_w_mmu_hdl ( s - > multi_cam_state - > video0_fd , size , & cam_packet_handle ) ;
struct cam_packet * pkt = ( struct cam_packet * ) alloc_w_mmu_hdl ( multi_cam_state - > video0_fd , size , & cam_packet_handle ) ;
pkt - > num_cmd_buf = 1 ;
pkt - > kmd_cmd_buf_index = - 1 ;
pkt - > header . size = size ;
@ -190,7 +195,7 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l
buf_desc [ 0 ] . size = buf_desc [ 0 ] . length = sizeof ( struct i2c_rdwr_header ) + len * sizeof ( struct i2c_random_wr_payload ) ;
buf_desc [ 0 ] . type = CAM_CMD_BUF_I2C ;
struct cam_cmd_i2c_random_wr * i2c_random_wr = ( struct cam_cmd_i2c_random_wr * ) alloc_w_mmu_hdl ( s - > multi_cam_state - > video0_fd , buf_desc [ 0 ] . size , ( uint32_t * ) & buf_desc [ 0 ] . mem_handle ) ;
struct cam_cmd_i2c_random_wr * i2c_random_wr = ( struct cam_cmd_i2c_random_wr * ) alloc_w_mmu_hdl ( multi_cam_state - > video0_fd , buf_desc [ 0 ] . size , ( uint32_t * ) & buf_desc [ 0 ] . mem_handle ) ;
i2c_random_wr - > header . count = len ;
i2c_random_wr - > header . op_code = 1 ;
i2c_random_wr - > header . cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR ;
@ -198,14 +203,15 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l
i2c_random_wr - > header . addr_type = CAMERA_SENSOR_I2C_TYPE_WORD ;
memcpy ( i2c_random_wr - > random_wr_payload , dat , len * sizeof ( struct i2c_random_wr_payload ) ) ;
int ret = device_config ( s - > s ensor_fd, s - > s ession_handle, s - > sensor_dev_handle , cam_packet_handle ) ;
int ret = device_config ( sensor_fd , session_handle , sensor_dev_handle , cam_packet_handle ) ;
assert ( ret = = 0 ) ;
munmap ( i2c_random_wr , buf_desc [ 0 ] . size ) ;
release_fd ( s - > multi_cam_state - > video0_fd , buf_desc [ 0 ] . mem_handle ) ;
release_fd ( multi_cam_state - > video0_fd , buf_desc [ 0 ] . mem_handle ) ;
munmap ( pkt , size ) ;
release_fd ( s - > multi_cam_state - > video0_fd , cam_packet_handle ) ;
release_fd ( multi_cam_state - > video0_fd , cam_packet_handle ) ;
}
static cam_cmd_power * power_set_wait ( cam_cmd_power * power , int16_t delay_ms ) {
cam_cmd_unconditional_wait * unconditional_wait = ( cam_cmd_unconditional_wait * ) ( ( char * ) power + ( sizeof ( struct cam_cmd_power ) + ( power - > count - 1 ) * sizeof ( struct cam_power_settings ) ) ) ;
unconditional_wait - > cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT ;
@ -214,7 +220,8 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) {
return ( struct cam_cmd_power * ) ( unconditional_wait + 1 ) ;
} ;
void sensors_init ( int video0_fd , int sensor_fd , int camera_num ) {
void CameraState : : sensors_init ( ) {
int video0_fd = multi_cam_state - > video0_fd ;
uint32_t cam_packet_handle = 0 ;
int size = sizeof ( struct cam_packet ) + sizeof ( struct cam_cmd_buf_desc ) * 2 ;
struct cam_packet * pkt = ( struct cam_packet * ) alloc_w_mmu_hdl ( video0_fd , size , & cam_packet_handle ) ;
@ -333,7 +340,7 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
power - > power_settings [ 2 ] . power_seq_type = 3 ;
LOGD ( " probing the sensor " ) ;
int ret = cam_control ( sensor_fd , CAM_SENSOR_PROBE_CMD , ( void * ) ( uintptr_t ) cam_packet_handle , 0 ) ;
int ret = do_ cam_control( sensor_fd , CAM_SENSOR_PROBE_CMD , ( void * ) ( uintptr_t ) cam_packet_handle , 0 ) ;
assert ( ret = = 0 ) ;
munmap ( i2c_info , buf_desc [ 0 ] . size ) ;
@ -344,13 +351,13 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
release_fd ( video0_fd , cam_packet_handle ) ;
}
void config_isp ( struct CameraState * s , int io_mem_handle , int fence , int request_id , int buf0_mem_handle , int buf0_offset ) {
void CameraState : : config_isp ( int io_mem_handle , int fence , int request_id , int buf0_mem_handle , int buf0_offset ) {
uint32_t cam_packet_handle = 0 ;
int size = sizeof ( struct cam_packet ) + sizeof ( struct cam_cmd_buf_desc ) * 2 ;
if ( io_mem_handle ! = 0 ) {
size + = sizeof ( struct cam_buf_io_cfg ) ;
}
struct cam_packet * pkt = ( struct cam_packet * ) alloc_w_mmu_hdl ( s - > multi_cam_state - > video0_fd , size , & cam_packet_handle ) ;
struct cam_packet * pkt = ( struct cam_packet * ) alloc_w_mmu_hdl ( multi_cam_state - > video0_fd , size , & cam_packet_handle ) ;
pkt - > num_cmd_buf = 2 ;
pkt - > kmd_cmd_buf_index = 0 ;
// YUV has kmd_cmd_buf_offset = 1780
@ -445,7 +452,7 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
buf_desc [ 1 ] . length = buf_desc [ 1 ] . size - buf_desc [ 1 ] . offset ;
buf_desc [ 1 ] . type = CAM_CMD_BUF_GENERIC ;
buf_desc [ 1 ] . meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON ;
uint32_t * buf2 = ( uint32_t * ) alloc_w_mmu_hdl ( s - > multi_cam_state - > video0_fd , buf_desc [ 1 ] . size , ( uint32_t * ) & buf_desc [ 1 ] . mem_handle , 0x20 ) ;
uint32_t * buf2 = ( uint32_t * ) alloc_w_mmu_hdl ( multi_cam_state - > video0_fd , buf_desc [ 1 ] . size , ( uint32_t * ) & buf_desc [ 1 ] . mem_handle , 0x20 ) ;
memcpy ( buf2 , & tmp , sizeof ( tmp ) ) ;
if ( io_mem_handle ! = 0 ) {
@ -464,10 +471,10 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
. h_init = 0x0 ,
. v_init = 0x0 ,
} ;
io_cfg [ 0 ] . format = CAM_FORMAT_MIPI_RAW_10 ; // CAM_FORMAT_UBWC_TP10 for YUV
io_cfg [ 0 ] . format = CAM_FORMAT_MIPI_RAW_12 ; // CAM_FORMAT_UBWC_TP10 for YUV
io_cfg [ 0 ] . color_space = CAM_COLOR_SPACE_BASE ; // CAM_COLOR_SPACE_BT601_FULL for YUV
io_cfg [ 0 ] . color_pattern = 0x5 ; // 0x0 for YUV
io_cfg [ 0 ] . bpp = 0xa ;
io_cfg [ 0 ] . bpp = 0xc ;
io_cfg [ 0 ] . resource_type = CAM_ISP_IFE_OUT_RES_RDI_0 ; // CAM_ISP_IFE_OUT_RES_FULL for YUV
io_cfg [ 0 ] . fence = fence ;
io_cfg [ 0 ] . direction = CAM_BUF_OUTPUT ;
@ -475,142 +482,141 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
io_cfg [ 0 ] . framedrop_pattern = 0x1 ;
}
int ret = device_config ( s - > multi_cam_state - > isp_fd , s - > s ession_handle, s - > isp_dev_handle , cam_packet_handle ) ;
int ret = device_config ( multi_cam_state - > isp_fd , session_handle , isp_dev_handle , cam_packet_handle ) ;
assert ( ret = = 0 ) ;
if ( ret ! = 0 ) {
printf ( " ISP CONFIG FAILED \n " ) ;
}
munmap ( buf2 , buf_desc [ 1 ] . size ) ;
release_fd ( s - > multi_cam_state - > video0_fd , buf_desc [ 1 ] . mem_handle ) ;
// release_fd(s-> multi_cam_state->video0_fd, buf_desc[0].mem_handle);
release_fd ( multi_cam_state - > video0_fd , buf_desc [ 1 ] . mem_handle ) ;
// release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle);
munmap ( pkt , size ) ;
release_fd ( s - > multi_cam_state - > video0_fd , cam_packet_handle ) ;
release_fd ( multi_cam_state - > video0_fd , cam_packet_handle ) ;
}
void enqueue_buffer ( struct CameraState * s , int i , bool dp ) {
void CameraState : : enqueue_buffer ( int i , bool dp ) {
int ret ;
int request_id = s - > request_ids [ i ] ;
int request_id = request_ids [ i ] ;
if ( s - > buf_handle [ i ] ) {
release ( s - > multi_cam_state - > video0_fd , s - > buf_handle [ i ] ) ;
if ( buf_handle [ i ] ) {
release ( multi_cam_state - > video0_fd , buf_handle [ i ] ) ;
// wait
struct cam_sync_wait sync_wait = { 0 } ;
sync_wait . sync_obj = s - > s ync_objs[ i ] ;
sync_wait . sync_obj = sync_objs [ i ] ;
sync_wait . timeout_ms = 50 ; // max dt tolerance, typical should be 23
ret = cam_control ( s - > multi_cam_state - > video1_fd , CAM_SYNC_WAIT , & sync_wait , sizeof ( sync_wait ) ) ;
ret = do_ cam_control( multi_cam_state - > video1_fd , CAM_SYNC_WAIT , & sync_wait , sizeof ( sync_wait ) ) ;
// LOGD("fence wait: %d %d", ret, sync_wait.sync_obj);
s - > buf . camera_bufs_metadata [ i ] . timestamp_eof = ( uint64_t ) nanos_since_boot ( ) ; // set true eof
if ( dp ) s - > buf . queue ( i ) ;
buf . camera_bufs_metadata [ i ] . timestamp_eof = ( uint64_t ) nanos_since_boot ( ) ; // set true eof
if ( dp ) buf . queue ( i ) ;
// destroy old output fence
struct cam_sync_info sync_destroy = { 0 } ;
strcpy ( sync_destroy . name , " NodeOutputPortFence " ) ;
sync_destroy . sync_obj = s - > s ync_objs[ i ] ;
ret = cam_control ( s - > multi_cam_state - > video1_fd , CAM_SYNC_DESTROY , & sync_destroy , sizeof ( sync_destroy ) ) ;
sync_destroy . sync_obj = sync_objs [ i ] ;
ret = do_ cam_control( multi_cam_state - > video1_fd , CAM_SYNC_DESTROY , & sync_destroy , sizeof ( sync_destroy ) ) ;
// LOGD("fence destroy: %d %d", ret, sync_destroy.sync_obj);
}
// do stuff
struct cam_req_mgr_sched_request req_mgr_sched_request = { 0 } ;
req_mgr_sched_request . session_hdl = s - > s ession_handle;
req_mgr_sched_request . link_hdl = s - > link_handle ;
req_mgr_sched_request . session_hdl = session_handle ;
req_mgr_sched_request . link_hdl = link_handle ;
req_mgr_sched_request . req_id = request_id ;
ret = cam_control ( s - > multi_cam_state - > video0_fd , CAM_REQ_MGR_SCHED_REQ , & req_mgr_sched_request , sizeof ( req_mgr_sched_request ) ) ;
ret = do_ cam_control( multi_cam_state - > video0_fd , CAM_REQ_MGR_SCHED_REQ , & req_mgr_sched_request , sizeof ( req_mgr_sched_request ) ) ;
// LOGD("sched req: %d %d", ret, request_id);
// create output fence
struct cam_sync_info sync_create = { 0 } ;
strcpy ( sync_create . name , " NodeOutputPortFence " ) ;
ret = cam_control ( s - > multi_cam_state - > video1_fd , CAM_SYNC_CREATE , & sync_create , sizeof ( sync_create ) ) ;
ret = do_ cam_control( multi_cam_state - > video1_fd , CAM_SYNC_CREATE , & sync_create , sizeof ( sync_create ) ) ;
// LOGD("fence req: %d %d", ret, sync_create.sync_obj);
s - > s ync_objs[ i ] = sync_create . sync_obj ;
sync_objs [ i ] = sync_create . sync_obj ;
// configure ISP to put the image in place
struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = { 0 } ;
mem_mgr_map_cmd . mmu_hdls [ 0 ] = s - > multi_cam_state - > device_iommu ;
mem_mgr_map_cmd . mmu_hdls [ 0 ] = multi_cam_state - > device_iommu ;
mem_mgr_map_cmd . num_hdl = 1 ;
mem_mgr_map_cmd . flags = CAM_MEM_FLAG_HW_READ_WRITE ;
mem_mgr_map_cmd . fd = s - > buf . camera_bufs [ i ] . fd ;
ret = cam_control ( s - > multi_cam_state - > video0_fd , CAM_REQ_MGR_MAP_BUF , & mem_mgr_map_cmd , sizeof ( mem_mgr_map_cmd ) ) ;
// LOGD("map buf req: (fd: %d) 0x%x %d", s-> bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
s - > buf_handle [ i ] = mem_mgr_map_cmd . out . buf_handle ;
mem_mgr_map_cmd . fd = buf . camera_bufs [ i ] . fd ;
ret = do_ cam_control( multi_cam_state - > video0_fd , CAM_REQ_MGR_MAP_BUF , & mem_mgr_map_cmd , sizeof ( mem_mgr_map_cmd ) ) ;
// LOGD("map buf req: (fd: %d) 0x%x %d", bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
buf_handle [ i ] = mem_mgr_map_cmd . out . buf_handle ;
// poke sensor
sensors_poke ( s , request_id ) ;
sensors_poke ( request_id ) ;
// LOGD("Poked sensor");
// push the buffer
config_isp ( s , s - > buf_handle [ i ] , s - > s ync_objs[ i ] , request_id , s - > buf0_handle , 65632 * ( i + 1 ) ) ;
config_isp ( buf_handle [ i ] , sync_objs [ i ] , request_id , buf0_handle , 65632 * ( i + 1 ) ) ;
}
void enqueue_req_multi ( struct CameraState * s , int start , int n , bool dp ) {
void CameraState : : enqueue_req_multi ( int start , int n , bool dp ) {
for ( int i = start ; i < start + n ; + + i ) {
s - > request_ids [ ( i - 1 ) % FRAME_BUF_COUNT ] = i ;
enqueue_buffer ( s , ( i - 1 ) % FRAME_BUF_COUNT , dp ) ;
request_ids [ ( i - 1 ) % FRAME_BUF_COUNT ] = i ;
enqueue_buffer ( ( i - 1 ) % FRAME_BUF_COUNT , dp ) ;
}
}
// ******************* camera *******************
static void camera_init ( MultiCameraState * multi_cam_state , VisionIpcServer * v , CameraState * s , int camera_id , int camera_num , unsigned int fps , cl_device_id device_id , cl_context ctx , VisionStreamType rgb_type , VisionStreamType yuv_type ) {
void CameraState : : camera_init ( MultiCameraState * multi_cam_state_ , VisionIpcServer * v , int camera_id , int camera_num_ , unsigned int fps , cl_device_id device_id , cl_context ctx , VisionStreamType rgb_type , VisionStreamType yuv_type ) {
LOGD ( " camera init %d " , camera_num ) ;
s - > multi_cam_state = multi_cam_state ;
multi_cam_state = multi_cam_state_ ;
assert ( camera_id < std : : size ( cameras_supported ) ) ;
s - > ci = cameras_supported [ camera_id ] ;
assert ( s - > ci . frame_width ! = 0 ) ;
ci = cameras_supported [ camera_id ] ;
assert ( ci . frame_width ! = 0 ) ;
s - > camera_num = camera_num ;
camera_num = camera_num_ ;
s - > request_id_last = 0 ;
s - > s kipped= true ;
request_id_last = 0 ;
skipped = true ;
s - > min_ev = EXPOSURE_TIME_MIN * sensor_analog_gains [ ANALOG_GAIN_MIN_IDX ] ;
s - > max_ev = EXPOSURE_TIME_MAX * sensor_analog_gains [ ANALOG_GAIN_MAX_IDX ] * DC_GAIN ;
s - > target_grey_fraction = 0.3 ;
min_ev = EXPOSURE_TIME_MIN * sensor_analog_gains [ ANALOG_GAIN_MIN_IDX ] ;
max_ev = EXPOSURE_TIME_MAX * sensor_analog_gains [ ANALOG_GAIN_MAX_IDX ] * DC_GAIN ;
target_grey_fraction = 0.3 ;
s - > dc_gain_enabled = false ;
s - > gain_idx = ANALOG_GAIN_REC_IDX ;
s - > exposure_time = 5 ;
s - > cur_ev [ 0 ] = s - > cur_ev [ 1 ] = s - > cur_ev [ 2 ] = ( s - > dc_gain_enabled ? DC_GAIN : 1 ) * sensor_analog_gains [ s - > gain_idx ] * s - > exposure_time ;
dc_gain_enabled = false ;
gain_idx = ANALOG_GAIN_REC_IDX ;
exposure_time = 5 ;
cur_ev [ 0 ] = cur_ev [ 1 ] = cur_ev [ 2 ] = ( dc_gain_enabled ? DC_GAIN : 1 ) * sensor_analog_gains [ gain_idx ] * exposure_time ;
s - > buf . init ( device_id , ctx , s , v , FRAME_BUF_COUNT , rgb_type , yuv_type ) ;
buf . init ( device_id , ctx , thi s, v , FRAME_BUF_COUNT , rgb_type , yuv_type ) ;
}
static void camera_open ( CameraState * s ) {
s - > s ensor_fd= open_v4l_by_name_and_index ( " cam-sensor-driver " , s - > camera_num ) ;
assert ( s - > s ensor_fd> = 0 ) ;
LOGD ( " opened sensor for %d " , s - > camera_num ) ;
void CameraState : : camera_open ( ) {
sensor_fd = open_v4l_by_name_and_index ( " cam-sensor-driver " , camera_num ) ;
assert ( sensor_fd > = 0 ) ;
LOGD ( " opened sensor for %d " , camera_num ) ;
// probe the sensor
LOGD ( " -- Probing sensor %d " , s - > camera_num ) ;
sensors_init ( s - > multi_cam_state - > video0_fd , s - > sensor_fd , s - > camera_num ) ;
LOGD ( " -- Probing sensor %d " , camera_num ) ;
sensors_init ( ) ;
// create session
struct cam_req_mgr_session_info session_info = { } ;
int ret = cam_control ( s - > multi_cam_state - > video0_fd , CAM_REQ_MGR_CREATE_SESSION , & session_info , sizeof ( session_info ) ) ;
int ret = do_ cam_control( multi_cam_state - > video0_fd , CAM_REQ_MGR_CREATE_SESSION , & session_info , sizeof ( session_info ) ) ;
LOGD ( " get session: %d 0x%X " , ret , session_info . session_hdl ) ;
s - > s ession_handle= session_info . session_hdl ;
session_handle = session_info . session_hdl ;
// access the sensor
LOGD ( " -- Accessing sensor " ) ;
auto sensor_dev_handle = device_acquire ( s - > sensor_fd , s - > session_handle , nullptr ) ;
assert ( sensor_dev_handle ) ;
s - > s ensor_dev_handle= * sensor_dev_handle ;
auto sensor_dev_handle_ = device_acquire ( sensor_fd , session_handle , nullptr ) ;
assert ( sensor_dev_handle_ ) ;
sensor_dev_handle = * sensor_dev_handle_ ;
LOGD ( " acquire sensor dev " ) ;
struct cam_isp_in_port_info in_port_info = {
. res_type = ( uint32_t [ ] ) { CAM_ISP_IFE_IN_RES_PHY_0 , CAM_ISP_IFE_IN_RES_PHY_1 , CAM_ISP_IFE_IN_RES_PHY_2 } [ s - > camera_num ] ,
. res_type = ( uint32_t [ ] ) { CAM_ISP_IFE_IN_RES_PHY_0 , CAM_ISP_IFE_IN_RES_PHY_1 , CAM_ISP_IFE_IN_RES_PHY_2 } [ camera_num ] ,
. lane_type = CAM_ISP_LANE_TYPE_DPHY ,
. lane_num = 4 ,
. lane_cfg = 0x3210 ,
. vc = 0x0 ,
// .dt = 0x2C; //CSI_RAW12
. dt = 0x2B , //CSI_RAW10
. format = CAM_FORMAT_MIPI_RAW_10 ,
. dt = 0x2C , // CSI_RAW12
. format = CAM_FORMAT_MIPI_RAW_12 ,
. test_pattern = 0x2 , // 0x3?
. usage_type = 0x0 ,
@ -636,7 +642,7 @@ static void camera_open(CameraState *s) {
. num_out_res = 0x1 ,
. data [ 0 ] = ( struct cam_isp_out_port_info ) {
. res_type = CAM_ISP_IFE_OUT_RES_RDI_0 ,
. format = CAM_FORMAT_MIPI_RAW_10 ,
. format = CAM_FORMAT_MIPI_RAW_12 ,
. width = FRAME_WIDTH ,
. height = FRAME_HEIGHT ,
. comp_grp_id = 0x0 , . split_point = 0x0 , . secure_mode = 0x0 ,
@ -649,29 +655,29 @@ static void camera_open(CameraState *s) {
. length = sizeof ( in_port_info ) ,
} ;
auto isp_dev_handle = device_acquire ( s - > multi_cam_state - > isp_fd , s - > session_handle , & isp_resource ) ;
assert ( isp_dev_handle ) ;
s - > isp_dev_handle = * isp_dev_handle ;
auto isp_dev_handle_ = device_acquire ( multi_cam_state - > isp_fd , session_handle , & isp_resource ) ;
assert ( isp_dev_handle_ ) ;
isp_dev_handle = * isp_dev_handle_ ;
LOGD ( " acquire isp dev " ) ;
s - > csiphy_fd = open_v4l_by_name_and_index ( " cam-csiphy-driver " , s - > camera_num ) ;
assert ( s - > csiphy_fd > = 0 ) ;
LOGD ( " opened csiphy for %d " , s - > camera_num ) ;
csiphy_fd = open_v4l_by_name_and_index ( " cam-csiphy-driver " , camera_num ) ;
assert ( csiphy_fd > = 0 ) ;
LOGD ( " opened csiphy for %d " , camera_num ) ;
struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = { . combo_mode = 0 } ;
auto csiphy_dev_handle = device_acquire ( s - > csiphy_fd , s - > session_handle , & csiphy_acquire_dev_info ) ;
assert ( csiphy_dev_handle ) ;
s - > csiphy_dev_handle = * csiphy_dev_handle ;
auto csiphy_dev_handle_ = device_acquire ( csiphy_fd , session_handle , & csiphy_acquire_dev_info ) ;
assert ( csiphy_dev_handle_ ) ;
csiphy_dev_handle = * csiphy_dev_handle_ ;
LOGD ( " acquire csiphy dev " ) ;
// config ISP
alloc_w_mmu_hdl ( s - > multi_cam_state - > video0_fd , 984480 , ( uint32_t * ) & s - > buf0_handle , 0x20 , CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE , s - > multi_cam_state - > device_iommu , s - > multi_cam_state - > cdm_iommu ) ;
config_isp ( s , 0 , 0 , 1 , s - > buf0_handle , 0 ) ;
alloc_w_mmu_hdl ( multi_cam_state - > video0_fd , 984480 , ( uint32_t * ) & buf0_handle , 0x20 , CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE , multi_cam_state - > device_iommu , multi_cam_state - > cdm_iommu ) ;
config_isp ( 0 , 0 , 1 , buf0_handle , 0 ) ;
LOG ( " -- Configuring sensor " ) ;
sensors_i2c ( s , init_array_ar0231 , std : : size ( init_array_ar0231 ) , CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG ) ;
//sensors_i2c(s, s tart_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
//sensors_i2c(s, s top_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
sensors_i2c ( init_array_ar0231 , std : : size ( init_array_ar0231 ) , CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG ) ;
//sensors_i2c(start_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
//sensors_i2c(stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
// config csiphy
@ -679,7 +685,7 @@ static void camera_open(CameraState *s) {
{
uint32_t cam_packet_handle = 0 ;
int size = sizeof ( struct cam_packet ) + sizeof ( struct cam_cmd_buf_desc ) * 1 ;
struct cam_packet * pkt = ( struct cam_packet * ) alloc_w_mmu_hdl ( s - > multi_cam_state - > video0_fd , size , & cam_packet_handle ) ;
struct cam_packet * pkt = ( struct cam_packet * ) alloc_w_mmu_hdl ( multi_cam_state - > video0_fd , size , & cam_packet_handle ) ;
pkt - > num_cmd_buf = 1 ;
pkt - > kmd_cmd_buf_index = - 1 ;
pkt - > header . size = size ;
@ -688,7 +694,7 @@ static void camera_open(CameraState *s) {
buf_desc [ 0 ] . size = buf_desc [ 0 ] . length = sizeof ( struct cam_csiphy_info ) ;
buf_desc [ 0 ] . type = CAM_CMD_BUF_GENERIC ;
struct cam_csiphy_info * csiphy_info = ( struct cam_csiphy_info * ) alloc_w_mmu_hdl ( s - > multi_cam_state - > video0_fd , buf_desc [ 0 ] . size , ( uint32_t * ) & buf_desc [ 0 ] . mem_handle ) ;
struct cam_csiphy_info * csiphy_info = ( struct cam_csiphy_info * ) alloc_w_mmu_hdl ( multi_cam_state - > video0_fd , buf_desc [ 0 ] . size , ( uint32_t * ) & buf_desc [ 0 ] . mem_handle ) ;
csiphy_info - > lane_mask = 0x1f ;
csiphy_info - > lane_assign = 0x3210 ; // skip clk. How is this 16 bit for 5 channels??
csiphy_info - > csiphy_3phase = 0x0 ; // no 3 phase, only 2 conductors per lane
@ -698,54 +704,51 @@ static void camera_open(CameraState *s) {
csiphy_info - > settle_time = MIPI_SETTLE_CNT * 200000000ULL ;
csiphy_info - > data_rate = 48000000 ; // Calculated by camera_freqs.py
int ret_ = device_config ( s - > csiphy_fd , s - > s ession_handle, s - > csiphy_dev_handle , cam_packet_handle ) ;
int ret_ = device_config ( csiphy_fd , session_handle , csiphy_dev_handle , cam_packet_handle ) ;
assert ( ret_ = = 0 ) ;
munmap ( csiphy_info , buf_desc [ 0 ] . size ) ;
release_fd ( s - > multi_cam_state - > video0_fd , buf_desc [ 0 ] . mem_handle ) ;
release_fd ( multi_cam_state - > video0_fd , buf_desc [ 0 ] . mem_handle ) ;
munmap ( pkt , size ) ;
release_fd ( s - > multi_cam_state - > video0_fd , cam_packet_handle ) ;
release_fd ( multi_cam_state - > video0_fd , cam_packet_handle ) ;
}
// link devices
LOG ( " -- Link devices " ) ;
struct cam_req_mgr_link_info req_mgr_link_info = { 0 } ;
req_mgr_link_info . session_hdl = s - > s ession_handle;
req_mgr_link_info . session_hdl = session_handle ;
req_mgr_link_info . num_devices = 2 ;
req_mgr_link_info . dev_hdls [ 0 ] = s - > isp_dev_handle ;
req_mgr_link_info . dev_hdls [ 1 ] = s - > s ensor_dev_handle;
ret = cam_control ( s - > multi_cam_state - > video0_fd , CAM_REQ_MGR_LINK , & req_mgr_link_info , sizeof ( req_mgr_link_info ) ) ;
s - > link_handle = req_mgr_link_info . link_hdl ;
LOGD ( " link: %d hdl: 0x%X " , ret , s - > link_handle ) ;
req_mgr_link_info . dev_hdls [ 0 ] = isp_dev_handle ;
req_mgr_link_info . dev_hdls [ 1 ] = sensor_dev_handle ;
ret = do_ cam_control( multi_cam_state - > video0_fd , CAM_REQ_MGR_LINK , & req_mgr_link_info , sizeof ( req_mgr_link_info ) ) ;
link_handle = req_mgr_link_info . link_hdl ;
LOGD ( " link: %d hdl: 0x%X " , ret , link_handle ) ;
struct cam_req_mgr_link_control req_mgr_link_control = { 0 } ;
req_mgr_link_control . ops = CAM_REQ_MGR_LINK_ACTIVATE ;
req_mgr_link_control . session_hdl = s - > s ession_handle;
req_mgr_link_control . session_hdl = session_handle ;
req_mgr_link_control . num_links = 1 ;
req_mgr_link_control . link_hdls [ 0 ] = s - > link_handle ;
ret = cam_control ( s - > multi_cam_state - > video0_fd , CAM_REQ_MGR_LINK_CONTROL , & req_mgr_link_control , sizeof ( req_mgr_link_control ) ) ;
req_mgr_link_control . link_hdls [ 0 ] = link_handle ;
ret = do_ cam_control( multi_cam_state - > video0_fd , CAM_REQ_MGR_LINK_CONTROL , & req_mgr_link_control , sizeof ( req_mgr_link_control ) ) ;
LOGD ( " link control: %d " , ret ) ;
ret = device_control ( s - > csiphy_fd , CAM_START_DEV , s - > s ession_handle, s - > csiphy_dev_handle ) ;
ret = device_control ( csiphy_fd , CAM_START_DEV , session_handle , csiphy_dev_handle ) ;
LOGD ( " start csiphy: %d " , ret ) ;
ret = device_control ( s - > multi_cam_state - > isp_fd , CAM_START_DEV , s - > s ession_handle, s - > isp_dev_handle ) ;
ret = device_control ( multi_cam_state - > isp_fd , CAM_START_DEV , session_handle , isp_dev_handle ) ;
LOGD ( " start isp: %d " , ret ) ;
ret = device_control ( s - > s ensor_fd, CAM_START_DEV , s - > s ession_handle, s - > sensor_dev_handle ) ;
ret = device_control ( sensor_fd , CAM_START_DEV , session_handle , sensor_dev_handle ) ;
LOGD ( " start sensor: %d " , ret ) ;
enqueue_req_multi ( s , 1 , FRAME_BUF_COUNT , 0 ) ;
enqueue_req_multi ( 1 , FRAME_BUF_COUNT , 0 ) ;
}
void cameras_init ( VisionIpcServer * v , MultiCameraState * s , cl_device_id device_id , cl_context ctx ) {
camera_init ( s , v , & s - > driver_cam , CAMERA_ID_AR0231 , 2 , 20 , device_id , ctx ,
VISION_STREAM_RGB_FRONT , VISION_STREAM_DRIVER ) ;
s - > driver_cam . camera_init ( s , v , CAMERA_ID_AR0231 , 2 , 20 , device_id , ctx , VISION_STREAM_RGB_DRIVER , VISION_STREAM_DRIVER ) ;
printf ( " driver camera initted \n " ) ;
if ( ! env_only_driver ) {
camera_init ( s , v , & s - > road_cam , CAMERA_ID_AR0231 , 1 , 20 , device_id , ctx ,
VISION_STREAM_RGB_BACK , VISION_STREAM_ROAD ) ; // swap left/right
s - > road_cam . camera_init ( s , v , CAMERA_ID_AR0231 , 1 , 20 , device_id , ctx , VISION_STREAM_RGB_ROAD , VISION_STREAM_ROAD ) ; // swap left/right
printf ( " road camera initted \n " ) ;
camera_init ( s , v , & s - > wide_road_cam , CAMERA_ID_AR0231 , 0 , 20 , device_id , ctx ,
VISION_STREAM_RGB_WIDE , VISION_STREAM_WIDE_ROAD ) ;
s - > wide_road_cam . camera_init ( s , v , CAMERA_ID_AR0231 , 0 , 20 , device_id , ctx , VISION_STREAM_RGB_WIDE_ROAD , VISION_STREAM_WIDE_ROAD ) ;
printf ( " wide road camera initted \n " ) ;
}
@ -768,7 +771,7 @@ void cameras_open(MultiCameraState *s) {
LOGD ( " opened video1 " ) ;
// looks like there's only one of these
s - > isp_fd = HANDLE_EINTR ( open ( " /dev/v4l-subdev1 " , O_RDWR | O_NONBLOCK ) ) ;
s - > isp_fd = open_v4l_by_name_and_index ( " cam-isp " ) ;
assert ( s - > isp_fd > = 0 ) ;
LOGD ( " opened isp " ) ;
@ -779,7 +782,7 @@ void cameras_open(MultiCameraState *s) {
query_cap_cmd . handle_type = 1 ;
query_cap_cmd . caps_handle = ( uint64_t ) & isp_query_cap_cmd ;
query_cap_cmd . size = sizeof ( isp_query_cap_cmd ) ;
ret = cam_control ( s - > isp_fd , CAM_QUERY_CAP , & query_cap_cmd , sizeof ( query_cap_cmd ) ) ;
ret = do_ cam_control( s - > isp_fd , CAM_QUERY_CAP , & query_cap_cmd , sizeof ( query_cap_cmd ) ) ;
assert ( ret = = 0 ) ;
LOGD ( " using MMU handle: %x " , isp_query_cap_cmd . device_iommu . non_secure ) ;
LOGD ( " using MMU handle: %x " , isp_query_cap_cmd . cdm_iommu . non_secure ) ;
@ -794,74 +797,72 @@ void cameras_open(MultiCameraState *s) {
ret = HANDLE_EINTR ( ioctl ( s - > video0_fd , VIDIOC_SUBSCRIBE_EVENT , & sub ) ) ;
printf ( " req mgr subscribe: %d \n " , ret ) ;
camera_open ( & s - > driver_cam ) ;
s - > driver_cam . camera_open ( ) ;
printf ( " driver camera opened \n " ) ;
if ( ! env_only_driver ) {
camera_open ( & s - > road_cam ) ;
s - > road_cam . camera_open ( ) ;
printf ( " road camera opened \n " ) ;
camera_open ( & s - > wide_road_cam ) ;
s - > wide_road_cam . camera_open ( ) ;
printf ( " wide road camera opened \n " ) ;
}
}
static void camera_close ( CameraState * s ) {
void CameraState : : camera_close ( ) {
int ret ;
// stop devices
LOG ( " -- Stop devices " ) ;
// ret = device_control(s->s ensor_fd, CAM_STOP_DEV, s->s ession_handle, s-> sensor_dev_handle);
// ret = device_control(sensor_fd, CAM_STOP_DEV, session_handle, sensor_dev_handle);
// LOGD("stop sensor: %d", ret);
ret = device_control ( s - > multi_cam_state - > isp_fd , CAM_STOP_DEV , s - > s ession_handle, s - > isp_dev_handle ) ;
ret = device_control ( multi_cam_state - > isp_fd , CAM_STOP_DEV , session_handle , isp_dev_handle ) ;
LOGD ( " stop isp: %d " , ret ) ;
ret = device_control ( s - > csiphy_fd , CAM_STOP_DEV , s - > s ession_handle, s - > csiphy_dev_handle ) ;
ret = device_control ( csiphy_fd , CAM_STOP_DEV , session_handle , csiphy_dev_handle ) ;
LOGD ( " stop csiphy: %d " , ret ) ;
// link control stop
LOG ( " -- Stop link control " ) ;
static struct cam_req_mgr_link_control req_mgr_link_control = { 0 } ;
req_mgr_link_control . ops = CAM_REQ_MGR_LINK_DEACTIVATE ;
req_mgr_link_control . session_hdl = s - > s ession_handle;
req_mgr_link_control . session_hdl = session_handle ;
req_mgr_link_control . num_links = 1 ;
req_mgr_link_control . link_hdls [ 0 ] = s - > link_handle ;
ret = cam_control ( s - > multi_cam_state - > video0_fd , CAM_REQ_MGR_LINK_CONTROL , & req_mgr_link_control , sizeof ( req_mgr_link_control ) ) ;
req_mgr_link_control . link_hdls [ 0 ] = link_handle ;
ret = do_ cam_control( multi_cam_state - > video0_fd , CAM_REQ_MGR_LINK_CONTROL , & req_mgr_link_control , sizeof ( req_mgr_link_control ) ) ;
LOGD ( " link control stop: %d " , ret ) ;
// unlink
LOG ( " -- Unlink " ) ;
static struct cam_req_mgr_unlink_info req_mgr_unlink_info = { 0 } ;
req_mgr_unlink_info . session_hdl = s - > s ession_handle;
req_mgr_unlink_info . link_hdl = s - > link_handle ;
ret = cam_control ( s - > multi_cam_state - > video0_fd , CAM_REQ_MGR_UNLINK , & req_mgr_unlink_info , sizeof ( req_mgr_unlink_info ) ) ;
req_mgr_unlink_info . session_hdl = session_handle ;
req_mgr_unlink_info . link_hdl = link_handle ;
ret = do_ cam_control( multi_cam_state - > video0_fd , CAM_REQ_MGR_UNLINK , & req_mgr_unlink_info , sizeof ( req_mgr_unlink_info ) ) ;
LOGD ( " unlink: %d " , ret ) ;
// release devices
LOGD ( " -- Release devices " ) ;
ret = device_control ( s - > s ensor_fd, CAM_RELEASE_DEV , s - > s ession_handle, s - > sensor_dev_handle ) ;
ret = device_control ( sensor_fd , CAM_RELEASE_DEV , session_handle , sensor_dev_handle ) ;
LOGD ( " release sensor: %d " , ret ) ;
ret = device_control ( s - > multi_cam_state - > isp_fd , CAM_RELEASE_DEV , s - > s ession_handle, s - > isp_dev_handle ) ;
ret = device_control ( multi_cam_state - > isp_fd , CAM_RELEASE_DEV , session_handle , isp_dev_handle ) ;
LOGD ( " release isp: %d " , ret ) ;
ret = device_control ( s - > csiphy_fd , CAM_RELEASE_DEV , s - > s ession_handle, s - > csiphy_dev_handle ) ;
ret = device_control ( csiphy_fd , CAM_RELEASE_DEV , session_handle , csiphy_dev_handle ) ;
LOGD ( " release csiphy: %d " , ret ) ;
// destroyed session
struct cam_req_mgr_session_info session_info = { . session_hdl = s - > s ession_handle} ;
ret = cam_control ( s - > multi_cam_state - > video0_fd , CAM_REQ_MGR_DESTROY_SESSION , & session_info , sizeof ( session_info ) ) ;
struct cam_req_mgr_session_info session_info = { . session_hdl = session_handle } ;
ret = do_ cam_control( multi_cam_state - > video0_fd , CAM_REQ_MGR_DESTROY_SESSION , & session_info , sizeof ( session_info ) ) ;
LOGD ( " destroyed session: %d " , ret ) ;
}
void cameras_close ( MultiCameraState * s ) {
camera_close ( & s - > driver_cam ) ;
s - > driver_cam . camera_close ( ) ;
if ( ! env_only_driver ) {
camera_close ( & s - > road_cam ) ;
camera_close ( & s - > wide_road_cam ) ;
s - > road_cam . camera_close ( ) ;
s - > wide_road_cam . camera_close ( ) ;
}
delete s - > sm ;
delete s - > pm ;
}
// ******************* just a helper *******************
void handle_camera_event ( CameraState * s , void * evdat ) {
void CameraState : : handle_camera_event ( void * evdat ) {
struct cam_req_mgr_message * event_data = ( struct cam_req_mgr_message * ) evdat ;
uint64_t timestamp = event_data - > u . frame_msg . timestamp ;
@ -869,53 +870,53 @@ void handle_camera_event(CameraState *s, void *evdat) {
int real_id = event_data - > u . frame_msg . request_id ;
if ( real_id ! = 0 ) { // next ready
if ( real_id = = 1 ) { s - > idx_offset = main_id ; }
if ( real_id = = 1 ) { idx_offset = main_id ; }
int buf_idx = ( real_id - 1 ) % FRAME_BUF_COUNT ;
// check for skipped frames
if ( main_id > s - > frame_id_last + 1 & & ! s - > skipped ) {
if ( main_id > frame_id_last + 1 & & ! skipped ) {
// realign
clear_req_queue ( s - > multi_cam_state - > video0_fd , event_data - > session_hdl , event_data - > u . frame_msg . link_hdl ) ;
enqueue_req_multi ( s , real_id + 1 , FRAME_BUF_COUNT - 1 , 0 ) ;
s - > s kipped= true ;
} else if ( main_id = = s - > frame_id_last + 1 ) {
s - > s kipped= false ;
clear_req_queue ( multi_cam_state - > video0_fd , event_data - > session_hdl , event_data - > u . frame_msg . link_hdl ) ;
enqueue_req_multi ( real_id + 1 , FRAME_BUF_COUNT - 1 , 0 ) ;
skipped = true ;
} else if ( main_id = = frame_id_last + 1 ) {
skipped = false ;
}
// check for dropped requests
if ( real_id > s - > request_id_last + 1 ) {
enqueue_req_multi ( s , s - > request_id_last + 1 + FRAME_BUF_COUNT , real_id - ( s - > request_id_last + 1 ) , 0 ) ;
if ( real_id > request_id_last + 1 ) {
enqueue_req_multi ( request_id_last + 1 + FRAME_BUF_COUNT , real_id - ( request_id_last + 1 ) , 0 ) ;
}
// metas
s - > frame_id_last = main_id ;
s - > request_id_last = real_id ;
frame_id_last = main_id ;
request_id_last = real_id ;
auto & meta_data = s - > buf . camera_bufs_metadata [ buf_idx ] ;
meta_data . frame_id = main_id - s - > idx_offset ;
auto & meta_data = buf . camera_bufs_metadata [ buf_idx ] ;
meta_data . frame_id = main_id - idx_offset ;
meta_data . timestamp_sof = timestamp ;
s - > exp_lock . lock ( ) ;
meta_data . gain = s - > dc_gain_enabled ? s - > analog_gain_frac * DC_GAIN : s - > analog_gain_frac ;
meta_data . high_conversion_gain = s - > dc_gain_enabled ;
meta_data . integ_lines = s - > exposure_time ;
meta_data . measured_grey_fraction = s - > measured_grey_fraction ;
meta_data . target_grey_fraction = s - > target_grey_fraction ;
s - > exp_lock . unlock ( ) ;
exp_lock . lock ( ) ;
meta_data . gain = dc_gain_enabled ? analog_gain_frac * DC_GAIN : analog_gain_frac ;
meta_data . high_conversion_gain = dc_gain_enabled ;
meta_data . integ_lines = exposure_time ;
meta_data . measured_grey_fraction = measured_grey_fraction ;
meta_data . target_grey_fraction = target_grey_fraction ;
exp_lock . unlock ( ) ;
// dispatch
enqueue_req_multi ( s , real_id + FRAME_BUF_COUNT , 1 , 1 ) ;
enqueue_req_multi ( real_id + FRAME_BUF_COUNT , 1 , 1 ) ;
} else { // not ready
// reset after half second of no response
if ( main_id > s - > frame_id_last + 10 ) {
clear_req_queue ( s - > multi_cam_state - > video0_fd , event_data - > session_hdl , event_data - > u . frame_msg . link_hdl ) ;
enqueue_req_multi ( s , s - > request_id_last + 1 , FRAME_BUF_COUNT , 0 ) ;
s - > frame_id_last = main_id ;
s - > s kipped= true ;
if ( main_id > frame_id_last + 10 ) {
clear_req_queue ( multi_cam_state - > video0_fd , event_data - > session_hdl , event_data - > u . frame_msg . link_hdl ) ;
enqueue_req_multi ( request_id_last + 1 , FRAME_BUF_COUNT , 0 ) ;
frame_id_last = main_id ;
skipped = true ;
}
}
}
static void set_camera_exposure ( CameraState * s , float grey_frac ) {
void CameraState : : set_camera_exposure ( float grey_frac ) {
const float dt = 0.05 ;
const float ts_grey = 10.0 ;
@ -929,15 +930,15 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
// Therefore we use the target EV from 3 frames ago, the grey fraction that was just measured was the result of that control action.
// TODO: Lower latency to 2 frames, by using the histogram outputed by the sensor we can do AE before the debayering is complete
const float cur_ev = s - > cur_ev [ s - > buf . cur_frame_data . frame_id % 3 ] ;
const float cur_ev_ = cur_ev [ buf . cur_frame_data . frame_id % 3 ] ;
// Scale target grey between 0.1 and 0.4 depending on lighting conditions
float new_target_grey = std : : clamp ( 0.4 - 0.3 * log2 ( 1.0 + cur_ev ) / log2 ( 6000.0 ) , 0.1 , 0.4 ) ;
float target_grey = ( 1.0 - k_grey ) * s - > target_grey_fraction + k_grey * new_target_grey ;
float new_target_grey = std : : clamp ( 0.4 - 0.3 * log2 ( 1.0 + cur_ev_ ) / log2 ( 6000.0 ) , 0.1 , 0.4 ) ;
float target_grey = ( 1.0 - k_grey ) * target_grey_fraction + k_grey * new_target_grey ;
float desired_ev = std : : clamp ( cur_ev * target_grey / grey_frac , s - > min_ev , s - > max_ev ) ;
float desired_ev = std : : clamp ( cur_ev_ * target_grey / grey_frac , min_ev , max_ev ) ;
float k = ( 1.0 - k_ev ) / 3.0 ;
desired_ev = ( k * s - > cur_ev [ 0 ] ) + ( k * s - > cur_ev [ 1 ] ) + ( k * s - > cur_ev [ 2 ] ) + ( k_ev * desired_ev ) ;
desired_ev = ( k * cur_ev [ 0 ] ) + ( k * cur_ev [ 1 ] ) + ( k * cur_ev [ 2 ] ) + ( k_ev * desired_ev ) ;
float best_ev_score = 1e6 ;
int new_g = 0 ;
@ -945,7 +946,7 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
// Hysteresis around high conversion gain
// We usually want this on since it results in lower noise, but turn off in very bright day scenes
bool enable_dc_gain = s - > dc_gain_enabled ;
bool enable_dc_gain = dc_gain_enabled ;
if ( ! enable_dc_gain & & target_grey < 0.2 ) {
enable_dc_gain = true ;
} else if ( enable_dc_gain & & target_grey > 0.3 ) {
@ -954,14 +955,14 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
// Simple brute force optimizer to choose sensor parameters
// to reach desired EV
for ( int g = std : : max ( ( int ) ANALOG_GAIN_MIN_IDX , s - > gain_idx - 1 ) ; g < = std : : min ( ( int ) ANALOG_GAIN_MAX_IDX , s - > gain_idx + 1 ) ; g + + ) {
for ( int g = std : : max ( ( int ) ANALOG_GAIN_MIN_IDX , gain_idx - 1 ) ; g < = std : : min ( ( int ) ANALOG_GAIN_MAX_IDX , gain_idx + 1 ) ; g + + ) {
float gain = sensor_analog_gains [ g ] * ( enable_dc_gain ? DC_GAIN : 1 ) ;
// Compute optimal time for given gain
int t = std : : clamp ( int ( std : : round ( desired_ev / gain ) ) , EXPOSURE_TIME_MIN , EXPOSURE_TIME_MAX ) ;
// Only go below recomended gain when absolutely necessary to not overexpose
if ( g < ANALOG_GAIN_REC_IDX & & t > 20 & & g < s - > gain_idx ) {
if ( g < ANALOG_GAIN_REC_IDX & & t > 20 & & g < gain_idx ) {
continue ;
}
@ -972,10 +973,10 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
float m = g > ANALOG_GAIN_REC_IDX ? 5.0 : 0.1 ;
score + = std : : abs ( g - ( int ) ANALOG_GAIN_REC_IDX ) * m ;
// LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", s-> camera_num, g, t, desired_ev / gain, score, score + std::abs(g - s-> gain_idx) * (score + 1.0) / 10.0, desired_ev, s-> min_ev);
// LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", camera_num, g, t, desired_ev / gain, score, score + std::abs(g - gain_idx) * (score + 1.0) / 10.0, desired_ev, min_ev);
// Small penalty on changing gain
score + = std : : abs ( g - s - > gain_idx ) * ( score + 1.0 ) / 10.0 ;
score + = std : : abs ( g - gain_idx ) * ( score + 1.0 ) / 10.0 ;
if ( score < best_ev_score ) {
new_t = t ;
@ -984,42 +985,41 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
}
}
s - > exp_lock . lock ( ) ;
exp_lock . lock ( ) ;
s - > measured_grey_fraction = grey_frac ;
s - > target_grey_fraction = target_grey ;
measured_grey_fraction = grey_frac ;
target_grey_fraction = target_grey ;
s - > analog_gain_frac = sensor_analog_gains [ new_g ] ;
s - > gain_idx = new_g ;
s - > exposure_time = new_t ;
s - > dc_gain_enabled = enable_dc_gain ;
analog_gain_frac = sensor_analog_gains [ new_g ] ;
gain_idx = new_g ;
exposure_time = new_t ;
dc_gain_enabled = enable_dc_gain ;
float gain = s - > analog_gain_frac * ( s - > dc_gain_enabled ? DC_GAIN : 1.0 ) ;
s - > cur_ev [ s - > buf . cur_frame_data . frame_id % 3 ] = s - > exposure_time * gain ;
float gain = analog_gain_frac * ( dc_gain_enabled ? DC_GAIN : 1.0 ) ;
cur_ev [ buf . cur_frame_data . frame_id % 3 ] = exposure_time * gain ;
s - > exp_lock . unlock ( ) ;
exp_lock . unlock ( ) ;
// Processing a frame takes right about 50ms, so we need to wait a few ms
// so we don't send i2c commands around the frame start.
int ms = ( nanos_since_boot ( ) - s - > buf . cur_frame_data . timestamp_sof ) / 1000000 ;
int ms = ( nanos_since_boot ( ) - buf . cur_frame_data . timestamp_sof ) / 1000000 ;
if ( ms < 60 ) {
util : : sleep_for ( 60 - ms ) ;
}
// LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", s-> camera_num, 1e-9 * nanos_since_boot(), 1e-9 * s-> buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - s-> buf.cur_frame_data.timestamp_sof));
// LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera_num, 1e-9 * nanos_since_boot(), 1e-9 * buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - buf.cur_frame_data.timestamp_sof));
uint16_t analog_gain_reg = 0xFF00 | ( new_g < < 4 ) | new_g ;
struct i2c_random_wr_payload exp_reg_array [ ] = {
{ 0x3366 , analog_gain_reg } ,
{ 0x3362 , ( uint16_t ) ( s - > dc_gain_enabled ? 0x1 : 0x0 ) } ,
{ 0x3012 , ( uint16_t ) s - > exposure_time } ,
{ 0x3362 , ( uint16_t ) ( dc_gain_enabled ? 0x1 : 0x0 ) } ,
{ 0x3012 , ( uint16_t ) exposure_time } ,
} ;
sensors_i2c ( s , exp_reg_array , sizeof ( exp_reg_array ) / sizeof ( struct i2c_random_wr_payload ) ,
sensors_i2c ( exp_reg_array , sizeof ( exp_reg_array ) / sizeof ( struct i2c_random_wr_payload ) ,
CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG ) ;
}
void camera_autoexposure ( CameraState * s , float grey_frac ) {
set_camera_exposure ( s , grey_frac ) ;
s - > set_camera_exposure ( grey_frac ) ;
}
// called by processing_thread
@ -1053,11 +1053,10 @@ void cameras_run(MultiCameraState *s) {
// start devices
LOG ( " -- Starting devices " ) ;
int start_reg_len = sizeof ( start_reg_array ) / sizeof ( struct i2c_random_wr_payload ) ;
sensors_i2c ( & s - > driver_cam , start_reg_array , start_reg_len , CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG ) ;
s - > driver_cam . sensors_start ( ) ;
if ( ! env_only_driver ) {
sensors_i2c ( & s - > road_cam , start_reg_array , start_reg_len , CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG ) ;
sensors_i2c ( & s - > wide_road_cam , start_reg_array , start_reg_len , CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG ) ;
s - > road_cam . sensors_start ( ) ;
s - > wide_road_cam . sensors_start ( ) ;
}
// poll events
@ -1088,11 +1087,11 @@ void cameras_run(MultiCameraState *s) {
}
if ( event_data - > session_hdl = = s - > road_cam . session_handle ) {
handle_camera_event ( & s - > road_cam , event_data ) ;
s - > road_cam . handle_camera_event ( event_data ) ;
} else if ( event_data - > session_hdl = = s - > wide_road_cam . session_handle ) {
handle_camera_event ( & s - > wide_road_cam , event_data ) ;
s - > wide_road_cam . handle_camera_event ( event_data ) ;
} else if ( event_data - > session_hdl = = s - > driver_cam . session_handle ) {
handle_camera_event ( & s - > driver_cam , event_data ) ;
s - > driver_cam . handle_camera_event ( event_data ) ;
} else {
printf ( " Unknown vidioc event source \n " ) ;
assert ( false ) ;
Shane Smiskol
3 years ago