@ -22,18 +22,13 @@
# include "selfdrive/common/swaglog.h"
# include "selfdrive/camerad/cameras/sensor2_i2c.h"
# define FRAME_WIDTH 1928
# define FRAME_HEIGHT 1208
//#define FRAME_STRIDE 1936 // for 8 bit output
# define FRAME_STRIDE 2416 // for 10 bit output
//#define FRAME_STRIDE 1936 // for 8 bit output
# define MIPI_SETTLE_CNT 33 // Calculated by camera_freqs.py
extern ExitHandler do_exit ;
// global var for AE ops
std : : atomic < CameraExpInfo > cam_exp [ 3 ] = { { { 0 } } } ;
const size_t FRAME_WIDTH = 1928 ;
const size_t FRAME_HEIGHT = 1208 ;
const size_t FRAME_STRIDE = 2416 ; // for 10 bit output
const int MIPI_SETTLE_CNT = 33 ; // Calculated by camera_freqs.py
CameraInfo cameras_supported [ CAMERA_ID_MAX ] = {
[ CAMERA_ID_AR0231 ] = {
@ -46,12 +41,25 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = {
} ,
} ;
float sensor_analog_gains [ ANALOG_GAIN_MAX_IDX ] = { 3.0 / 6.0 , 4.0 / 6.0 , 4.0 / 5.0 , 5.0 / 5.0 ,
5.0 / 4.0 , 6.0 / 4.0 , 6.0 / 3.0 , 7.0 / 3.0 ,
7.0 / 2.0 , 8.0 / 2.0 } ;
const bool enable_dc_gain = true ;
const float DC_GAIN = 2.5 ;
const float sensor_analog_gains [ ] = {
1.0 / 8.0 , 2.0 / 8.0 , 2.0 / 7.0 , 3.0 / 7.0 , // 0, 1, 2, 3
3.0 / 6.0 , 4.0 / 6.0 , 4.0 / 5.0 , 5.0 / 5.0 , // 4, 5, 6, 7
5.0 / 4.0 , 6.0 / 4.0 , 6.0 / 3.0 , 7.0 / 3.0 , // 8, 9, 10, 11
7.0 / 2.0 , 8.0 / 2.0 , 8.0 / 1.0 } ; // 12, 13, 14, 15 = bypass
// ************** low level camera helpers ****************
const int ANALOG_GAIN_MIN_IDX = 0x0 ; // 0.125x
const int ANALOG_GAIN_REC_IDX = 0x7 ; // 1.0x
const int ANALOG_GAIN_MAX_IDX = 0xD ; // 4.0x
const int EXPOSURE_TIME_MIN = 2 ; // with HDR, fastest ss
const int EXPOSURE_TIME_MAX = 1904 ; // with HDR, slowest ss
// global var for AE ops
std : : atomic < CameraExpInfo > cam_exp [ 3 ] = { { { 0 } } } ;
// ************** low level camera helpers ****************
int cam_control ( int fd , int op_code , void * handle , int size ) {
struct cam_control camcontrol = { 0 } ;
camcontrol . op_code = op_code ;
@ -521,14 +529,16 @@ static void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v,
s - > camera_num = camera_num ;
s - > dc_gain_enabled = false ;
s - > analog_gain = 0x5 ;
s - > analog_gain_frac = sensor_analog_gains [ s - > analog_gain ] ;
s - > exposure_time = 256 ;
s - > exposure_time_max = 1.2 * EXPOSURE_TIME_MAX / 2 ;
s - > exposure_time_min = 0.75 * EXPOSURE_TIME_MIN * 2 ;
s - > request_id_last = 0 ;
s - > skipped = true ;
s - > ef_filtered = 1.0 ;
s - > min_ev = EXPOSURE_TIME_MIN * sensor_analog_gains [ ANALOG_GAIN_MIN_IDX ] * ( enable_dc_gain ? DC_GAIN : 1 ) ;
s - > max_ev = EXPOSURE_TIME_MAX * sensor_analog_gains [ ANALOG_GAIN_MAX_IDX ] * DC_GAIN ;
s - > cur_ev = ( s - > max_ev - s - > min_ev ) / 2 ;
s - > target_grey_fraction = 0.3 ;
s - > gain_idx = ANALOG_GAIN_MIN_IDX ;
s - > buf . init ( device_id , ctx , s , v , FRAME_BUF_COUNT , rgb_type , yuv_type ) ;
}
@ -905,7 +915,7 @@ void handle_camera_event(CameraState *s, void *evdat) {
meta_data . frame_id = main_id - s - > idx_offset ;
meta_data . timestamp_sof = timestamp ;
s - > exp_lock . lock ( ) ;
meta_data . gain = s - > dc_gain_enabled ? s - > analog_gain_frac * 2.5 : s - > analog_gain_frac ;
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 ;
@ -925,100 +935,75 @@ void handle_camera_event(CameraState *s, void *evdat) {
}
}
// ******************* exposure control helpers *******************
static void set_camera_exposure ( CameraState * s , float grey_frac ) {
const float dt = 0.15 ;
void set_exposure_time_bounds ( CameraState * s ) {
switch ( s - > analog_gain ) {
case 0 : {
s - > exposure_time_min = EXPOSURE_TIME_MIN ;
s - > exposure_time_max = EXPOSURE_TIME_MAX ; // EXPOSURE_TIME_MIN * 4;
break ;
}
case ANALOG_GAIN_MAX_IDX - 1 : {
s - > exposure_time_min = EXPOSURE_TIME_MIN ; // EXPOSURE_TIME_MAX / 4;
s - > exposure_time_max = EXPOSURE_TIME_MAX ;
break ;
}
default : {
// finetune margins on both ends
float k_up = sensor_analog_gains [ s - > analog_gain + 1 ] / sensor_analog_gains [ s - > analog_gain ] ;
float k_down = sensor_analog_gains [ s - > analog_gain - 1 ] / sensor_analog_gains [ s - > analog_gain ] ;
s - > exposure_time_min = k_down * EXPOSURE_TIME_MIN * 2 ;
s - > exposure_time_max = k_up * EXPOSURE_TIME_MAX / 2 ;
}
}
}
const float ts_grey = 10.0 ;
const float ts_ev = 0.05 ;
void switch_conversion_gain ( CameraState * s ) {
if ( ! s - > dc_gain_enabled ) {
s - > dc_gain_enabled = true ;
s - > analog_gain - = 4 ;
} else {
s - > dc_gain_enabled = false ;
s - > analog_gain + = 4 ;
}
}
const float k_grey = ( dt / ts_grey ) / ( 1.0 + dt / ts_grey ) ;
const float k_ev = ( dt / ts_ev ) / ( 1.0 + dt / ts_ev ) ;
static void set_camera_exposure ( CameraState * s , float grey_frac ) {
// TODO: get stats from sensor?
float target_grey = 0.4 - ( ( float ) ( s - > analog_gain + 4 * s - > dc_gain_enabled ) / 48.0f ) ;
float exposure_factor = 1 + 30 * pow ( ( target_grey - grey_frac ) , 3 ) ;
exposure_factor = std : : max ( exposure_factor , 0.56f ) ;
if ( s - > camera_num ! = 1 ) {
s - > ef_filtered = ( 1 - EF_LOWPASS_K ) * s - > ef_filtered + EF_LOWPASS_K * exposure_factor ;
exposure_factor = s - > ef_filtered ;
}
// Scale target grey between 0.2 and 0.4 depending on lighting conditions
float new_target_grey = std : : clamp ( 0.4 - 0.2 * log2 ( 1.0 + s - > cur_ev ) / log2 ( 6000.0 ) , 0.2 , 0.4 ) ;
float target_grey = ( 1.0 - k_grey ) * s - > target_grey_fraction + k_grey * new_target_grey ;
s - > exp_lock . lock ( ) ;
s - > measured_grey_fraction = grey_frac ;
s - > target_grey_fraction = target_grey ;
float desired_ev = std : : clamp ( s - > cur_ev * target_grey / grey_frac , s - > min_ev , s - > max_ev ) ;
desired_ev = ( 1.0 - k_ev ) * s - > cur_ev + k_ev * desired_ev ;
// always prioritize exposure time adjust
s - > exposure_time * = exposure_factor ;
float best_ev_score = 1e6 ;
int new_g = 0 ;
int new_t = 0 ;
// switch gain if max/min exposure time is reached
// or always switch down to a lower gain when possible
bool kd = false ;
if ( s - > analog_gain > 0 ) {
kd = 1.1 * s - > exposure_time / ( sensor_analog_gains [ s - > analog_gain - 1 ] / sensor_analog_gains [ s - > analog_gain ] ) < EXPOSURE_TIME_MAX / 2 ;
}
// Simple brute force optimizer to choose sensor parameters
// to reach desired EV
for ( int g = ANALOG_GAIN_MIN_IDX ; g < = ANALOG_GAIN_MAX_IDX ; g + + ) {
float gain = sensor_analog_gains [ g ] * ( enable_dc_gain ? DC_GAIN : 1 ) ;
if ( s - > exposure_time > s - > exposure_time_max ) {
if ( s - > analog_gain < ANALOG_GAIN_MAX_IDX - 1 ) {
s - > exposure_time = EXPOSURE_TIME_MAX / 2 ;
s - > analog_gain + = 1 ;
if ( ! s - > dc_gain_enabled & & sensor_analog_gains [ s - > analog_gain ] > = 4.0 ) { // switch to HCG
switch_conversion_gain ( s ) ;
}
set_exposure_time_bounds ( s ) ;
} else {
s - > exposure_time = s - > exposure_time_max ;
}
} else if ( s - > exposure_time < s - > exposure_time_min | | kd ) {
if ( s - > analog_gain > 0 ) {
s - > exposure_time = std : : max ( EXPOSURE_TIME_MIN * 2 , ( int ) ( s - > exposure_time / ( sensor_analog_gains [ s - > analog_gain - 1 ] / sensor_analog_gains [ s - > analog_gain ] ) ) ) ;
s - > analog_gain - = 1 ;
if ( s - > dc_gain_enabled & & sensor_analog_gains [ s - > analog_gain ] < = 1.25 ) { // switch back to LCG
switch_conversion_gain ( s ) ;
// 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
// behavior is not completely linear causing visible step changes when switching gain
if ( g < ANALOG_GAIN_REC_IDX & & t > 10 ) {
continue ;
}
set_exposure_time_bounds ( s ) ;
} else {
s - > exposure_time = s - > exposure_time_min ;
// Compute error to desired ev
float score = std : : abs ( desired_ev - ( t * gain ) ) * 10 ;
// Going below recomended gain needs lower penalty to not overexpose
float m = g > ANALOG_GAIN_REC_IDX ? 5.0 : 1.0 ;
score + = std : : abs ( g - ( int ) ANALOG_GAIN_REC_IDX ) * m ;
// Small penalty on changing gain
score + = std : : abs ( g - s - > gain_idx ) ;
if ( score < best_ev_score ) {
new_t = t ;
new_g = g ;
best_ev_score = score ;
}
}
// set up config
uint16_t AG = s - > analog_gain + 4 ;
AG = 0xFF00 + AG * 16 + AG ;
s - > analog_gain_frac = sensor_analog_gains [ s - > analog_gain ] ;
s - > exp_lock . lock ( ) ;
s - > measured_grey_fraction = grey_frac ;
s - > 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 ;
float gain = s - > analog_gain_frac * ( s - > dc_gain_enabled ? DC_GAIN : 1.0 ) ;
s - > cur_ev = s - > exposure_time * gain ;
s - > exp_lock . unlock ( ) ;
// printf("cam %d, min %d, max %d \n", s->camera_num, s->exposure_time_min, s->exposure_time_max);
// printf("cam %d, set AG to 0x%X, S to %d, dc %d \n", s->camera_num, AG, s->exposure_time, s->dc_gain_enabled);
uint16_t analog_gain_reg = 0xFF00 | ( new_g < < 4 ) | new_g ;
struct i2c_random_wr_payload exp_reg_array [ ] = {
{ 0x3366 , AG } , // analog gain
{ 0x3366 , analog_gain_reg } , // analog gain
{ 0x3362 , ( uint16_t ) ( s - > dc_gain_enabled ? 0x1 : 0x0 ) } , // DC_GAIN
{ 0x305A , 0x00F8 } , // red gain
{ 0x3058 , 0x0122 } , // blue gain
@ -1081,7 +1066,7 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
if ( cnt % 3 = = 0 ) {
const auto [ x , y , w , h ] = ( c = = & s - > wide_road_cam ) ? std : : tuple ( 96 , 250 , 1734 , 524 ) : std : : tuple ( 96 , 160 , 1734 , 986 ) ;
const int skip = 2 ;
camera_autoexposure ( c , set_exposure_target ( b , x , x + w , skip , y , y + h , skip , ( int ) c - > analog_gain , true , true ) ) ;
camera_autoexposure ( c , set_exposure_target ( b , x , x + w , skip , y , y + h , skip ) ) ;
}
}
Willem Melching
4 years ago
committed by
GitHub