diff --git a/selfdrive/camerad/cameras/camera_frame_stream.cc b/selfdrive/camerad/cameras/camera_frame_stream.cc index 45e5013206..aecca564a2 100644 --- a/selfdrive/camerad/cameras/camera_frame_stream.cc +++ b/selfdrive/camerad/cameras/camera_frame_stream.cc @@ -94,8 +94,6 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = { }; void cameras_init(DualCameraState *s) { - memset(s, 0, sizeof(*s)); - camera_init(&s->rear, CAMERA_ID_IMX298, 20); s->rear.transform = (mat3){{ 1.0, 0.0, 0.0, diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc index 6e9c3e3a24..dfed65238c 100644 --- a/selfdrive/camerad/cameras/camera_qcom.cc +++ b/selfdrive/camerad/cameras/camera_qcom.cc @@ -107,8 +107,6 @@ static void camera_release_buffer(void* cookie, int buf_idx) { static void camera_init(CameraState *s, int camera_id, int camera_num, uint32_t pixel_clock, uint32_t line_length_pclk, unsigned int max_gain, unsigned int fps) { - memset(s, 0, sizeof(*s)); - s->camera_num = camera_num; s->camera_id = camera_id; @@ -261,8 +259,6 @@ static int imx179_s5k3p8sp_apply_exposure(CameraState *s, int gain, int integ_li } void cameras_init(DualCameraState *s) { - memset(s, 0, sizeof(*s)); - char project_name[1024] = {0}; property_get("ro.boot.project_name", project_name, ""); @@ -396,7 +392,9 @@ static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) { if (err == 0) { s->cur_exposure_frac = exposure_frac; + pthread_mutex_lock(&s->frame_info_lock); s->cur_gain_frac = gain_frac; + pthread_mutex_unlock(&s->frame_info_lock); } //LOGD("set exposure: %f %f - %d", exposure_frac, gain_frac, err); @@ -413,16 +411,20 @@ static void do_autoexposure(CameraState *s, float grey_frac) { const unsigned int exposure_time_min = 16; const unsigned int exposure_time_max = frame_length - 11; // copied from set_exposure() + float cur_gain_frac = s->cur_gain_frac; float exposure_factor = pow(1.05, (target_grey - grey_frac) / 0.05); - if (s->cur_gain_frac > 0.125 && exposure_factor < 1) { - s->cur_gain_frac *= exposure_factor; + if (cur_gain_frac > 0.125 && exposure_factor < 1) { + cur_gain_frac *= exposure_factor; } else if (s->cur_integ_lines * exposure_factor <= exposure_time_max && s->cur_integ_lines * exposure_factor >= exposure_time_min) { // adjust exposure time first s->cur_exposure_frac *= exposure_factor; - } else if (s->cur_gain_frac * exposure_factor <= gain_frac_max && s->cur_gain_frac * exposure_factor >= gain_frac_min) { - s->cur_gain_frac *= exposure_factor; + } else if (cur_gain_frac * exposure_factor <= gain_frac_max && cur_gain_frac * exposure_factor >= gain_frac_min) { + cur_gain_frac *= exposure_factor; } + pthread_mutex_lock(&s->frame_info_lock); + s->cur_gain_frac = cur_gain_frac; + pthread_mutex_unlock(&s->frame_info_lock); - set_exposure(s, s->cur_exposure_frac, s->cur_gain_frac); + set_exposure(s, s->cur_exposure_frac, cur_gain_frac); } else { // keep the old for others float new_exposure = s->cur_exposure_frac; @@ -1794,15 +1796,16 @@ static void do_autofocus(CameraState *s) { const int dac_up = s->device == DEVICE_LP3? LP3_AF_DAC_UP:OP3T_AF_DAC_UP; const int dac_down = s->device == DEVICE_LP3? LP3_AF_DAC_DOWN:OP3T_AF_DAC_DOWN; + float lens_true_pos = s->lens_true_pos; if (!isnan(err)) { // learn lens_true_pos - s->lens_true_pos -= err*focus_kp; + lens_true_pos -= err*focus_kp; } // stay off the walls - s->lens_true_pos = clamp(s->lens_true_pos, dac_down, dac_up); - - int target = clamp(s->lens_true_pos - sag, dac_down, dac_up); + lens_true_pos = clamp(lens_true_pos, dac_down, dac_up); + int target = clamp(lens_true_pos - sag, dac_down, dac_up); + s->lens_true_pos = lens_true_pos; /*char debug[4096]; char *pdebug = debug; diff --git a/selfdrive/camerad/cameras/camera_qcom.h b/selfdrive/camerad/cameras/camera_qcom.h index 81c84b9ebf..812f1a5968 100644 --- a/selfdrive/camerad/cameras/camera_qcom.h +++ b/selfdrive/camerad/cameras/camera_qcom.h @@ -1,10 +1,10 @@ -#ifndef CAMERA_H -#define CAMERA_H +#pragma once #include #include #include #include +#include #include "messaging.hpp" #include "msmb_isp.h" @@ -96,7 +96,7 @@ typedef struct CameraState { int cur_frame_length; int cur_integ_lines; - float digital_gain; + std::atomic digital_gain; StreamState ss[3]; @@ -113,9 +113,9 @@ typedef struct CameraState { uint16_t cur_lens_pos; uint64_t last_sag_ts; float last_sag_acc_z; - float lens_true_pos; + std::atomic lens_true_pos; - int self_recover; // af recovery counter, neg is patience, pos is active + std::atomic self_recover; // af recovery counter, neg is patience, pos is active int fps; @@ -144,5 +144,3 @@ int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size #ifdef __cplusplus } // extern "C" #endif - -#endif diff --git a/selfdrive/camerad/cameras/camera_webcam.cc b/selfdrive/camerad/cameras/camera_webcam.cc index 4ce1c442da..4a7c6626a6 100644 --- a/selfdrive/camerad/cameras/camera_webcam.cc +++ b/selfdrive/camerad/cameras/camera_webcam.cc @@ -222,7 +222,6 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = { }; void cameras_init(DualCameraState *s) { - memset(s, 0, sizeof(*s)); camera_init(&s->rear, CAMERA_ID_LGC920, 20); s->rear.transform = (mat3){{ diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index aef7925536..658ab55bae 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -477,25 +477,26 @@ void* processing_thread(void *arg) { t10 = millis_since_boot();*/ // setup self recover + const float lens_true_pos = s->cameras.rear.lens_true_pos; if (is_blur(&s->lapres[0]) && - (s->cameras.rear.lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_DOWN:OP3T_AF_DAC_DOWN)+1 || - s->cameras.rear.lens_true_pos > (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_UP:OP3T_AF_DAC_UP)-1) && + (lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_DOWN:OP3T_AF_DAC_DOWN)+1 || + lens_true_pos > (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_UP:OP3T_AF_DAC_UP)-1) && s->cameras.rear.self_recover < 2) { // truly stuck, needs help s->cameras.rear.self_recover -= 1; if (s->cameras.rear.self_recover < -FOCUS_RECOVER_PATIENCE) { LOGW("rear camera bad state detected. attempting recovery from %.1f, recover state is %d", - s->cameras.rear.lens_true_pos, s->cameras.rear.self_recover); - s->cameras.rear.self_recover = FOCUS_RECOVER_STEPS + ((s->cameras.rear.lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M:OP3T_AF_DAC_M))?1:0); // parity determined by which end is stuck at + lens_true_pos, s->cameras.rear.self_recover.load()); + s->cameras.rear.self_recover = FOCUS_RECOVER_STEPS + ((lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M:OP3T_AF_DAC_M))?1:0); // parity determined by which end is stuck at } - } else if ((s->cameras.rear.lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M - LP3_AF_DAC_3SIG:OP3T_AF_DAC_M - OP3T_AF_DAC_3SIG) || - s->cameras.rear.lens_true_pos > (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M + LP3_AF_DAC_3SIG:OP3T_AF_DAC_M + OP3T_AF_DAC_3SIG)) && + } else if ((lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M - LP3_AF_DAC_3SIG:OP3T_AF_DAC_M - OP3T_AF_DAC_3SIG) || + lens_true_pos > (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M + LP3_AF_DAC_3SIG:OP3T_AF_DAC_M + OP3T_AF_DAC_3SIG)) && s->cameras.rear.self_recover < 2) { // in suboptimal position with high prob, but may still recover by itself s->cameras.rear.self_recover -= 1; if (s->cameras.rear.self_recover < -(FOCUS_RECOVER_PATIENCE*3)) { - LOGW("rear camera bad state detected. attempting recovery from %.1f, recover state is %d", s->cameras.rear.lens_true_pos, s->cameras.rear.self_recover); - s->cameras.rear.self_recover = FOCUS_RECOVER_STEPS/2 + ((s->cameras.rear.lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M:OP3T_AF_DAC_M))?1:0); + LOGW("rear camera bad state detected. attempting recovery from %.1f, recover state is %d", lens_true_pos, s->cameras.rear.self_recover.load()); + s->cameras.rear.self_recover = FOCUS_RECOVER_STEPS/2 + ((lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M:OP3T_AF_DAC_M))?1:0); } } else if (s->cameras.rear.self_recover < 0) { s->cameras.rear.self_recover += 1; // reset if fine @@ -1267,7 +1268,7 @@ int main(int argc, char *argv[]) { signal(SIGINT, (sighandler_t)set_do_exit); signal(SIGTERM, (sighandler_t)set_do_exit); - VisionState state = {0}; + VisionState state = {}; VisionState *s = &state; clu_init(); diff --git a/selfdrive/camerad/test/camera/test.c b/selfdrive/camerad/test/camera/test.c index 8a7d44517b..2a7272cb9f 100644 --- a/selfdrive/camerad/test/camera/test.c +++ b/selfdrive/camerad/test/camera/test.c @@ -34,7 +34,7 @@ void tbuffer_stop(TBuffer *tb) { } int main() { - DualCameraState s; + DualCameraState s={}; cameras_init(&s); VisionBuf camera_bufs_rear[0x10] = {0}; VisionBuf camera_bufs_focus[0x10] = {0};