159 lines
3.5 KiB
C++
159 lines
3.5 KiB
C++
#pragma once
|
|
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
#include <pthread.h>
|
|
#include <memory>
|
|
#include <atomic>
|
|
#include "messaging.hpp"
|
|
|
|
#include "msmb_isp.h"
|
|
#include "msmb_ispif.h"
|
|
#include "msmb_camera.h"
|
|
#include "msm_cam_sensor.h"
|
|
|
|
#include "common/mat.h"
|
|
#include "common/visionbuf.h"
|
|
#include "common/buffering.h"
|
|
#include "common/utilpp.h"
|
|
|
|
#include "camera_common.h"
|
|
|
|
#define FRAME_BUF_COUNT 4
|
|
#define METADATA_BUF_COUNT 4
|
|
|
|
#define DEVICE_OP3 0
|
|
#define DEVICE_OP3T 1
|
|
#define DEVICE_LP3 2
|
|
|
|
#define NUM_FOCUS 8
|
|
|
|
#define LP3_AF_DAC_DOWN 366
|
|
#define LP3_AF_DAC_UP 634
|
|
#define LP3_AF_DAC_M 440
|
|
#define LP3_AF_DAC_3SIG 52
|
|
#define OP3T_AF_DAC_DOWN 224
|
|
#define OP3T_AF_DAC_UP 456
|
|
#define OP3T_AF_DAC_M 300
|
|
#define OP3T_AF_DAC_3SIG 96
|
|
|
|
#define FOCUS_RECOVER_PATIENCE 50 // 2.5 seconds of complete blur
|
|
#define FOCUS_RECOVER_STEPS 240 // 6 seconds
|
|
|
|
typedef struct CameraState CameraState;
|
|
|
|
typedef int (*camera_apply_exposure_func)(CameraState *s, int gain, int integ_lines, int frame_length);
|
|
|
|
typedef struct StreamState {
|
|
struct msm_isp_buf_request buf_request;
|
|
struct msm_vfe_axi_stream_request_cmd stream_req;
|
|
struct msm_isp_qbuf_info qbuf_info[FRAME_BUF_COUNT];
|
|
VisionBuf *bufs;
|
|
} StreamState;
|
|
|
|
typedef struct CameraState {
|
|
int camera_num;
|
|
int camera_id;
|
|
CameraInfo ci;
|
|
|
|
int device;
|
|
|
|
uint32_t pixel_clock;
|
|
uint32_t line_length_pclk;
|
|
unsigned int max_gain;
|
|
|
|
unique_fd csid_fd;
|
|
unique_fd csiphy_fd;
|
|
unique_fd sensor_fd;
|
|
unique_fd isp_fd;
|
|
unique_fd eeprom_fd;
|
|
// rear only
|
|
unique_fd ois_fd, actuator_fd;
|
|
uint16_t infinity_dac;
|
|
|
|
struct msm_vfe_axi_stream_cfg_cmd stream_cfg;
|
|
|
|
size_t eeprom_size;
|
|
uint8_t *eeprom;
|
|
|
|
// uint32_t camera_bufs_ids[FRAME_BUF_COUNT];
|
|
|
|
pthread_mutex_t frame_info_lock;
|
|
FrameMetadata frame_metadata[METADATA_BUF_COUNT];
|
|
int frame_metadata_idx;
|
|
float cur_exposure_frac;
|
|
float cur_gain_frac;
|
|
int cur_gain;
|
|
int cur_frame_length;
|
|
int cur_integ_lines;
|
|
|
|
std::atomic<float> digital_gain;
|
|
|
|
StreamState ss[3];
|
|
|
|
uint64_t last_t;
|
|
|
|
camera_apply_exposure_func apply_exposure;
|
|
|
|
int16_t focus[NUM_FOCUS];
|
|
uint8_t confidence[NUM_FOCUS];
|
|
|
|
float focus_err;
|
|
|
|
uint16_t cur_step_pos;
|
|
uint16_t cur_lens_pos;
|
|
uint64_t last_sag_ts;
|
|
float last_sag_acc_z;
|
|
std::atomic<float> lens_true_pos;
|
|
|
|
std::atomic<int> self_recover; // af recovery counter, neg is patience, pos is active
|
|
|
|
int fps;
|
|
|
|
mat3 transform;
|
|
|
|
CameraBuf buf;
|
|
} CameraState;
|
|
|
|
|
|
typedef struct MultiCameraState {
|
|
int device;
|
|
|
|
unique_fd ispif_fd;
|
|
unique_fd msmcfg_fd;
|
|
unique_fd v4l_fd;
|
|
|
|
cl_mem rgb_conv_roi_cl, rgb_conv_result_cl, rgb_conv_filter_cl;
|
|
uint16_t lapres[(ROI_X_MAX-ROI_X_MIN+1)*(ROI_Y_MAX-ROI_Y_MIN+1)];
|
|
|
|
VisionBuf focus_bufs[FRAME_BUF_COUNT];
|
|
VisionBuf stats_bufs[FRAME_BUF_COUNT];
|
|
|
|
cl_program prg_rgb_laplacian;
|
|
cl_kernel krnl_rgb_laplacian;
|
|
|
|
std::unique_ptr<uint8_t[]> rgb_roi_buf;
|
|
std::unique_ptr<int16_t[]> conv_result;
|
|
|
|
int conv_cl_localMemSize;
|
|
size_t conv_cl_globalWorkSize[2];
|
|
size_t conv_cl_localWorkSize[2];
|
|
|
|
CameraState rear;
|
|
CameraState front;
|
|
|
|
SubMaster *sm_front;
|
|
SubMaster *sm_rear;
|
|
PubMaster *pm;
|
|
|
|
} MultiCameraState;
|
|
|
|
void cameras_init(MultiCameraState *s, cl_device_id device_id, cl_context ctx);
|
|
void cameras_open(MultiCameraState *s);
|
|
void cameras_run(MultiCameraState *s);
|
|
void cameras_close(MultiCameraState *s);
|
|
|
|
void camera_autoexposure(CameraState *s, float grey_frac);
|
|
void actuator_move(CameraState *s, uint16_t target);
|
|
int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size_t size, int data_type);
|