init camera in AbstraceCamera

commaai · Sep 12, 2022 · ef86a44 · ef86a44
1 parent bfed601
commit ef86a44
Show file tree

Hide file tree

Showing 7 changed files with 172 additions and 152 deletions.
diff --git a/system/camerad/SConscript b/system/camerad/SConscript
@@ -5,7 +5,7 @@ libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq
 cenv = env.Clone()
 cenv['CPPPATH'].append('include/')
 
-camera_obj = cenv.Object(['cameras/camera_qcom2.cc',   'cameras/camera_ar0231.cc','cameras/camera_ox03c10.cc','cameras/camera_common.cc', 'cameras/camera_util.cc'])
+camera_obj = cenv.Object(['cameras/camera_qcom2.cc', 'cameras/camera.cc', 'cameras/camera_ar0231.cc','cameras/camera_ox03c10.cc','cameras/camera_common.cc', 'cameras/camera_util.cc'])
 cenv.Program('camerad', [
     'main.cc',
     camera_obj,

diff --git a/system/camerad/cameras/camera.cc b/system/camerad/cameras/camera.cc
@@ -0,0 +1,129 @@
+#include "system/camerad/cameras/camera.h"
+
+#include "common/swaglog.h"
+#include "system/camerad/cameras/camera_util.h"
+
+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;
+  unconditional_wait->delay = delay_ms;
+  unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND;
+  return (struct cam_cmd_power *)(unconditional_wait + 1);
+};
+
+bool AbstractCamera::init(int video0_fd, int sensor_fd, int camera_num) {
+  // init memorymanager for this camera
+  mm.init(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 *)mm.alloc(size, &cam_packet_handle);
+  pkt->num_cmd_buf = 2;
+  pkt->kmd_cmd_buf_index = -1;
+  pkt->header.op_code = 0x1000000 | CAM_SENSOR_PACKET_OPCODE_SENSOR_PROBE;
+  pkt->header.size = size;
+  struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload;
+
+  buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_cmd_i2c_info) + sizeof(struct cam_cmd_probe);
+  buf_desc[0].type = CAM_CMD_BUF_LEGACY;
+  struct cam_cmd_i2c_info *i2c_info = (struct cam_cmd_i2c_info *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
+  auto probe = (struct cam_cmd_probe *)(i2c_info + 1);
+
+  probe->camera_id = camera_num;
+  i2c_info->slave_addr = getSlaveAddress(camera_num);
+
+  // 0(I2C_STANDARD_MODE) = 100khz, 1(I2C_FAST_MODE) = 400khz
+  //i2c_info->i2c_freq_mode = I2C_STANDARD_MODE;
+  i2c_info->i2c_freq_mode = I2C_FAST_MODE;
+  i2c_info->cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_INFO;
+
+  probe->data_type = CAMERA_SENSOR_I2C_TYPE_WORD;
+  probe->addr_type = CAMERA_SENSOR_I2C_TYPE_WORD;
+  probe->op_code = 3;   // don't care?
+  probe->cmd_type = CAMERA_SENSOR_CMD_TYPE_PROBE;
+  probe->reg_addr = reg_addr;
+  probe->expected_data = expected_data;
+  probe->data_mask = 0;
+
+  //buf_desc[1].size = buf_desc[1].length = 148;
+  buf_desc[1].size = buf_desc[1].length = 196;
+  buf_desc[1].type = CAM_CMD_BUF_I2C;
+  struct cam_cmd_power *power_settings = (struct cam_cmd_power *)mm.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle);
+  memset(power_settings, 0, buf_desc[1].size);
+
+  // power on
+  struct cam_cmd_power *power = power_settings;
+  power->count = 4;
+  power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP;
+  power->power_settings[0].power_seq_type = 3; // clock??
+  power->power_settings[1].power_seq_type = 1; // analog
+  power->power_settings[2].power_seq_type = 2; // digital
+  power->power_settings[3].power_seq_type = 8; // reset low
+  power = power_set_wait(power, 1);
+
+  // set clock
+  power->count = 1;
+  power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP;
+  power->power_settings[0].power_seq_type = 0;
+  power->power_settings[0].config_val_low = config_val_low;
+  power = power_set_wait(power, 1);
+
+  // reset high
+  power->count = 1;
+  power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP;
+  power->power_settings[0].power_seq_type = 8;
+  power->power_settings[0].config_val_low = 1;
+  // wait 650000 cycles @ 19.2 mhz = 33.8 ms
+  power = power_set_wait(power, 34);
+
+  // probe happens here
+
+  // disable clock
+  power->count = 1;
+  power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN;
+  power->power_settings[0].power_seq_type = 0;
+  power->power_settings[0].config_val_low = 0;
+  power = power_set_wait(power, 1);
+
+  // reset high
+  power->count = 1;
+  power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN;
+  power->power_settings[0].power_seq_type = 8;
+  power->power_settings[0].config_val_low = 1;
+  power = power_set_wait(power, 1);
+
+  // reset low
+  power->count = 1;
+  power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN;
+  power->power_settings[0].power_seq_type = 8;
+  power->power_settings[0].config_val_low = 0;
+  power = power_set_wait(power, 1);
+
+  // power off
+  power->count = 3;
+  power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN;
+  power->power_settings[0].power_seq_type = 2;
+  power->power_settings[1].power_seq_type = 1;
+  power->power_settings[2].power_seq_type = 3;
+
+  int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0);
+  LOGD("probing the sensor: %d", ret);
+
+  mm.free(i2c_info);
+  mm.free(power_settings);
+  mm.free(pkt);
+
+  return ret == 0;
+}
+
+
+std::unique_ptr<AbstractCamera> AbstractCamera::initCamera(int video0_fd, int sensor_fd, int camera_num) {
+  std::unique_ptr<AbstractCamera> cam =  std::make_unique<CameraAR0231>();
+  if (!cam->init(video0_fd, sensor_fd, camera_num)) {
+    cam.reset(new CameraOX03C10());
+    if (!cam->init(video0_fd, sensor_fd, camera_num)) {
+      cam.reset(nullptr);
+    }
+  }
+  return cam;
+}
diff --git a/system/camerad/cameras/camera.h b/system/camerad/cameras/camera.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <cassert>
 #include <map>
 #include <mutex>
 #include <optional>
