camerad: rework ox exposure score (#26948)

* wip

* make separate score func

* n4ot g

* clean up

* remove

* simplify

* b2b

* balance costs

* clean up

* no if tici

* box view

* new baselines

* Revert "new baselines"

This reverts commit f7a5d059ed.

* Revert "box view"

This reverts commit a57924be36.

Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: 028527423c

system/camerad/cameras/camera_qcom2.cc

@@ -104,8 +104,8 @@ const int ANALOG_GAIN_MIN_IDX_OX03C10 = 0x0;
 const int ANALOG_GAIN_REC_IDX_OX03C10 = 0x11; // 2.5x
 const int ANALOG_GAIN_MAX_IDX_OX03C10 = 0x36;
 const int ANALOG_GAIN_COST_DELTA_OX03C10 = -1;
-const float ANALOG_GAIN_COST_LOW_OX03C10 = 0.05;
+const float ANALOG_GAIN_COST_LOW_OX03C10 = 0.4;
-const float ANALOG_GAIN_COST_HIGH_OX03C10 = 0.8;
+const float ANALOG_GAIN_COST_HIGH_OX03C10 = 6.4;
 
 const int EXPOSURE_TIME_MIN_AR0231 = 2; // with HDR, fastest ss
 const int EXPOSURE_TIME_MAX_AR0231 = 0x0855; // with HDR, slowest ss, 40ms
@@ -1041,6 +1041,30 @@ void CameraState::handle_camera_event(void *evdat) {
   }
 }
 
