From 6ae193c1b39457916837eee3c5c892faec51d6c5 Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Mon, 10 Feb 2020 13:55:50 -0800 Subject: [PATCH] fix not focusing when all readings broken --- selfdrive/camerad/cameras/camera_qcom.c | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/selfdrive/camerad/cameras/camera_qcom.c b/selfdrive/camerad/cameras/camera_qcom.c index 2fcbc77d6bfbc0..059597d973434a 100644 --- 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;