@@ -13,6 +14,9 @@
 #include "media/cam_sensor.h"
 #include "media/cam_sensor_cmn_header.h"
 #include "media/cam_sync.h"
+
+#include "common/util.h"
+#include "system/camerad/cameras/camera_util.h"
 #include "system/camerad/cameras/sensor2_i2c.h"
 
 #define CAMERA_ID_IMX298 0
@@ -45,8 +49,12 @@ class AbstractCamera {
   virtual ~AbstractCamera(){};
   virtual std::vector<struct i2c_random_wr_payload> getExposureVector(int new_g, bool dc_gain_enabled, int exposure_time, int dc_gain_weight) const = 0;
   virtual void processRegisters(void *addr, cereal::FrameData::Builder &framed) {}
+  virtual int getSlaveAddress(int port) = 0;
+  static std::unique_ptr<AbstractCamera> initCamera(int video0_fd, int sensor_fd, int camera_num);
 
   CameraInfo ci;
+  MemoryManager mm;
+
   int id;
   float dc_gain_factor;
   int dc_gain_max_weight;
@@ -60,11 +68,15 @@ class AbstractCamera {
   float min_ev, max_ev;
   int registers_offset = 0;
   float sensor_analog_gains[16];
+  uint32_t in_port_info_dt;
 
   camera_sensor_i2c_type i2c_type;
   std::vector<struct i2c_random_wr_payload> start_reg_array;
   std::vector<struct i2c_random_wr_payload> init_array;
-  uint32_t in_port_info_dt;
+
+protected:
+  bool init(int video0_fd, int sensor_fd, int camera_num);
+
   uint32_t reg_addr;
   uint32_t expected_data;
   uint32_t config_val_low;
@@ -74,6 +86,7 @@ class CameraAR0231 : public AbstractCamera {
 public:
   CameraAR0231();
   ~CameraAR0231();
+  int getSlaveAddress(int port) override;
   std::vector<struct i2c_random_wr_payload> getExposureVector(int new_g, bool dc_gain_enabled, int exposure_time, int dc_gain_weight) const override;
   void processRegisters(void *addr, cereal::FrameData::Builder &framed) override;
 
@@ -87,5 +100,6 @@ class CameraOX03C10 : public AbstractCamera {
 public:
   CameraOX03C10();
   ~CameraOX03C10();
+  int getSlaveAddress(int port) override;
   std::vector<struct i2c_random_wr_payload> getExposureVector(int new_g, bool dc_gain_enabled, int exposure_time, int dc_gain_weight) const override;
 };
diff --git a/system/camerad/cameras/camera_ar0231.cc b/system/camerad/cameras/camera_ar0231.cc
@@ -64,6 +64,11 @@ CameraAR0231::~CameraAR0231() {
 
 }
 
+int CameraAR0231::getSlaveAddress(int port) {
+  assert(port >=0 && port <= 2);
+  return (int[]){0x20, 0x30, 0x20}[port];
+}
+
 std::vector<struct i2c_random_wr_payload> CameraAR0231::getExposureVector(int new_g, bool dc_gain_enabled, int exposure_time, int dc_gain_weight) const {
   uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g;
   return {

diff --git a/system/camerad/cameras/camera_ox03c10.cc b/system/camerad/cameras/camera_ox03c10.cc
@@ -52,6 +52,11 @@ CameraOX03C10::CameraOX03C10() {
 CameraOX03C10::~CameraOX03C10() {
 }
 
+int CameraOX03C10::getSlaveAddress(int port) {
+  assert(port >=0 && port <= 2);
+  return (int[]){0x6C, 0x20, 0x6C}[port];
+}
+
 std::vector<struct i2c_random_wr_payload> CameraOX03C10::getExposureVector(int new_g, bool dc_gain_enabled, int exposure_time, int dc_gain_weight) const {
   static constexpr uint32_t ox03c10_analog_gains_reg[] = {
       0x100, 0x140, 0x150, 0x190,