fix not focusing when all readings broken (#1072)

commaai · Feb 10, 2020 · 08042ab · 08042ab
1 parent dcb14dd
commit 08042ab
Showing 1 changed file with 10 additions and 3 deletions.
diff --git a/selfdrive/camerad/cameras/camera_qcom.c b/selfdrive/camerad/cameras/camera_qcom.c
@@ -1737,18 +1737,23 @@ static void parse_autofocus(CameraState *s, uint8_t *d) {
   if (good_count < 4) {
     s->focus_err = nan("");
     nan_cnt += 1;
+    if (nan_cnt > patience_cnt) {
+      s->focus_err = 16*8.0;
+      nan_cnt = 0;
+    }
     return;
   }
 
   avg_focus /= good_count;
 
-  if (abs(avg_focus - max_focus) > 16) {
+  if (abs(avg_focus - max_focus) > 32) {
     if (nan_cnt < patience_cnt) {
       s->focus_err = nan("");
       nan_cnt += 1;
       return;
     } else {
-      s->focus_err = max_focus*8.0;
+      s->focus_err = 16*8.0;
+      // s->focus_err = max_focus*8.0;
       nan_cnt = 0;
     }
   } else {
@@ -1759,10 +1764,12 @@ static void parse_autofocus(CameraState *s, uint8_t *d) {
 }
 
 static void do_autofocus(CameraState *s) {
-  // params for focus PI controller
+  // params for focus P controller
   const float focus_kp = 0.1;
 
   float err = s->focus_err;
+  // don't allow big change
+  err = clamp(err, -128, 128);
   float sag = (s->last_sag_acc_z/9.8) * 128;
 
   const int dac_up = s->device == DEVICE_LP3? 634:456;