static int sensor_subdev_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
{
struct sensor_info *info = to_sensor(sd);
int ret = 0;
struct platform_device *pdev;
int irq = 0;
v4l2_dbg(1, sensor_debug, sd, "%s", __func__);
ret = sensor_set_clock(sd, FLITE_ID_C);
if (ret < 0) {
sensor_err("sensor set clock fail");
return ret;
}
ret = sensor_init_controls(info);
if (ret < 0) {
sensor_err("sensor init contols fail");
return ret;
}
fh->vfh.ctrl_handler = &info->handle;
sensor_init_formats(sd, fh);
pdev = get_mipi_csis_pdev(FLITE_ID_C);
irq = platform_get_irq(pdev, 0);
v4l2_dbg(1, sensor_debug, sd, "%s : mipi irq num : %d", __func__, irq);
enable_irq(irq);
return 0;
}
static int sensor_ctrl_frame_rate(struct v4l2_subdev *sd, int val)
{
struct sensor_info *info = to_sensor(sd);
u8 *value = (u8 *)&val;
int ret = 0;
sensor_dbg("val = %d", val);
if (info->status == STATUS_STREAMING) {
ret = sensor_add_request(sd, SENSOR_REG_VIS_DURATION_MSB, value[1]);
if (ret < 0) {
sensor_err("Failed to add request(MSB)");
goto ret;
}
ret = sensor_add_request(sd, SENSOR_REG_VIS_DURATION_LSB, value[0]);
if (ret < 0) {
sensor_err("Failed to add request(LSB)");
goto ret;
}
} else {
ret = vision_sensor_write_reg(sd, SENSOR_REG_VIS_DURATION_MSB, (u8)value[1], I2C_8BIT);
if (ret < 0) {
sensor_err("file to write reg(ret = %d)", ret);
goto ret;
}
ret = vision_sensor_write_reg(sd, SENSOR_REG_VIS_DURATION_LSB, (u8)value[0], I2C_8BIT);
if (ret < 0) {
sensor_err("file to write reg(ret = %d)", ret);
goto ret;
}
}
ret:
return ret;
}
/*
* sensor_straming - start and stop streaming.
*/
int sensor_streaming(struct v4l2_subdev *sd, enum sensor_status state)
{
struct sensor_info *info = to_sensor(sd);
int ret = 0;
if (state == info->status) {
sensor_err("sensor is already stream %s",
state == STATUS_STREAMING ? "on" : "off");
return ret;
}
ret = vision_sensor_write_reg(sd, 0x4100, (u8)state, I2C_8BIT);
if (ret < 0) {
sensor_err("file to write reg(ret = %d)", ret);
return ret;
}
if (state == STATUS_STANDBY) {
ret = sensor_drain_request(sd, true);
if (ret < 0) {
sensor_err("fail to drain request");
return ret;
}
}
info->status = state;
return ret;
}
static int sensor_s_power(struct v4l2_subdev *sd, int on)
{
struct sensor_info *info = to_sensor(sd);
int ret;
if (on) {
/*
* HACK
* s_power of flite make mux to change to ipll and then
* mclk for vision should be cacaluate again after mux setting changes
* vision clock source is channel 2
*/
ret = sensor_set_clock(sd, FLITE_ID_C);
if (ret < 0)
sensor_err("sensor set clock fail");
ret = sensor_power(info, true);
if (!ret)
ret = sensor_setup_default(sd);
} else {
ret = sensor_power(info, false);
}
return ret;
}
static int sensor_add_request(struct v4l2_subdev *sd, u32 addr, u8 val)
{
struct sensor_info *info = to_sensor(sd);
if (info->req_head == NULL) {
info->req_head = kzalloc(sizeof(struct sensor_req_list), GFP_ATOMIC);
if (info->req_head == NULL) {
sensor_err("Failed to allocate new requeset\n");
return -ENOMEM;
}
info->req_tail = info->req_head;
info->req_head->next = NULL;
info->req_head->addr = addr;
info->req_head->value = val;
} else if (info->req_tail->next == NULL) {
info->req_tail->next = kzalloc(sizeof(struct sensor_req_list), GFP_ATOMIC);
if (info->req_tail->next == NULL) {
sensor_err("Failed to allocate new requeset\n");
return -ENOMEM;
}
info->req_tail = info->req_tail->next;
info->req_tail->next = NULL;
info->req_tail->addr = addr;
info->req_tail->value = val;
} else {
sensor_err("Failed to add request, abnormal state!");
return -1;
}
return 0;
}
static int sensor_ctrl_rg_weight(struct v4l2_subdev *sd, int val)
{
struct sensor_info *info = to_sensor(sd);
u8 *value = (u8 *)&val;
int ret = 0;
sensor_dbg("val = %d", val);
if (info->status == STATUS_STREAMING) {
ret = sensor_add_request(sd, SENSOR_REG_VIS_GAIN_RED, value[1]);
if (ret < 0) {
sensor_err("Failed to add request(RED)");
goto ret;
}
ret = sensor_add_request(sd, SENSOR_REG_VIS_GAIN_GREEN, value[0]);
if (ret < 0) {
sensor_err("Failed to add request(GREEN)");
goto ret;
}
} else {
ret = vision_sensor_write_reg(sd, SENSOR_REG_VIS_GAIN_RED, (u8)value[1], I2C_8BIT);
if (ret < 0) {
sensor_err("file to write reg(ret = %d)", ret);
goto ret;
}
ret = vision_sensor_write_reg(sd, SENSOR_REG_VIS_GAIN_GREEN, (u8)value[0], I2C_8BIT);
if (ret < 0) {
sensor_err("file to write reg(ret = %d)", ret);
goto ret;
}
}
ret:
return ret;
}
static int sensor_log_status(struct v4l2_subdev *sd)
{
struct sensor_info *info = to_sensor(sd);
v4l2_ctrl_handler_log_status(&info->handle, sd->name);
return 0;
}
static int sensor_g_mbus_fmt(struct v4l2_subdev *sd,
struct v4l2_mbus_framefmt *ffmt)
{
struct sensor_info *info = to_sensor(sd);
*ffmt = info->fmt;
return 0;
}
static irqreturn_t sensor_irq_handler(int irq, void *data)
{
struct v4l2_subdev *sd = data;
struct sensor_info *info = to_sensor(sd);
v4l2_info(sd, "%s\n", __func__);
schedule_work(&info->work);
return IRQ_HANDLED;
}
static int sensor_subdev_close(struct v4l2_subdev *sd,
struct v4l2_subdev_fh *fh)
{
struct sensor_info *info = to_sensor(sd);
v4l2_dbg(1, sensor_debug, sd, "%s", __func__);
v4l2_ctrl_handler_free(&info->handle);
return 0;
}
struct sensor_req_list *sensor_get_request(struct v4l2_subdev *sd)
{
struct sensor_info *info = to_sensor(sd);
struct sensor_req_list *req = NULL;
req = info->req_head;
if (info->req_head != NULL)
info->req_head = info->req_head->next;
return req;
}
static int sensor_s_mbus_fmt(struct v4l2_subdev *sd,
struct v4l2_mbus_framefmt *ffmt)
{
struct sensor_info *info = to_sensor(sd);
int ret = -EINVAL;
info->fmt = default_fmt;
info->fmt.width = ffmt->width;
info->fmt.height = ffmt->height;
info->code = ffmt->code;
return ret;
}
static int sensor_remove(struct i2c_client *client)
{
struct v4l2_subdev *sd = i2c_get_clientdata(client);
struct sensor_info *info = to_sensor(sd);
v4l2_device_unregister_subdev(sd);
v4l2_ctrl_handler_free(&info->handle);
free_irq(info->pdata->irq, sd);
gpio_free(info->pdata->gpio_rst);
media_entity_cleanup(&sd->entity);
kfree(info);
return 0;
}
static int sensor_s_power(struct v4l2_subdev *sd, int on)
{
struct sensor_info *info = to_sensor(sd);
int ret;
if (on) {
ret = sensor_power(info, true);
if (!ret)
ret = sensor_setup_default(sd);
} else {
ret = sensor_power(info, false);
}
return ret;
}
static int sensor_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
struct v4l2_subdev_format *fmt)
{
struct sensor_info *info = to_sensor(sd);
struct v4l2_mbus_framefmt *format;
if (fmt->pad != 0)
return -EINVAL;
format = __find_format(info, fh, fmt->which);
if (!format)
return -EINVAL;
fmt->format = *format;
return 0;
}
static int sensor_subdev_close(struct v4l2_subdev *sd,
struct v4l2_subdev_fh *fh)
{
struct sensor_info *info = to_sensor(sd);
struct platform_device *pdev;
int irq = 0;
v4l2_dbg(1, sensor_debug, sd, "%s", __func__);
pdev = get_mipi_csis_pdev(FLITE_ID_B);
irq = platform_get_irq(pdev, 0);
v4l2_dbg(1, sensor_debug, sd, "%s : mipi irq num : %d", __func__, irq);
disable_irq(irq);
v4l2_ctrl_handler_free(&info->handle);
return 0;
}
static int sensor_ctrl_auto_exposure(struct v4l2_subdev *sd, int val)
{
struct sensor_info *info = to_sensor(sd);
u8 *value = (u8 *)&val;
int ret = 0;
sensor_dbg("val = %d", val);
if (info->status == STATUS_STREAMING) {
ret = sensor_add_request(sd, SENSOR_REG_VIS_AE_OFF, value[0]);
if (ret < 0)
sensor_err("Failed to add request");
} else {
ret = vision_sensor_write_reg(sd, SENSOR_REG_VIS_AE_OFF, (u8)value[0], I2C_8BIT);
if (ret < 0)
sensor_err("file to write reg(ret = %d)", ret);
}
return ret;
}
static void sensor_handler(void *data, int arg)
{
struct v4l2_subdev *sd = data;
struct sensor_info *info = to_sensor(sd);
sensor_dbg("%s: irq(%d), info->status(%d)", __func__, arg, info->status);
switch (arg) {
case FLITE_REG_CISTATUS_IRQ_SRC_FRMSTART:
sensor_dbg("Flite frame start");
tasklet_schedule(&info->tasklet_flite_str);
break;
case FLITE_REG_CISTATUS_IRQ_SRC_FRMEND:
sensor_dbg("Flite frame end");
schedule_work(&info->work);
break;
default :
sensor_err("unknown irq(%d)", arg);
break;
}
}
static int sensor_drain_request(struct v4l2_subdev *sd, bool isValid)
{
struct sensor_info *info = to_sensor(sd);
struct sensor_req_list *req = NULL;
int ret = 0;
req = sensor_get_request(&info->sd);
while (req) {
if (isValid)
ret = vision_sensor_write_reg(&info->sd, req->addr, (u8)req->value, I2C_8BIT);
kfree(req);
req = NULL;
if (ret < 0) {
sensor_err("file to write reg(ret = %d)", ret);
break;
}
req = sensor_get_request(&info->sd);
}
return ret;
}
static int sensor_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
struct v4l2_subdev_format *fmt)
{
struct sensor_info *info = to_sensor(sd);
struct v4l2_mbus_framefmt *format = &fmt->format;
struct v4l2_mbus_framefmt *sfmt;
if (fmt->pad != 0)
return -EINVAL;
sfmt = __find_format(info, fh, fmt->which);
if (!sfmt)
return 0;
sfmt = &default_fmt;
sfmt->width = format->width;
sfmt->height = format->height;
if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
info->code = format->code;
return 0;
}
/*
* sensor_setup_default - set default size & fps in the monitor mode.
*/
static int sensor_setup_default(struct v4l2_subdev *sd)
{
struct sensor_info *info = to_sensor(sd);
int ret = 0;
/* TODO : Hear set sensor init */
info->fmt = default_fmt;
/* sensor init */
/* 8 bit mode */
vision_sensor_write_reg(sd, 0x7030, 0x0E, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7031, 0x2F, I2C_8BIT);
/* Analog Tuning */
vision_sensor_write_reg(sd, 0x7067, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7074, 0x22, I2C_8BIT);
/* Dark Tuning */
vision_sensor_write_reg(sd, 0x7402, 0x1F, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7403, 0xC0, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7247, 0x01, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7412, 0x09, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7413, 0xB9, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7430, 0x05, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7432, 0x02, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7433, 0x32, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7075, 0x3D, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7066, 0x09, I2C_8BIT);
/* AE setting */
vision_sensor_write_reg(sd, 0x6000, 0x11, I2C_8BIT);
vision_sensor_write_reg(sd, 0x6001, 0x11, I2C_8BIT);
vision_sensor_write_reg(sd, 0x6002, 0x14, I2C_8BIT);
vision_sensor_write_reg(sd, 0x6003, 0x41, I2C_8BIT);
vision_sensor_write_reg(sd, 0x6004, 0x14, I2C_8BIT);
vision_sensor_write_reg(sd, 0x6005, 0x41, I2C_8BIT);
vision_sensor_write_reg(sd, 0x6006, 0x11, I2C_8BIT);
vision_sensor_write_reg(sd, 0x6007, 0x11, I2C_8BIT);
/* Number of pixel */
vision_sensor_write_reg(sd, 0x5030, 0x20, I2C_8BIT);
vision_sensor_write_reg(sd, 0x5031, 0xB4, I2C_8BIT);
/* AE target */
vision_sensor_write_reg(sd, 0x600A, 0x2A, I2C_8BIT);
/* AE speed */
vision_sensor_write_reg(sd, 0x5034, 0x00, I2C_8BIT);
/* Inner Target Tolerance */
vision_sensor_write_reg(sd, 0x503F, 0x03, I2C_8BIT);
/* patch height */
vision_sensor_write_reg(sd, 0x6015, 0x19, I2C_8BIT);
/* G + R Setting */
/* Vision Senser Data = 0.5*Gr + 0.5*R */
vision_sensor_write_reg(sd, 0x6029, 0x02, I2C_8BIT);
vision_sensor_write_reg(sd, 0x602A, 0x02, I2C_8BIT);
/* For Analog Gain 16x */
vision_sensor_write_reg(sd, 0x7018, 0xCF, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7019, 0xDB, I2C_8BIT);
vision_sensor_write_reg(sd, 0x702A, 0x8D, I2C_8BIT);
vision_sensor_write_reg(sd, 0x702B, 0x60, I2C_8BIT);
vision_sensor_write_reg(sd, 0x5035, 0x02, I2C_8BIT);
/* frame rate - default 10fps*/
vision_sensor_write_reg(sd, SENSOR_REG_VIS_DURATION_MSB, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, SENSOR_REG_VIS_DURATION_LSB, 0x6A, I2C_8BIT);
/* BIT_RATE_MBPS_alv */
vision_sensor_write_reg(sd, 0x7351, 0x02, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7352, 0x48, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7353, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7354, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7339, 0x03, I2C_8BIT);
/* TODO : clock control is need */
#if 0
/* VT Pixel clock devider */
vision_sensor_write_reg(sd, 0x4300, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x4301, 0x08, I2C_8BIT);
/* VT System clock devider */
vision_sensor_write_reg(sd, 0x4302, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x4303, 0x01, I2C_8BIT);
/* Pre PLL clock devider */
vision_sensor_write_reg(sd, 0x4304, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x4305, 0x06, I2C_8BIT);
/* PLL multiplier */
vision_sensor_write_reg(sd, 0x4306, 0x01, I2C_8BIT);
vision_sensor_write_reg(sd, 0x4307, 0x46, I2C_8BIT);
/* Output Pixel clock devider */
vision_sensor_write_reg(sd, 0x4308, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x4309, 0x08, I2C_8BIT);
/* Output System clock devider */
vision_sensor_write_reg(sd, 0x430A, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x430B, 0x01, I2C_8BIT);
#endif
return ret;
}
static int sensor_ctrl_ae_weight(struct v4l2_subdev *sd, int cmd, int val)
{
struct sensor_info *info = to_sensor(sd);
u8 *value = (u8 *)&val;
int ret = 0;
sensor_dbg("cmd = %x val = %d", cmd, val);
switch (cmd) {
case V4L2_CID_SENSOR_SET_AE_WEIGHT_1x1_2:
if (info->status == STATUS_STREAMING) {
ret = sensor_add_request(sd, SENSOR_REG_VIS_AE_WINDOW_WEIGHT_1x1_2, value[0]);
} else {
ret = vision_sensor_write_reg(sd, SENSOR_REG_VIS_AE_WINDOW_WEIGHT_1x1_2, (u8)value[0], I2C_8BIT);
}
break;
case V4L2_CID_SENSOR_SET_AE_WEIGHT_1x3_4:
if (info->status == STATUS_STREAMING) {
ret = sensor_add_request(sd, SENSOR_REG_VIS_AE_WINDOW_WEIGHT_1x3_4, value[0]);
} else {
ret = vision_sensor_write_reg(sd, SENSOR_REG_VIS_AE_WINDOW_WEIGHT_1x3_4, (u8)value[0], I2C_8BIT);
}
break;
case V4L2_CID_SENSOR_SET_AE_WEIGHT_2x1_2:
if (info->status == STATUS_STREAMING) {
ret = sensor_add_request(sd, SENSOR_REG_VIS_AE_WINDOW_WEIGHT_2x1_2, value[0]);
} else {
ret = vision_sensor_write_reg(sd, SENSOR_REG_VIS_AE_WINDOW_WEIGHT_2x1_2, (u8)value[0], I2C_8BIT);
}
break;
case V4L2_CID_SENSOR_SET_AE_WEIGHT_2x3_4:
if (info->status == STATUS_STREAMING) {
ret = sensor_add_request(sd, SENSOR_REG_VIS_AE_WINDOW_WEIGHT_2x3_4, value[0]);
} else {
ret = vision_sensor_write_reg(sd, SENSOR_REG_VIS_AE_WINDOW_WEIGHT_2x3_4, (u8)value[0], I2C_8BIT);
}
break;
case V4L2_CID_SENSOR_SET_AE_WEIGHT_3x1_2:
if (info->status == STATUS_STREAMING) {
ret = sensor_add_request(sd, SENSOR_REG_VIS_AE_WINDOW_WEIGHT_3x1_2, value[0]);
} else {
ret = vision_sensor_write_reg(sd, SENSOR_REG_VIS_AE_WINDOW_WEIGHT_3x1_2, (u8)value[0], I2C_8BIT);
}
break;
case V4L2_CID_SENSOR_SET_AE_WEIGHT_3x3_4:
if (info->status == STATUS_STREAMING) {
ret = sensor_add_request(sd, SENSOR_REG_VIS_AE_WINDOW_WEIGHT_3x3_4, value[0]);
} else {
ret = vision_sensor_write_reg(sd, SENSOR_REG_VIS_AE_WINDOW_WEIGHT_3x3_4, (u8)value[0], I2C_8BIT);
}
break;
case V4L2_CID_SENSOR_SET_AE_WEIGHT_4x1_2:
if (info->status == STATUS_STREAMING) {
ret = sensor_add_request(sd, SENSOR_REG_VIS_AE_WINDOW_WEIGHT_4x1_2, value[0]);
} else {
ret = vision_sensor_write_reg(sd, SENSOR_REG_VIS_AE_WINDOW_WEIGHT_4x1_2, (u8)value[0], I2C_8BIT);
}
break;
case V4L2_CID_SENSOR_SET_AE_WEIGHT_4x3_4:
if (info->status == STATUS_STREAMING) {
ret = sensor_add_request(sd, SENSOR_REG_VIS_AE_WINDOW_WEIGHT_4x3_4, value[0]);
} else {
ret = vision_sensor_write_reg(sd, SENSOR_REG_VIS_AE_WINDOW_WEIGHT_4x3_4, (u8)value[0], I2C_8BIT);
}
break;
default :
sensor_err("Unknown CID");
ret = -EINVAL;
break;
}
if (ret < 0)
sensor_err("Failed to set ae weight(CID:%x)", cmd);
return ret;
}
/*
* sensor_setup_default - set default size & fps in the monitor mode.
*/
static int sensor_setup_default(struct v4l2_subdev *sd)
{
struct sensor_info *info = to_sensor(sd);
int ret = 0;
/* TODO : Hear set sensor init */
info->fmt = default_fmt;
/* sensor init */
/* 8 bit mode */
vision_sensor_write_reg(sd, 0x7203, 0x40, I2C_8BIT);
vision_sensor_write_reg(sd, 0x602B, 0x02, I2C_8BIT);
vision_sensor_write_reg(sd, 0x702A, 0x3D, I2C_8BIT);
vision_sensor_write_reg(sd, 0x702B, 0xB0, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7030, 0x0E, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7031, 0x2F, I2C_8BIT);
/* Analog Tuning */
vision_sensor_write_reg(sd, 0x7067, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7073, 0xFF, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7074, 0x22, I2C_8BIT);
/* Dark Tuning */
vision_sensor_write_reg(sd, 0x7042, 0x1F, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7403, 0xC0, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7245, 0x04, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7205, 0xA1, I2C_8BIT);
/* Remove Dark Band */
vision_sensor_write_reg(sd, 0x7430, 0x07, I2C_8BIT);
vision_sensor_write_reg(sd, 0x705C, 0x7E, I2C_8BIT);
/* Remove Sun spot */
vision_sensor_write_reg(sd, 0x702C, 0x3C, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7075, 0x3D, I2C_8BIT);
/* Remove CFPN */
vision_sensor_write_reg(sd, 0x7066, 0x0C, I2C_8BIT);
/* AE setting */
vision_sensor_write_reg(sd, 0x6000, 0x44, I2C_8BIT);
vision_sensor_write_reg(sd, 0x6001, 0x44, I2C_8BIT);
vision_sensor_write_reg(sd, 0x6002, 0x44, I2C_8BIT);
vision_sensor_write_reg(sd, 0x6003, 0x44, I2C_8BIT);
vision_sensor_write_reg(sd, 0x6004, 0x44, I2C_8BIT);
vision_sensor_write_reg(sd, 0x6005, 0x44, I2C_8BIT);
vision_sensor_write_reg(sd, 0x6006, 0x44, I2C_8BIT);
vision_sensor_write_reg(sd, 0x6007, 0x44, I2C_8BIT);
/* AE target */
vision_sensor_write_reg(sd, 0x600A, 0xB4, I2C_8BIT);
/* speed */
vision_sensor_write_reg(sd, 0x5034, 0x00, I2C_8BIT);
/* Cintc_min */
vision_sensor_write_reg(sd, 0x5017, 0x01, I2C_8BIT);
/* Number of pixel */
vision_sensor_write_reg(sd, 0x5030, 0x4A, I2C_8BIT);
vision_sensor_write_reg(sd, 0x5031, 0xC0, I2C_8BIT);
/* G + R Setting */
/* Vision Senser Data = 0.5*Gr + 0.5*R */
vision_sensor_write_reg(sd, 0x6029, 0x02, I2C_8BIT);
vision_sensor_write_reg(sd, 0x602A, 0x02, I2C_8BIT);
/* For Analog Gain 16x */
vision_sensor_write_reg(sd, 0x7018, 0xCF, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7019, 0xDB, I2C_8BIT);
vision_sensor_write_reg(sd, 0x702A, 0x8D, I2C_8BIT);
vision_sensor_write_reg(sd, 0x702B, 0x60, I2C_8BIT);
vision_sensor_write_reg(sd, 0x5035, 0x02, I2C_8BIT);
/* BIT_RATE_MBPS_alv */
vision_sensor_write_reg(sd, 0x7351, 0x02, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7352, 0x48, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7353, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7354, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x7339, 0x03, I2C_8BIT);
/* Analog gain */
vision_sensor_write_reg(sd, 0x4204, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x4205, 0x32, I2C_8BIT);
/* frame rate - default 10fps*/
vision_sensor_write_reg(sd, SENSOR_REG_VIS_DURATION_MSB, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, SENSOR_REG_VIS_DURATION_LSB, 0x6A, I2C_8BIT);
/* TODO : clock control is need */
#if 0
/* VT Pixel clock devider */
vision_sensor_write_reg(sd, 0x4300, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x4301, 0x08, I2C_8BIT);
/* VT System clock devider */
vision_sensor_write_reg(sd, 0x4302, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x4303, 0x01, I2C_8BIT);
/* Pre PLL clock devider */
vision_sensor_write_reg(sd, 0x4304, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x4305, 0x06, I2C_8BIT);
/* PLL multiplier */
vision_sensor_write_reg(sd, 0x4306, 0x01, I2C_8BIT);
vision_sensor_write_reg(sd, 0x4307, 0x46, I2C_8BIT);
/* Output Pixel clock devider */
vision_sensor_write_reg(sd, 0x4308, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x4309, 0x08, I2C_8BIT);
/* Output System clock devider */
vision_sensor_write_reg(sd, 0x430A, 0x00, I2C_8BIT);
vision_sensor_write_reg(sd, 0x430B, 0x01, I2C_8BIT);
#endif
return ret;
}
/**
* Filtering function implementation
*/
void srs_env_model::COcFilterRaycast::filterInternal( tButServerOcTree & tree )
{
assert( m_cloudPtr != 0 );
m_numLeafsRemoved = 0;
m_numLeafsOutOfCone = 0;
//octomap::pose6d sensor_pose(m_sensor_origin.x(), m_sensor_origin.y(), m_sensor_origin.z(), 0, 0, 0);
// Is camera model initialized?
if (!m_bCamModelInitialized)
{
ROS_ERROR("ERROR: camera model not initialized.");
return;
}
// Get sensor origin
m_sensor_origin = getSensorOrigin(m_sensor_header);
tf::StampedTransform trans;
try
{
m_tfListener.lookupTransform(m_sensor_header.frame_id, m_cloudPtr->header.frame_id, m_cloudPtr->header.stamp, trans);
}
catch (tf::TransformException& ex)
{
ROS_ERROR_STREAM("ERROR: Cannot find transform: " << m_cloudPtr->header.frame_id << " -> " << m_sensor_header.frame_id);
return;
}
tf::Transform to_sensor = trans;
// compute bbx from sensor cone
octomap::point3d min;
octomap::point3d max;
computeBBX(m_sensor_header, min, max);
double resolution(tree.getResolution());
float probMiss(tree.getProbMissLog());
boost::mutex::scoped_lock lock(m_lockCamera);
float probDeleted(tree.getProbMiss() * 1.0f);
unsigned query_time = time(NULL);
unsigned max_update_time = 1;
for (tButServerOcTree::leaf_bbx_iterator it =
tree.begin_leafs_bbx(min, max), end = tree.end_leafs_bbx(); it
!= end; ++it) {
if (tree.isNodeOccupied(*it) && ((query_time - it->getTimestamp())
> max_update_time)) {
tf::Point pos(it.getX(), it.getY(), it.getZ());
tf::Point posRel = to_sensor(pos);
cv::Point2d uv = m_camera_model.project3dToPixel(cv::Point3d(
posRel.x(), posRel.y(), posRel.z()));
// ignore point if not in sensor cone
if (!inSensorCone(uv))
{
++m_numLeafsOutOfCone;
continue;
}
// ignore point if it is occluded in the map
if (isOccludedMap(m_sensor_origin, it.getCoordinate(), resolution, tree))
continue;
// otherwise: degrade node
// it->setColor(255, 0, 0);
it->setValue(probMiss);
++m_numLeafsRemoved;
}
}
}
static int sensor_set_clock(struct v4l2_subdev *sd, int sensor_id)
{
struct sensor_info *info = to_sensor(sd);
char sensor_mclk[20];
struct clk *sclk_mout_sensor = NULL;
struct clk *sclk_sensor = NULL;
unsigned long sensor;
if (soc_is_exynos5410()) {
/* SENSOR */
sprintf(sensor_mclk, "sclk_isp_sensor%d", sensor_id);
sclk_mout_sensor = clk_get(NULL, "sclk_mout_isp_sensor");
if (IS_ERR(sclk_mout_sensor)) {
pr_err("%s : clk_get(sclk_mout_sensor%d) failed\n", __func__, sensor_id);
return PTR_ERR(sclk_mout_sensor);
}
sclk_sensor = clk_get(NULL, sensor_mclk);
if (IS_ERR(sclk_sensor)) {
pr_err("%s : clk_get(sclk_isp_sensor%d) failed\n", __func__, sensor_id);
return PTR_ERR(sclk_sensor);
}
clk_set_parent(sclk_mout_sensor, clk_get(NULL, "mout_ipll"));
clk_set_rate(sclk_sensor, 24 * 1000000);
sensor = clk_get_rate(sclk_sensor);
pr_debug("sensor : %ld\n", sensor);
clk_put(sclk_mout_sensor);
clk_put(sclk_sensor);
} else {
char source_name[30];
struct clk *mout_ipll = NULL;
struct clk *sclk_isp_sensor = NULL;
u32 sensor_mclk;
mout_ipll = clk_get(NULL, "mout_ipll");
if (IS_ERR(mout_ipll)) {
pr_err("%s : clk_get(mout_ipll ) failed\n", __func__);
return PTR_ERR(mout_ipll);
}
snprintf(source_name, sizeof(source_name), "sclk_isp_sensor%d", sensor_id);
sclk_isp_sensor = clk_get(NULL, source_name);
if (IS_ERR(sclk_isp_sensor)) {
pr_err("%s : clk_get(sclk_isp_sensor%d) failed\n", __func__, sensor_id);
return PTR_ERR(sclk_isp_sensor);
}
clk_set_parent(sclk_isp_sensor, mout_ipll);
/* HACK : 24Mhz */
clk_set_rate(sclk_isp_sensor, 24 * 1000 * 1000);
pr_info("sclk_isp_sensor%d mclk : %ldHz\n", sensor_id,
clk_get_rate(sclk_isp_sensor));
clk_put(mout_ipll);
clk_put(sclk_isp_sensor);
}
return 0;
}