From 2b98a08180780407b4ed289457b9ad971a2a3bf7 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 9 Dec 2023 08:15:58 +0800 Subject: [PATCH] camerad: remove camera_id (#30654) * remove camera_id * use variable old-commit-hash: ea0b8920f578e2b6f348d2e18b5783166731c880 --- system/camerad/cameras/camera_common.cc | 9 ++--- system/camerad/cameras/camera_common.h | 3 -- system/camerad/cameras/camera_qcom2.cc | 46 ++++++++++--------------- system/camerad/cameras/camera_qcom2.h | 5 ++- system/camerad/sensors/ar0231.cc | 1 + system/camerad/sensors/ox03c10.cc | 1 + system/camerad/sensors/sensor.h | 1 + 7 files changed, 26 insertions(+), 40 deletions(-) diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index a53b0d412a..e36766ca2d 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -36,7 +36,7 @@ public: "-DIS_OX=%d -DCAM_NUM=%d%s", ci->frame_width, ci->frame_height, ci->frame_stride, ci->frame_offset, b->rgb_width, b->rgb_height, b->rgb_stride, buf_width, uv_offset, - s->camera_id==CAMERA_ID_OX03C10 ? 1 : 0, s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : ""); + ci->image_sensor == cereal::FrameData::ImageSensor::OX03C10, s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : ""); const char *cl_file = "cameras/real_debayer.cl"; cl_program prg_debayer = cl_program_from_file(context, device_id, cl_file, args); krnl_ = CL_CHECK_ERR(clCreateKernel(prg_debayer, "debayer10", &err)); @@ -154,12 +154,7 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr const float ev = c->cur_ev[frame_data.frame_id % 3]; const float perc = util::map_val(ev, c->ci->min_ev, c->ci->max_ev, 0.0f, 100.0f); framed.setExposureValPercent(perc); - - if (c->camera_id == CAMERA_ID_AR0231) { - framed.setSensor(cereal::FrameData::ImageSensor::AR0231); - } else if (c->camera_id == CAMERA_ID_OX03C10) { - framed.setSensor(cereal::FrameData::ImageSensor::OX03C10); - } + framed.setSensor(c->ci->image_sensor); } kj::Array get_raw_frame_image(const CameraBuf *b) { diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index eedeaf1b7a..e93d357b8d 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -14,9 +14,6 @@ #include "common/swaglog.h" #include "system/hardware/hw.h" -#define CAMERA_ID_AR0231 0 -#define CAMERA_ID_OX03C10 1 - const int YUV_BUFFER_COUNT = 20; enum CameraType { diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 2462e969c4..194f11fe6d 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -106,8 +106,6 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { } int CameraState::sensors_init() { - create_sensor(); - uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2; auto pkt = mm.alloc(size, &cam_packet_handle); @@ -403,15 +401,7 @@ void CameraState::enqueue_req_multi(int start, int n, bool dp) { // ******************* camera ******************* -void CameraState::create_sensor() { - if (camera_id == CAMERA_ID_AR0231) { - ci = std::make_unique(); - } else if (camera_id == CAMERA_ID_OX03C10) { - ci = std::make_unique(); - } else { - assert(false); - } - +void CameraState::sensor_set_parameters() { target_grey_fraction = 0.3; dc_gain_enabled = false; @@ -436,9 +426,8 @@ void CameraState::camera_map_bufs(MultiCameraState *s) { enqueue_req_multi(1, FRAME_BUF_COUNT, 0); } -void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, int camera_id_, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type) { +void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type) { if (!enabled) return; - camera_id = camera_id_; LOGD("camera init %d", camera_num); request_id_last = 0; @@ -462,22 +451,25 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num // init memorymanager for this camera mm.init(multi_cam_state->video0_fd); - // probe the sensor LOGD("-- Probing sensor %d", camera_num); - camera_id = CAMERA_ID_AR0231; - ret = sensors_init(); - if (ret != 0) { - // TODO: use build flag instead? - LOGD("AR0231 init failed, trying OX03C10"); - camera_id = CAMERA_ID_OX03C10; - ret = sensors_init(); - } - LOGD("-- Probing sensor %d done with %d", camera_num, ret); - if (ret != 0) { + + auto init_sensor_lambda = [this](SensorInfo *sensor) { + ci.reset(sensor); + int ret = sensors_init(); + if (ret == 0) { + sensor_set_parameters(); + } + return ret == 0; + }; + + // Try different sensors one by one until it success. + if (!init_sensor_lambda(new AR0231) && + !init_sensor_lambda(new OX03C10)) { LOGE("** sensor %d FAILED bringup, disabling", camera_num); enabled = false; return; } + LOGD("-- Probing sensor %d success", camera_num); // create session struct cam_req_mgr_session_info session_info = {}; @@ -626,9 +618,9 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num } void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { - s->driver_cam.camera_init(s, v, s->driver_cam.camera_id, 20, device_id, ctx, VISION_STREAM_DRIVER); - s->road_cam.camera_init(s, v, s->road_cam.camera_id, 20, device_id, ctx, VISION_STREAM_ROAD); - s->wide_road_cam.camera_init(s, v, s->wide_road_cam.camera_id, 20, device_id, ctx, VISION_STREAM_WIDE_ROAD); + s->driver_cam.camera_init(s, v, 20, device_id, ctx, VISION_STREAM_DRIVER); + s->road_cam.camera_init(s, v, 20, device_id, ctx, VISION_STREAM_ROAD); + s->wide_road_cam.camera_init(s, v, 20, device_id, ctx, VISION_STREAM_WIDE_ROAD); s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"}); } diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index 50a68b0b19..3da5ec207d 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -48,9 +48,9 @@ public: void sensors_start(); void camera_open(MultiCameraState *multi_cam_state, int camera_num, bool enabled); - void create_sensor(); + void sensor_set_parameters(); void camera_map_bufs(MultiCameraState *s); - void camera_init(MultiCameraState *s, VisionIpcServer *v, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type); + void camera_init(MultiCameraState *s, VisionIpcServer *v, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type); void camera_close(); int32_t session_handle; @@ -68,7 +68,6 @@ public: int frame_id_last; int idx_offset; bool skipped; - int camera_id; CameraBuf buf; MemoryManager mm; diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc index 9a70befdcc..9a657982a8 100644 --- a/system/camerad/sensors/ar0231.cc +++ b/system/camerad/sensors/ar0231.cc @@ -77,6 +77,7 @@ float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_r } // namespace AR0231::AR0231() { + image_sensor = cereal::FrameData::ImageSensor::AR0231; data_word = true; frame_width = FRAME_WIDTH; frame_height = FRAME_HEIGHT; diff --git a/system/camerad/sensors/ox03c10.cc b/system/camerad/sensors/ox03c10.cc index c30838a7fe..2bad50cff9 100644 --- a/system/camerad/sensors/ox03c10.cc +++ b/system/camerad/sensors/ox03c10.cc @@ -22,6 +22,7 @@ const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35 } // namespace OX03C10::OX03C10() { + image_sensor = cereal::FrameData::ImageSensor::OX03C10; data_word = false; frame_width = FRAME_WIDTH; frame_height = FRAME_HEIGHT; diff --git a/system/camerad/sensors/sensor.h b/system/camerad/sensors/sensor.h index 1e45778f20..06d3a43a21 100644 --- a/system/camerad/sensors/sensor.h +++ b/system/camerad/sensors/sensor.h @@ -23,6 +23,7 @@ public: virtual int getSlaveAddress(int port) const { assert(0); } virtual void processRegisters(CameraState *c, cereal::FrameData::Builder &framed) const {} + cereal::FrameData::ImageSensor image_sensor = cereal::FrameData::ImageSensor::UNKNOWN; uint32_t frame_width, frame_height; uint32_t frame_stride; uint32_t frame_offset = 0;