diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index a8e8ebecf0..1cfff20088 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -445,9 +445,10 @@ void hardware_control_thread() { if (sm.updated("driverCameraState")) { auto event = sm["driverCameraState"]; int cur_integ_lines = event.getDriverCameraState().getIntegLines(); + float cur_gain = event.getDriverCameraState().getGain(); if (Hardware::TICI()) { - cur_integ_lines = integ_lines_filter.update(cur_integ_lines); + cur_integ_lines = integ_lines_filter.update(cur_integ_lines * cur_gain); } last_front_frame_t = event.getLogMonoTime(); diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index 4cc8b8fcae..89052e5e14 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -127,8 +127,6 @@ bool CameraBuf::acquire() { const size_t globalWorkSize[] = {size_t(camera_state->ci.frame_width), size_t(camera_state->ci.frame_height)}; const size_t localWorkSize[] = {DEBAYER_LOCAL_WORKSIZE, DEBAYER_LOCAL_WORKSIZE}; CL_CHECK(clSetKernelArg(krnl_debayer, 2, localMemSize, 0)); - int ggain = camera_state->analog_gain + 4*camera_state->dc_gain_enabled; - CL_CHECK(clSetKernelArg(krnl_debayer, 3, sizeof(int), &ggain)); CL_CHECK(clEnqueueNDRangeKernel(q, krnl_debayer, 2, NULL, globalWorkSize, localWorkSize, 0, 0, &debayer_event)); #else @@ -280,39 +278,30 @@ static void publish_thumbnail(PubMaster *pm, const CameraBuf *b) { free(thumbnail_buffer); } -float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip, int analog_gain, bool hist_ceil, bool hl_weighted) { - const uint8_t *pix_ptr = b->cur_yuv_buf->y; +float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) { + int lum_med; uint32_t lum_binning[256] = {0}; + const uint8_t *pix_ptr = b->cur_yuv_buf->y; + unsigned int lum_total = 0; for (int y = y_start; y < y_end; y += y_skip) { for (int x = x_start; x < x_end; x += x_skip) { uint8_t lum = pix_ptr[(y * b->rgb_width) + x]; - if (hist_ceil && lum < 80 && lum_binning[lum] > HISTO_CEIL_K * (y_end - y_start) * (x_end - x_start) / x_skip / y_skip / 256) { - continue; - } lum_binning[lum]++; lum_total += 1; } } + + // Find mean lumimance value unsigned int lum_cur = 0; - int lum_med = 0; - int lum_med_alt = 0; - for (lum_med=255; lum_med>=0; lum_med--) { + for (lum_med = 255; lum_med >= 0; lum_med--) { lum_cur += lum_binning[lum_med]; - if (hl_weighted) { - int lum_med_tmp = 0; - int hb = HLC_THRESH + (10 - analog_gain); - if (lum_cur > 0 && lum_med > hb) { - lum_med_tmp = (lum_med - hb) + 100; - } - lum_med_alt = lum_med_alt>lum_med_tmp?lum_med_alt:lum_med_tmp; - } + if (lum_cur >= lum_total / 2) { break; } } - lum_med = lum_med_alt>0 ? lum_med + lum_med/32*lum_cur*abs(lum_med_alt - lum_med)/lum_total:lum_med; return lum_med / 256.0; } @@ -355,18 +344,12 @@ static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) { struct ExpRect {int x1, x2, x_skip, y1, y2, y_skip;}; const CameraBuf *b = &c->buf; - bool hist_ceil = false, hl_weighted = false; int x_offset = 0, y_offset = 0; int frame_width = b->rgb_width, frame_height = b->rgb_height; -#ifndef QCOM2 - int analog_gain = -1; -#else - int analog_gain = c->analog_gain; -#endif + ExpRect def_rect; if (Hardware::TICI()) { - hist_ceil = hl_weighted = true; x_offset = 630, y_offset = 156; frame_width = 668, frame_height = frame_width / 1.33; def_rect = {96, 1832, 2, 242, 1148, 4}; @@ -377,7 +360,7 @@ static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) { static ExpRect rect = def_rect; // use driver face crop for AE - if (sm.updated("driverState")) { + if (Hardware::EON() && sm.updated("driverState")) { if (auto state = sm["driverState"].getDriverState(); state.getFaceProb() > 0.4) { auto face_position = state.getFacePosition(); int x = is_rhd ? 0 : frame_width - (0.5 * frame_height); @@ -385,16 +368,15 @@ static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) { int y = (face_position[1] + 0.5) * frame_height + y_offset; rect = {std::max(0, x - 72), std::min(b->rgb_width - 1, x + 72), 2, std::max(0, y - 72), std::min(b->rgb_height - 1, y + 72), 1}; - } else { - rect = def_rect; } } - camera_autoexposure(c, set_exposure_target(b, rect.x1, rect.x2, rect.x_skip, rect.y1, rect.y2, rect.y_skip, analog_gain, hist_ceil, hl_weighted)); + camera_autoexposure(c, set_exposure_target(b, rect.x1, rect.x2, rect.x_skip, rect.y1, rect.y2, rect.y_skip)); } void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt) { - if (cnt % 3 == 0) { + int j = Hardware::TICI() ? 1 : 3; + if (cnt % j == 0) { sm->update(0); driver_cam_auto_exposure(c, *sm); } diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 17051de408..16b5942d07 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -27,6 +27,7 @@ #define CAMERA_ID_MAX 9 #define UI_BUF_COUNT 4 + #define LOG_CAMERA_ID_FCAMERA 0 #define LOG_CAMERA_ID_DCAMERA 1 #define LOG_CAMERA_ID_ECAMERA 2 @@ -134,7 +135,7 @@ typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt); void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data); kj::Array get_frame_image(const CameraBuf *b); -float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip, int analog_gain, bool hist_ceil, bool hl_weighted); +float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip); std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback); void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt); diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc index 5ac90e4b3d..7b350215d1 100644 --- a/selfdrive/camerad/cameras/camera_qcom.cc +++ b/selfdrive/camerad/cameras/camera_qcom.cc @@ -1113,7 +1113,7 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { if (cnt % 3 == 0) { const int x = 290, y = 322, width = 560, height = 314; const int skip = 1; - camera_autoexposure(c, set_exposure_target(b, x, x + width, skip, y, y + height, skip, -1, false, false)); + camera_autoexposure(c, set_exposure_target(b, x, x + width, skip, y, y + height, skip)); } } diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index ff512e6585..8cb430bfaa 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -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 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,24 @@ 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 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 = 0x1; // 0.25x +const int ANALOG_GAIN_REC_IDX = 0x6; // 0.8x +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 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; @@ -520,15 +527,17 @@ 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]; + s->max_ev = EXPOSURE_TIME_MAX * sensor_analog_gains[ANALOG_GAIN_MAX_IDX] * DC_GAIN; + s->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; s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type); } @@ -905,7 +914,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,139 +934,113 @@ 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.05; -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; - } + // It takes 3 frames for the commanded exposure settings to take effect. The first frame is already started by the time + // we reach this function, the other 2 are due to the register buffering in the sensor. + // 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 - s->exp_lock.lock(); - s->measured_grey_fraction = grey_frac; - s->target_grey_fraction = target_grey; + const float cur_ev = s->cur_ev[s->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; - // always prioritize exposure time adjust - s->exposure_time *= exposure_factor; + float desired_ev = std::clamp(cur_ev * target_grey / grey_frac, s->min_ev, s->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); - // 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; + float best_ev_score = 1e6; + int new_g = 0; + int new_t = 0; + + // 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; + if (!enable_dc_gain && target_grey < 0.2) { + enable_dc_gain = true; + } else if (enable_dc_gain && target_grey > 0.3) { + enable_dc_gain = false; } - 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; + // 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++) { + 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) { + continue; } - } 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); - } - 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 : 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); + + // Small penalty on changing gain + score += std::abs(g - s->gain_idx) * (score + 1.0) / 10.0; + + 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->buf.cur_frame_data.frame_id % 3] = 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); + // 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; + 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)); + + uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g; struct i2c_random_wr_payload exp_reg_array[] = { - {0x3366, AG}, // analog gain - {0x3362, (uint16_t)(s->dc_gain_enabled?0x1:0x0)}, // DC_GAIN - {0x305A, 0x00F8}, // red gain - {0x3058, 0x0122}, // blue gain - {0x3056, 0x009A}, // g1 gain - {0x305C, 0x009A}, // g2 gain - {0x3012, (uint16_t)s->exposure_time}, // integ time - }; - //{0x301A, 0x091C}}; // reset + {0x3366, analog_gain_reg}, + {0x3362, (uint16_t)(s->dc_gain_enabled ? 0x1 : 0x0)}, + {0x3012, (uint16_t)s->exposure_time}, + }; sensors_i2c(s, exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), - CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); + CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); + } void camera_autoexposure(CameraState *s, float grey_frac) { - CameraExpInfo tmp = cam_exp[s->camera_num].load(); - tmp.op_id++; - tmp.grey_frac = grey_frac; - cam_exp[s->camera_num].store(tmp); + set_camera_exposure(s, grey_frac); } -static void ae_thread(MultiCameraState *s) { - CameraState *c_handles[3] = {&s->wide_road_cam, &s->road_cam, &s->driver_cam}; - - int op_id_last[3] = {0}; - CameraExpInfo cam_op[3]; - - set_thread_name("camera_settings"); - - while(!do_exit) { - for (int i=0;i<3;i++) { - cam_op[i] = cam_exp[i].load(); - if (cam_op[i].op_id != op_id_last[i]) { - set_camera_exposure(c_handles[i], cam_op[i].grey_frac); - op_id_last[i] = cam_op[i].op_id; - } - } - - util::sleep_for(50); - } -} void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) { common_process_driver_camera(s->sm, s->pm, c, cnt); @@ -1078,17 +1061,14 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { } s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg); - 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)); - } + 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)); } void cameras_run(MultiCameraState *s) { LOG("-- Starting threads"); std::vector threads; - threads.push_back(std::thread(ae_thread, s)); threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera)); threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera)); threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera)); diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h index 469a2f8580..c7dd59787d 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.h +++ b/selfdrive/camerad/cameras/camera_qcom2.h @@ -10,30 +10,23 @@ #include "selfdrive/common/util.h" #define FRAME_BUF_COUNT 4 - -#define ANALOG_GAIN_MAX_IDX 10 // 0xF is bypass -#define EXPOSURE_TIME_MIN 2 // with HDR, fastest ss -#define EXPOSURE_TIME_MAX 1904 // with HDR, slowest ss - -#define EF_LOWPASS_K 0.35 - #define DEBAYER_LOCAL_WORKSIZE 16 - typedef struct CameraState { MultiCameraState *multi_cam_state; CameraInfo ci; std::mutex exp_lock; - float analog_gain_frac; - uint16_t analog_gain; - bool dc_gain_enabled; + int exposure_time; - int exposure_time_min; - int exposure_time_max; - float ef_filtered; + bool dc_gain_enabled; + float analog_gain_frac; + + float cur_ev[3]; + float min_ev, max_ev; float measured_grey_fraction; float target_grey_fraction; + int gain_idx; unique_fd sensor_fd; unique_fd csiphy_fd; diff --git a/selfdrive/camerad/cameras/real_debayer.cl b/selfdrive/camerad/cameras/real_debayer.cl index 10cb5ae760..fe6a99f373 100644 --- a/selfdrive/camerad/cameras/real_debayer.cl +++ b/selfdrive/camerad/cameras/real_debayer.cl @@ -26,9 +26,9 @@ half mf(half x, half cp) { } } -half3 color_correct(half3 rgb, int ggain) { +half3 color_correct(half3 rgb) { half3 ret = (0,0,0); - half cpx = 0.01; //clamp(0.01h, 0.05h, cpxb + cpxk * min(10, ggain)); + half cpx = 0.01; ret += (half)rgb.x * color_correction[0]; ret += (half)rgb.y * color_correction[1]; ret += (half)rgb.z * color_correction[2]; @@ -89,8 +89,7 @@ half phi(half x) { __kernel void debayer10(const __global uchar * in, __global uchar * out, - __local half * cached, - uint ggain + __local half * cached ) { const int x_global = get_global_id(0); @@ -200,10 +199,9 @@ __kernel void debayer10(const __global uchar * in, } rgb = clamp(0.0h, 1.0h, rgb); - rgb = color_correct(rgb, (int)ggain); + rgb = color_correct(rgb); out[out_idx + 0] = (uchar)(rgb.z); out[out_idx + 1] = (uchar)(rgb.y); out[out_idx + 2] = (uchar)(rgb.x); - } diff --git a/selfdrive/camerad/cameras/sensor2_i2c.h b/selfdrive/camerad/cameras/sensor2_i2c.h index 38a05820f4..c3d8861a97 100644 --- a/selfdrive/camerad/cameras/sensor2_i2c.h +++ b/selfdrive/camerad/cameras/sensor2_i2c.h @@ -14,13 +14,19 @@ struct i2c_random_wr_payload init_array_ar0231[] = { // FORMAT {0x3040, 0xC000}, // READ_MODE - {0x3004, 0x0000}, // X_ADDR_START_ - {0x3008, 0x0787}, // X_ADDR_END_ - {0x3002, 0x0000}, // Y_ADDR_START_ - {0x3006, 0x04B7}, // Y_ADDR_END_ + {0x3004, 0x0000}, // X_ADDR_START_ (A) + {0x308A, 0x0000}, // X_ADDR_START_ (B) + {0x3008, 0x0787}, // X_ADDR_END_ (A) + {0x308E, 0x0787}, // X_ADDR_END_ (B) + {0x3002, 0x0000}, // Y_ADDR_START_ (A) + {0x308C, 0x0000}, // Y_ADDR_START_ (B) + {0x3006, 0x04B7}, // Y_ADDR_END_ (A) + {0x3090, 0x04B7}, // Y_ADDR_END_ (B) {0x3032, 0x0000}, // SCALING_MODE - {0x30A2, 0x0001}, // X_ODD_INC_ - {0x30A6, 0x0001}, // Y_ODD_INC_ + {0x30A2, 0x0001}, // X_ODD_INC_ (A) + {0x30AE, 0x0001}, // X_ODD_INC_ (B) + {0x30A6, 0x0001}, // Y_ODD_INC_ (A) + {0x30A8, 0x0001}, // Y_ODD_INC_ (B) {0x3402, 0x0F10}, // X_OUTPUT_CONTROL {0x3404, 0x0970}, // Y_OUTPUT_CONTROL {0x3064, 0x1802}, // SMIA_TEST @@ -32,8 +38,10 @@ struct i2c_random_wr_payload init_array_ar0231[] = { {0x340C, 0x802}, // 2 // 0000 0000 0010 // Readout timing - {0x300C, 0x07B9}, // LINE_LENGTH_PCK - {0x300A, 0x07E7}, // FRAME_LENGTH_LINES + {0x300C, 0x07B9}, // LINE_LENGTH_PCK (A) + {0x303E, 0x07B9}, // LINE_LENGTH_PCK (B) + {0x300A, 0x07E7}, // FRAME_LENGTH_LINES (A) + {0x30AA, 0x07E7}, // FRAME_LENGTH_LINES (B) {0x3042, 0x0000}, // EXTRA_DELAY // Readout Settings @@ -49,7 +57,7 @@ struct i2c_random_wr_payload init_array_ar0231[] = { {0x3350, 0x0311}, // MIPI_F4_VDT_VC {0x31B0, 0x0053}, // FRAME_PREAMBLE {0x31B2, 0x003B}, // LINE_PREAMBLE - {0x301A, 0x01C}, // RESET_REGISTER + {0x301A, 0x001C}, // RESET_REGISTER // Noise Corrections {0x3092, 0x0C24}, // ROW_NOISE_CONTROL @@ -62,10 +70,18 @@ struct i2c_random_wr_payload init_array_ar0231[] = { {0x31E0, 0x0003}, // HDR Settings - {0x3082, 0x0004}, // OPERATION_MODE_CTRL - {0x3238, 0x0004}, // EXPOSURE_RATIO - {0x3014, 0x098E}, // FINE_INTEGRATION_TIME_ - {0x321E, 0x098E}, // FINE_INTEGRATION_TIME2 + {0x3082, 0x0004}, // OPERATION_MODE_CTRL (A) + {0x3084, 0x0004}, // OPERATION_MODE_CTRL (B) + + {0x3238, 0x0004}, // EXPOSURE_RATIO (A) + {0x323A, 0x0004}, // EXPOSURE_RATIO (B) + + {0x3014, 0x098E}, // FINE_INTEGRATION_TIME_ (A) + {0x3018, 0x098E}, // FINE_INTEGRATION_TIME_ (B) + + {0x321E, 0x098E}, // FINE_INTEGRATION_TIME2 (A) + {0x3220, 0x098E}, // FINE_INTEGRATION_TIME2 (B) + {0x31D0, 0x0000}, // COMPANDING, no good in 10 bit? {0x33DA, 0x0000}, // COMPANDING {0x318E, 0x0200}, // PRE_HDR_GAIN_EN @@ -82,16 +98,27 @@ struct i2c_random_wr_payload init_array_ar0231[] = { {0x328E, 0x0FA0}, // T2 G2 // Initial Gains - {0x3022, 0x01}, // GROUPED_PARAMETER_HOLD_ - {0x3366, 0x5555}, // ANALOG_GAIN + {0x3022, 0x0001}, // GROUPED_PARAMETER_HOLD_ + {0x3366, 0xFF77}, // ANALOG_GAIN (1x) (A) + {0x3368, 0xFF77}, // ANALOG_GAIN (1x) (B) + {0x3060, 0x3333}, // ANALOG_COLOR_GAIN - {0x3362, 0x0000}, // DC GAIN - {0x305A, 0x0108}, // RED_GAIN - {0x3058, 0x00FB}, // BLUE_GAIN - {0x3056, 0x009A}, // GREEN1_GAIN - {0x305C, 0x009A}, // GREEN2_GAIN - {0x3022, 0x00}, // GROUPED_PARAMETER_HOLD_ + + {0x3362, 0x0000}, // DC GAIN (A & B) + + {0x305A, 0x00F8}, // red gain (A) + {0x3058, 0x0122}, // blue gain (A) + {0x3056, 0x009A}, // g1 gain (A) + {0x305C, 0x009A}, // g2 gain (A) + + {0x30C0, 0x00F8}, // red gain (B) + {0x30BE, 0x0122}, // blue gain (B) + {0x30BC, 0x009A}, // g1 gain (B) + {0x30C2, 0x009A}, // g2 gain (B) + + {0x3022, 0x0000}, // GROUPED_PARAMETER_HOLD_ // Initial Integration Time - {0x3012, 0x256}, + {0x3012, 0x0005}, // (A) + {0x3016, 0x0005}, // (B) }; diff --git a/selfdrive/camerad/test/ae_gray_test.cc b/selfdrive/camerad/test/ae_gray_test.cc index d5d71608af..0f14a23794 100644 --- a/selfdrive/camerad/test/ae_gray_test.cc +++ b/selfdrive/camerad/test/ae_gray_test.cc @@ -34,37 +34,33 @@ int main() { float rtol = 0.05; // generate pattern and calculate EV int cnt = 0; - for (int is_qcom2=0; is_qcom2<2; is_qcom2++) { - for (int g=0; g rtol*evgt) { - passed = false; - } - - // report - printf("%d/%d/%d/%d/%d/%d/%d: ev %f, gt %f, err %f\n", is_qcom2, g*10, h_0, h_1, h_2, h_3, h_4, ev, evgt, fabs(ev - evgt) / (evgt != 0 ? evgt : 0.00001f)); - cnt++; - } + // compare to gt + float evgt = gts[cnt]; + if (fabs(ev - evgt) > rtol*evgt) { + passed = false; } + + // report + printf("%d/%d/%d/%d/%d: ev %f, gt %f, err %f\n", h_0, h_1, h_2, h_3, h_4, ev, evgt, fabs(ev - evgt) / (evgt != 0 ? evgt : 0.00001f)); + cnt++; } } } diff --git a/selfdrive/camerad/test/ae_gray_test.h b/selfdrive/camerad/test/ae_gray_test.h index 4e747bb45a..fb54cd9584 100644 --- a/selfdrive/camerad/test/ae_gray_test.h +++ b/selfdrive/camerad/test/ae_gray_test.h @@ -4,9 +4,8 @@ #define H 160 #define TONE_SPLITS 3 -#define GAIN_SPLITS 2 -float gts[2*TONE_SPLITS*TONE_SPLITS*TONE_SPLITS*TONE_SPLITS*GAIN_SPLITS] = { +float gts[TONE_SPLITS*TONE_SPLITS*TONE_SPLITS*TONE_SPLITS] = { 0.917969,0.917969,0.375000,0.917969,0.375000,0.375000,0.187500,0.187500,0.187500,0.917969, 0.375000,0.375000,0.187500,0.187500,0.187500,0.187500,0.187500,0.187500,0.093750,0.093750, 0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.917969,0.375000,0.375000, @@ -15,29 +14,5 @@ float gts[2*TONE_SPLITS*TONE_SPLITS*TONE_SPLITS*TONE_SPLITS*GAIN_SPLITS] = { 0.093750,0.093750,0.093750,0.093750,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, - 0.000000,0.917969,0.917969,0.375000,0.917969,0.375000,0.375000,0.187500,0.187500,0.187500, - 0.917969,0.375000,0.375000,0.187500,0.187500,0.187500,0.187500,0.187500,0.187500,0.093750, - 0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.917969,0.375000, - 0.375000,0.187500,0.187500,0.187500,0.187500,0.187500,0.187500,0.093750,0.093750,0.093750, - 0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750, - 0.093750,0.093750,0.093750,0.093750,0.093750,0.000000,0.000000,0.000000,0.000000,0.000000, - 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, - 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, - 0.000000,0.000000,4.527344,3.324219,0.457031,4.421875,3.265625,0.453125,4.324219,3.167969, - 0.449219,4.421875,3.265625,0.453125,4.234375,3.113281,0.449219,3.980469,2.929688,0.441406, - 4.324219,3.167969,0.449219,3.980469,2.929688,0.441406,3.558594,0.433594,0.433594,4.421875, - 3.265625,0.453125,4.234375,3.113281,0.449219,3.980469,2.929688,0.441406,4.234375,3.113281, - 0.449219,3.929688,2.902344,0.441406,3.484375,0.429688,0.429688,3.980469,2.929688,0.441406, - 3.484375,0.429688,0.429688,2.871094,0.417969,0.417969,4.324219,3.167969,0.449219,3.980469, - 2.929688,0.441406,3.558594,0.433594,0.433594,3.980469,2.929688,0.441406,3.484375,0.429688, - 0.429688,2.871094,0.417969,0.417969,3.558594,0.433594,0.433594,2.871094,0.417969,0.417969, - 0.308594,0.308594,0.308594,4.253906,3.140625,0.574219,4.156250,3.085938,0.566406,4.066406, - 2.996094,0.562500,4.156250,3.085938,0.566406,3.984375,2.945312,0.554688,3.750000,2.777344, - 0.542969,4.066406,2.996094,0.562500,3.750000,2.777344,0.542969,3.359375,0.519531,0.519531, - 4.156250,3.085938,0.566406,3.984375,2.945312,0.554688,3.750000,2.777344,0.542969,3.984375, - 2.945312,0.554688,3.699219,2.753906,0.539062,3.289062,0.515625,0.515625,3.750000,2.777344, - 0.542969,3.289062,0.515625,0.515625,2.722656,0.480469,0.480469,4.066406,2.996094,0.562500, - 3.750000,2.777344,0.542969,3.359375,0.519531,0.519531,3.750000,2.777344,0.542969,3.289062, - 0.515625,0.515625,2.722656,0.480469,0.480469,3.359375,0.519531,0.519531,2.722656,0.480469, - 0.480469,0.328125,0.328125,0.328125, + 0.000000 }; diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index b5b7e3be0d..7a92941742 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -48,7 +48,7 @@ if TICI: PROCS.update({ "./loggerd": 60.0, "selfdrive.controls.controlsd": 26.0, - "./camerad": 25.0, + "./camerad": 31.0, "./_ui": 21.0, "selfdrive.controls.plannerd": 12.0, "selfdrive.locationd.paramsd": 5.0,