revert AF for release

RELEASES.md

@@ -1,6 +1,5 @@
 Version 0.7.3 (2020-02-21)
 ========================
- * Improved autofocus for road facing camera
  * Support for 2020 Highlander thanks to che220!
  * Support for 2018 Lexus NX 300h thanks to kengggg!
  * Speed up ECU firmware query

selfdrive/camerad/cameras/camera_qcom.c

@@ -1710,66 +1710,48 @@ static void parse_autofocus(CameraState *s, uint8_t *d) {
   int good_count = 0;
   int16_t max_focus = -32767;
   int avg_focus = 0;
-  // force to max if not able to determine focus for long
-  const int patience_cnt = 20;
-  static int nan_cnt = 0;
 
   /*printf("FOCUS: ");
   for (int i = 0; i < 0x10; i++) {
     printf("%2.2X ", d[i]);
   }*/
 
   for (int i = 0; i < NUM_FOCUS; i++) {
-    int pd_idx = (i+1)*5;
-    s->confidence[i] = d[pd_idx];
-    int16_t focus_delta = d[pd_idx+1];
-    if (focus_delta >= 128) focus_delta = - (256 - focus_delta);
-    s->focus[i] = focus_delta;
-
-    if (s->confidence[i] > 64) {
+    int doff = i*5+5;
+    s->confidence[i] = d[doff];
+    int16_t focus_t = (d[doff+1] << 3) | (d[doff+2] >> 5);
+    if (focus_t >= 1024) focus_t = -(2048-focus_t);
+    s->focus[i] = focus_t;
+    //printf("%x->%d ", d[doff], focus_t);
+    if (s->confidence[i] > 0x20) {
       good_count++;
       max_focus = max(max_focus, s->focus[i]);
       avg_focus += s->focus[i];
-    // printf("%d\n", s->focus[i]);
     }
   }
 
-  if (good_count < 7) {
+  //printf("\n");
+  if (good_count < 4) {
     s->focus_err = nan("");
-    nan_cnt += 1;
-    if (nan_cnt > patience_cnt) {
-      s->focus_err = 16;
-      nan_cnt = 0;
-    }
     return;
   }
 
   avg_focus /= good_count;
 
-  if (abs(avg_focus - max_focus) > 32) {
-    if (nan_cnt < patience_cnt) {
-      s->focus_err = nan("");
-      nan_cnt += 1;
-      return;
-    } else {
-      s->focus_err = 16;
-      // s->focus_err = max_focus*8.0;
-      nan_cnt = 0;
-    }
-  } else {
-    s->focus_err = max_focus;
-    nan_cnt = 0;
+  // outlier rejection
+  if (abs(avg_focus - max_focus) > 200) {
+    s->focus_err = nan("");
+    return;
   }
-  // printf("fe=%f\n", s->focus_err);
+
+  s->focus_err = max_focus*1.0;
 }
 
 static void do_autofocus(CameraState *s) {
-  // params for focus P controller
-  const float focus_kp = 0.1;
+  // params for focus PI controller
+  const float focus_kp = 0.005;
 
   float err = s->focus_err;
-  // don't allow big change
-  err = clamp(err, -16, 16);
   float sag = (s->last_sag_acc_z/9.8) * 128;
 
   const int dac_up = s->device == DEVICE_LP3? 634:456;
@@ -1793,7 +1775,6 @@ static void do_autofocus(CameraState *s) {
   LOGD(debug);*/
 
   actuator_move(s, target);
-  // printf("ltp=%f, clp=%d\n",s->lens_true_pos,s->cur_lens_pos);
 }
 
 
@@ -2084,6 +2065,7 @@ static void* ops_thread(void* arg) {
         if (cmsg.type == CAMERA_MSG_AUTOEXPOSE) {
           if (cmsg.camera_num == 0) {
             do_autoexposure(&s->rear, cmsg.grey_frac);
+            do_autofocus(&s->rear);
           } else {
             do_autoexposure(&s->front, cmsg.grey_frac);
           }
@@ -2182,11 +2164,7 @@ void cameras_run(DualCameraState *s) {
         } else {
           uint8_t *d = c->ss[buffer].bufs[buf_idx].addr;
           if (buffer == 1) {
-            // FILE *df = fopen("/sdcard/focus_buf","wb");
-            // fwrite(d, c->ss[buffer].bufs[buf_idx].len, sizeof(uint8_t), df);
-            // fclose(df);
             parse_autofocus(c, d);
-            do_autofocus(&s->rear);
           }
           c->ss[buffer].qbuf_info[buf_idx].dirty_buf = 1;
           ioctl(c->isp_fd, VIDIOC_MSM_ISP_ENQUEUE_BUF, &c->ss[buffer].qbuf_info[buf_idx]);