mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-02-27 13:03:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

Focusss (#1059)

Browse Source
This commit is contained in:
ZwX1616
2020-02-07 13:30:45 -08:00
committed by GitHub
parent dabe9d71ab
commit 2b97ff70b5
1 changed files with 30 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
selfdrive/camerad/cameras/camera_qcom.c
View File

@@ -1710,6 +1710,9 @@ 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 = 100;
static int nan_cnt = 0;
/*printf("FOCUS: ");
for (int i = 0; i < 0x10; i++) {
@@ -1717,42 +1720,49 @@ static void parse_autofocus(CameraState *s, uint8_t *d) {
}*/
for (int i = 0; i < NUM_FOCUS; i++) {
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) {
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) {
good_count++;
max_focus = max(max_focus, s->focus[i]);
avg_focus += s->focus[i];
// printf("%d\n", s->focus[i]);
}
}
//printf("\n");
if (good_count < 4) {
s->focus_err = nan("");
nan_cnt += 1;
return;
}
avg_focus /= good_count;
// outlier rejection
if (abs(avg_focus - max_focus) > 200) {
s->focus_err = nan("");
return;
if (abs(avg_focus - max_focus) > 16) {
if (nan_cnt < patience_cnt) {
s->focus_err = nan("");
nan_cnt += 1;
return;
} else {
s->focus_err = max_focus*8.0;
nan_cnt = 0;
}
} else {
s->focus_err = avg_focus*8.0;
nan_cnt = 0;
}
s->focus_err = max_focus*1.0;
// printf("fe=%f\n", s->focus_err);
}
static void do_autofocus(CameraState *s) {
// params for focus PI controller
const float focus_kp = 0.005;
const float focus_kp = 0.1;
float err = s->focus_err;
float offset = 0;
float sag = (s->last_sag_acc_z/9.8) * 128;
const int dac_up = s->device == DEVICE_LP3? 634:456;
@@ -1776,6 +1786,7 @@ 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);
}
@@ -2165,6 +2176,9 @@ 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);
}
c->ss[buffer].qbuf_info[buf_idx].dirty_buf = 1;