+void CameraState::update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain) {
+  float score = 1e6;
+  if (camera_id == CAMERA_ID_AR0231) {
+    // Cost of ev diff
+    score = std::abs(desired_ev - (exp_t * exp_gain)) * 10;
+    // Cost of absolute gain
+    float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low;
+    score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m;
+    // Cost of changing gain
+    score += std::abs(exp_g_idx - gain_idx) * (score + 1.0) / 10.0;
+  } else if (camera_id == CAMERA_ID_OX03C10) {
+    score = std::abs(desired_ev - (exp_t * exp_gain));
+    float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low;
+    score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m;
+    score += ((1 - analog_gain_cost_delta) + analog_gain_cost_delta * (exp_g_idx - analog_gain_min_idx) / (analog_gain_max_idx - analog_gain_min_idx)) * std::abs(exp_g_idx - gain_idx) * 5.0;
+  }
+
+  if (score < best_ev_score) {
+    new_exp_t = exp_t;
+    new_exp_g = exp_g_idx;
+    best_ev_score = score;
+  }
+}
+
 void CameraState::set_camera_exposure(float grey_frac) {
   if (!enabled) return;
   const float dt = 0.05;
@@ -1066,9 +1090,9 @@ void CameraState::set_camera_exposure(float grey_frac) {
   float k = (1.0 - k_ev) / 3.0;
   desired_ev = (k * cur_ev[0]) + (k * cur_ev[1]) + (k * cur_ev[2]) + (k_ev * desired_ev);
 
-  float best_ev_score = 1e6;
+  best_ev_score = 1e6;
-  int new_g = 0;
+  new_exp_g = 0;
-  int new_t = 0;
+  new_exp_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
@@ -1095,8 +1119,8 @@ void CameraState::set_camera_exposure(float grey_frac) {
     gain_idx = std::stoi(gain_bytes);
     exposure_time = std::stoi(time_bytes);
 
-    new_g = gain_idx;
+    new_exp_g = gain_idx;
-    new_t = exposure_time;
+    new_exp_t = exposure_time;
     enable_dc_gain = false;
   } else {
     // Simple brute force optimizer to choose sensor parameters
@@ -1112,23 +1136,7 @@ void CameraState::set_camera_exposure(float grey_frac) {
         continue;
       }
 
-      // Compute error to desired ev
+      update_exposure_score(desired_ev, t, g, gain);
-      float score = std::abs(desired_ev - (t * gain)) * 10;
-
-      // Going below recommended gain needs lower penalty to not overexpose
-      float m = g > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low;
-      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", camera_num, g, t, desired_ev / gain, score, score + std::abs(g - gain_idx) * (score + 1.0) / 10.0, desired_ev, min_ev);
-
-      // Small penalty on changing gain
-      score += ((1 - analog_gain_cost_delta) + analog_gain_cost_delta * (g - analog_gain_min_idx) / (analog_gain_max_idx - analog_gain_min_idx)) * std::abs(g - gain_idx) * (score + 1.0) / 10.0;
-
-      if (score < best_ev_score) {
-        new_t = t;
-        new_g = g;
-        best_ev_score = score;
-      }
     }
   }
 
@@ -1137,9 +1145,9 @@ void CameraState::set_camera_exposure(float grey_frac) {
   measured_grey_fraction = grey_frac;
   target_grey_fraction = target_grey;
 
-  analog_gain_frac = sensor_analog_gains[new_g];
+  analog_gain_frac = sensor_analog_gains[new_exp_g];
-  gain_idx = new_g;
+  gain_idx = new_exp_g;
-  exposure_time = new_t;
+  exposure_time = new_exp_t;
   dc_gain_enabled = enable_dc_gain;
 
   float gain = analog_gain_frac * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight);
@@ -1156,7 +1164,7 @@ void CameraState::set_camera_exposure(float grey_frac) {
   // LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera_num, 1e-9 * nanos_since_boot(), 1e-9 * buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - buf.cur_frame_data.timestamp_sof));
 
   if (camera_id == CAMERA_ID_AR0231) {
-    uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g;
+    uint16_t analog_gain_reg = 0xFF00 | (new_exp_g << 4) | new_exp_g;
     struct i2c_random_wr_payload exp_reg_array[] = {
       {0x3366, analog_gain_reg},
       {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)},
@@ -1171,7 +1179,7 @@ void CameraState::set_camera_exposure(float grey_frac) {
     uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 128, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10);
     uint32_t spd_time = vs_time;
 
-    uint32_t real_gain = ox03c10_analog_gains_reg[new_g];
+    uint32_t real_gain = ox03c10_analog_gains_reg[new_exp_g];
     uint32_t min_gain = ox03c10_analog_gains_reg[0];
     struct i2c_random_wr_payload exp_reg_array[] = {
       {0x3501, hcg_time>>8}, {0x3502, hcg_time&0xFF},

system/camerad/cameras/camera_qcom2.h

@@ -47,6 +47,9 @@ public:
 
   float cur_ev[3];
   float min_ev, max_ev;
+  float best_ev_score;
+  int new_exp_g;
+  int new_exp_t;
 
   float measured_grey_fraction;
   float target_grey_fraction;
@@ -58,6 +61,7 @@ public:
   int camera_num;
 
   void handle_camera_event(void *evdat);
+  void update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain);
   void set_camera_exposure(float grey_frac);
 
   void sensors_start();

system/camerad/test/test_exposure.py

@@ -6,16 +6,13 @@ import numpy as np
 from selfdrive.test.helpers import with_processes
 from system.camerad.snapshot.snapshot import get_snapshots
 
-from system.hardware import TICI
-
 TEST_TIME = 45
 REPEAT = 5
 
 class TestCamerad(unittest.TestCase):
   @classmethod
   def setUpClass(cls):
-    if not TICI:
+    pass
-      raise unittest.SkipTest
 
   def _numpy_rgb2gray(self, im):
     ret = np.clip(im[:,:,2] * 0.114 + im[:,:,1] * 0.587 + im[:,:,0] * 0.299, 0, 255).astype(np.uint8)
@@ -37,13 +34,11 @@ class TestCamerad(unittest.TestCase):
     start = time.time()
     while time.time() - start < TEST_TIME and passed < REPEAT:
       rpic, dpic = get_snapshots(frame="roadCameraState", front_frame="driverCameraState")
+      wpic, _ = get_snapshots(frame="wideRoadCameraState")
 
       res = self._is_exposure_okay(rpic)
       res = res and self._is_exposure_okay(dpic)
-
+      res = res and self._is_exposure_okay(wpic)
-      if TICI:
-        wpic, _ = get_snapshots(frame="wideRoadCameraState")
-        res = res and self._is_exposure_okay(wpic)
 
       if passed > 0 and not res:
         passed = -passed # fails test if any failure after first sus