camerad: move ar0231_register_lut to AR0231 (commaai#30652)

deanlee · Dec 8, 2023 · 21d5d7d · 21d5d7d
1 parent 80bc583
commit 21d5d7d
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 25 deletions.
diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc
@@ -920,7 +920,7 @@ static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt)
   framed.setFrameType(cereal::FrameData::FrameType::FRONT);
   fill_frame_data(framed, c->buf.cur_frame_data, c);
 
-  c->ci->processRegisters(s, c, framed);
+  c->ci->processRegisters(c, framed);
   s->pm->send("driverCameraState", msg);
 }
 
@@ -935,7 +935,7 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
   }
   LOGT(c->buf.cur_frame_data.frame_id, "%s: Image set", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera");
 
-  c->ci->processRegisters(s, c, framed);
+  c->ci->processRegisters(c, framed);
   s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg);
 
   const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986);

diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h
@@ -1,7 +1,6 @@
 #pragma once
 
 #include <cstdint>
-#include <map>
 #include <memory>
 #include <utility>
 
@@ -54,8 +53,6 @@ class CameraState {
   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_close();
 
-  std::map<uint16_t, uint16_t> ar0231_parse_registers(uint8_t *data, std::initializer_list<uint16_t> addrs);
-
   int32_t session_handle;
   int32_t sensor_dev_handle;
   int32_t isp_dev_handle;
@@ -85,9 +82,6 @@ class CameraState {
   void sensors_poke(int request_id);
   void sensors_i2c(const struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word);
 
-  // Register parsing
-  std::map<uint16_t, std::pair<int, int>> ar0231_register_lut;
-
 private:
   // for debugging
   Params params;

diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc
@@ -67,19 +67,6 @@ std::map<uint16_t, std::pair<int, int>> ar0231_build_register_lut(CameraState *c
   return registers;
 }
 
-std::map<uint16_t, uint16_t> ar0231_parse_registers(CameraState *c, uint8_t *data, std::initializer_list<uint16_t> addrs) {
-  if (c->ar0231_register_lut.empty()) {
-    c->ar0231_register_lut = ar0231_build_register_lut(c, data);
-  }
-
-  std::map<uint16_t, uint16_t> registers;
-  for (uint16_t addr : addrs) {
-    auto offset = c->ar0231_register_lut[addr];
-    registers[addr] = ((uint16_t)data[offset.first] << 8) | data[offset.second];
-  }
-  return registers;
-}
-
 float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_reg) {
   // See AR0231 Developer Guide - page 36
   float slope = (125.0 - 55.0) / ((float)calib1 - (float)calib2);
@@ -128,7 +115,7 @@ AR0231::AR0231() {
   target_grey_factor = 1.0;
 }
 
-void AR0231::processRegisters(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) const {
+void AR0231::processRegisters(CameraState *c, cereal::FrameData::Builder &framed) const {
   const uint8_t expected_preamble[] = {0x0a, 0xaa, 0x55, 0x20, 0xa5, 0x55};
   uint8_t *data = (uint8_t *)c->buf.cur_camera_buf->addr + c->ci->registers_offset;
 
@@ -137,7 +124,14 @@ void AR0231::processRegisters(MultiCameraState *s, CameraState *c, cereal::Frame
     return;
   }
 
-  auto registers = ar0231_parse_registers(c, data, {0x2000, 0x2002, 0x20b0, 0x20b2, 0x30c6, 0x30c8, 0x30ca, 0x30cc});
+  if (ar0231_register_lut.empty()) {
+    ar0231_register_lut = ar0231_build_register_lut(c, data);
+  }
+  std::map<uint16_t, uint16_t> registers;
+  for (uint16_t addr : {0x2000, 0x2002, 0x20b0, 0x20b2, 0x30c6, 0x30c8, 0x30ca, 0x30cc}) {
+    auto offset = ar0231_register_lut[addr];
+    registers[addr] = ((uint16_t)data[offset.first] << 8) | data[offset.second];
+  }
 
   uint32_t frame_id = ((uint32_t)registers[0x2000] << 16) | registers[0x2002];
   framed.setFrameIdSensor(frame_id);

diff --git a/system/camerad/sensors/sensor.h b/system/camerad/sensors/sensor.h
@@ -2,6 +2,8 @@
 
 #include <cassert>
 #include <cstdint>
+#include <map>
+#include <utility>
 #include <vector>
 #include "media/cam_sensor.h"
 #include "system/camerad/cameras/camera_common.h"
@@ -19,7 +21,7 @@ class SensorInfo {
   virtual std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { return {}; }
   virtual float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const {return 0; }
   virtual int getSlaveAddress(int port) const { assert(0); }
-  virtual void processRegisters(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) const {}
+  virtual void processRegisters(CameraState *c, cereal::FrameData::Builder &framed) const {}
 
   uint32_t frame_width, frame_height;
   uint32_t frame_stride;
@@ -63,7 +65,10 @@ class AR0231 : public SensorInfo {
   std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override;
   float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override;
   int getSlaveAddress(int port) const override;
-  void processRegisters(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) const override;
+  void processRegisters(CameraState *c, cereal::FrameData::Builder &framed) const override;
+
+private:
+  mutable std::map<uint16_t, std::pair<int, int>> ar0231_register_lut;
 };
 
 class OX03C10 : public SensorInfo {