parent
98c8888cf7
commit
42e0d13389
|
@ -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
|
||||
|
|
|
@ -1710,9 +1710,6 @@ 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++) {
|
||||
|
@ -1720,56 +1717,41 @@ static void parse_autofocus(CameraState *s, uint8_t *d) {
|
|||
}*/
|
||||
|
||||
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]);
|
||||
|
|
Loading…
Reference in New Issue