mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-24 15:32:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix not focusing when all readings broken (#1072)

Browse Source
This commit is contained in:
ZwX1616
2020-02-10 15:28:35 -08:00
committed by GitHub
parent dcb14dd830
commit 08042ab649
1 changed files with 10 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/camerad/cameras/camera_qcom.c
+10 -3
View File
@@ -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;