static int autogain_active(struct v4lprocessing_data *data) {
int autogain;
autogain = v4lcontrol_get_ctrl(data->control, V4LCONTROL_AUTOGAIN);
if (!autogain) {
/* Reset last_correction val */
data->last_gain_correction = 0;
}
return autogain;
}
int v4lconvert_convert(struct v4lconvert_data *data,
const struct v4l2_format *src_fmt, /* in */
const struct v4l2_format *dest_fmt, /* in */
unsigned char *src, int src_size, unsigned char *dest, int dest_size)
{
int res, dest_needed, temp_needed, processing, convert = 0;
int rotate90, vflip, hflip, crop;
unsigned char *convert1_dest = dest;
int convert1_dest_size = dest_size;
unsigned char *convert2_src = src, *convert2_dest = dest;
int convert2_dest_size = dest_size;
unsigned char *rotate90_src = src, *rotate90_dest = dest;
unsigned char *flip_src = src, *flip_dest = dest;
unsigned char *crop_src = src;
struct v4l2_format my_src_fmt = *src_fmt;
struct v4l2_format my_dest_fmt = *dest_fmt;
processing = v4lprocessing_pre_processing(data->processing);
rotate90 = data->control_flags & V4LCONTROL_ROTATED_90_JPEG;
hflip = v4lcontrol_get_ctrl(data->control, V4LCONTROL_HFLIP);
vflip = v4lcontrol_get_ctrl(data->control, V4LCONTROL_VFLIP);
crop = my_dest_fmt.fmt.pix.width != my_src_fmt.fmt.pix.width ||
my_dest_fmt.fmt.pix.height != my_src_fmt.fmt.pix.height;
if (/* If no conversion/processing is needed */
(src_fmt->fmt.pix.pixelformat == dest_fmt->fmt.pix.pixelformat &&
!processing && !rotate90 && !hflip && !vflip && !crop) ||
/* or if we should do processing/rotating/flipping but the app tries to
use the native cam format, we just return an unprocessed frame copy */
!v4lconvert_supported_dst_format(dest_fmt->fmt.pix.pixelformat)) {
int to_copy = MIN(dest_size, src_size);
memcpy(dest, src, to_copy);
return to_copy;
}
/* When field is V4L2_FIELD_ALTERNATE, each buffer only contains half the
lines */
if (my_src_fmt.fmt.pix.field == V4L2_FIELD_ALTERNATE) {
my_src_fmt.fmt.pix.height /= 2;
my_dest_fmt.fmt.pix.height /= 2;
}
/* sanity check, is the dest buffer large enough? */
switch (my_dest_fmt.fmt.pix.pixelformat) {
case V4L2_PIX_FMT_RGB24:
case V4L2_PIX_FMT_BGR24:
dest_needed = my_dest_fmt.fmt.pix.width * my_dest_fmt.fmt.pix.height * 3;
temp_needed = my_src_fmt.fmt.pix.width * my_src_fmt.fmt.pix.height * 3;
break;
case V4L2_PIX_FMT_YUV420:
case V4L2_PIX_FMT_YVU420:
dest_needed =
my_dest_fmt.fmt.pix.width * my_dest_fmt.fmt.pix.height * 3 / 2;
temp_needed =
my_src_fmt.fmt.pix.width * my_src_fmt.fmt.pix.height * 3 / 2;
break;
default:
V4LCONVERT_ERR("Unknown dest format in conversion\n");
errno = EINVAL;
return -1;
}
if (dest_size < dest_needed) {
V4LCONVERT_ERR("destination buffer too small (%d < %d)\n",
dest_size, dest_needed);
errno = EFAULT;
return -1;
}
/* Sometimes we need foo -> rgb -> bar as video processing (whitebalance,
etc.) can only be done on rgb data */
if (processing && v4lconvert_processing_needs_double_conversion(
my_src_fmt.fmt.pix.pixelformat,
my_dest_fmt.fmt.pix.pixelformat))
convert = 2;
else if (my_dest_fmt.fmt.pix.pixelformat !=
my_src_fmt.fmt.pix.pixelformat ||
/* Special case if we do not need to do conversion, but we
are not doing any other step involving copying either,
force going through convert_pixfmt to copy the data from
source to dest */
(!rotate90 && !hflip && !vflip && !crop))
convert = 1;
/* convert_pixfmt (only if convert == 2) -> processing -> convert_pixfmt ->
rotate -> flip -> crop, all steps are optional */
if (convert == 2) {
convert1_dest = v4lconvert_alloc_buffer(
my_src_fmt.fmt.pix.width * my_src_fmt.fmt.pix.height * 3,
&data->convert1_buf, &data->convert1_buf_size);
if (!convert1_dest)
return v4lconvert_oom_error(data);
convert1_dest_size =
my_src_fmt.fmt.pix.width * my_src_fmt.fmt.pix.height * 3;
convert2_src = convert1_dest;
}
if (convert && (rotate90 || hflip || vflip || crop)) {
convert2_dest = v4lconvert_alloc_buffer(temp_needed,
&data->convert2_buf, &data->convert2_buf_size);
if (!convert2_dest)
return v4lconvert_oom_error(data);
convert2_dest_size = temp_needed;
rotate90_src = flip_src = crop_src = convert2_dest;
}
if (rotate90 && (hflip || vflip || crop)) {
rotate90_dest = v4lconvert_alloc_buffer(temp_needed,
&data->rotate90_buf, &data->rotate90_buf_size);
if (!rotate90_dest)
return v4lconvert_oom_error(data);
flip_src = crop_src = rotate90_dest;
}
if ((vflip || hflip) && crop) {
flip_dest = v4lconvert_alloc_buffer(temp_needed, &data->flip_buf,
&data->flip_buf_size);
if (!flip_dest)
return v4lconvert_oom_error(data);
crop_src = flip_dest;
}
/* Done setting sources / dest and allocating intermediate buffers,
real conversion / processing / ... starts here. */
if (convert == 2) {
res = v4lconvert_convert_pixfmt(data, src, src_size,
convert1_dest, convert1_dest_size,
&my_src_fmt,
V4L2_PIX_FMT_RGB24);
if (res)
return res;
src_size = my_src_fmt.fmt.pix.sizeimage;
}
if (processing)
v4lprocessing_processing(data->processing, convert2_src, &my_src_fmt);
if (convert) {
res = v4lconvert_convert_pixfmt(data, convert2_src, src_size,
convert2_dest, convert2_dest_size,
&my_src_fmt,
my_dest_fmt.fmt.pix.pixelformat);
if (res)
return res;
src_size = my_src_fmt.fmt.pix.sizeimage;
/* We call processing here again in case the source format was not
rgb, but the dest is. v4lprocessing checks it self it only actually
does the processing once per frame. */
if (processing)
v4lprocessing_processing(data->processing, convert2_dest, &my_src_fmt);
}
if (rotate90)
v4lconvert_rotate90(rotate90_src, rotate90_dest, &my_src_fmt);
if (hflip || vflip)
v4lconvert_flip(flip_src, flip_dest, &my_src_fmt, hflip, vflip);
if (crop)
v4lconvert_crop(crop_src, dest, &my_src_fmt, &my_dest_fmt);
return dest_needed;
}
/* auto gain and exposure algorithm based on the knee algorithm described here:
http://ytse.tricolour.net/docs/LowLightOptimization.html */
static int autogain_calculate_lookup_tables(
struct v4lprocessing_data *data,
unsigned char *buf, const struct v4l2_format *fmt)
{
int x, y, target, steps, avg_lum = 0;
int gain, exposure, orig_gain, orig_exposure, exposure_low;
struct v4l2_control ctrl;
struct v4l2_queryctrl gainctrl, expoctrl;
const int deadzone = 6;
ctrl.id = V4L2_CID_EXPOSURE;
expoctrl.id = V4L2_CID_EXPOSURE;
if (SYS_IOCTL(data->fd, VIDIOC_QUERYCTRL, &expoctrl) ||
SYS_IOCTL(data->fd, VIDIOC_G_CTRL, &ctrl))
return 0;
exposure = orig_exposure = ctrl.value;
/* Determine a value below which we try to not lower the exposure,
as most exposure controls tend to jump with big steps in the low
range, causing oscilation, so we prefer to use gain when exposure
has hit this value */
exposure_low = expoctrl.maximum / 10;
/* If we have a fine grained exposure control only avoid the last 10 steps */
if (exposure_low > 10)
exposure_low = 10;
exposure_low += expoctrl.minimum;
ctrl.id = V4L2_CID_GAIN;
gainctrl.id = V4L2_CID_GAIN;
if (SYS_IOCTL(data->fd, VIDIOC_QUERYCTRL, &gainctrl) ||
SYS_IOCTL(data->fd, VIDIOC_G_CTRL, &ctrl))
return 0;
gain = orig_gain = ctrl.value;
switch (fmt->fmt.pix.pixelformat) {
case V4L2_PIX_FMT_SGBRG8:
case V4L2_PIX_FMT_SGRBG8:
case V4L2_PIX_FMT_SBGGR8:
case V4L2_PIX_FMT_SRGGB8:
buf += fmt->fmt.pix.height * fmt->fmt.pix.bytesperline / 4 +
fmt->fmt.pix.width / 4;
for (y = 0; y < fmt->fmt.pix.height / 2; y++) {
for (x = 0; x < fmt->fmt.pix.width / 2; x++) {
avg_lum += *buf++;
}
buf += fmt->fmt.pix.bytesperline - fmt->fmt.pix.width / 2;
}
avg_lum /= fmt->fmt.pix.height * fmt->fmt.pix.width / 4;
break;
case V4L2_PIX_FMT_RGB24:
case V4L2_PIX_FMT_BGR24:
buf += fmt->fmt.pix.height * fmt->fmt.pix.bytesperline / 4 +
fmt->fmt.pix.width * 3 / 4;
for (y = 0; y < fmt->fmt.pix.height / 2; y++) {
for (x = 0; x < fmt->fmt.pix.width / 2; x++) {
avg_lum += *buf++;
avg_lum += *buf++;
avg_lum += *buf++;
}
buf += fmt->fmt.pix.bytesperline - fmt->fmt.pix.width * 3 / 2;
}
avg_lum /= fmt->fmt.pix.height * fmt->fmt.pix.width * 3 / 4;
break;
}
/* If we are off a multiple of deadzone, do multiple steps to reach the
desired lumination fast (with the risc of a slight overshoot) */
target = v4lcontrol_get_ctrl(data->control, V4LCONTROL_AUTOGAIN_TARGET);
steps = (target - avg_lum) / deadzone;
/* If we were decreasing and are now increasing, or vica versa, half the
number of steps to avoid overshooting and oscilating */
if ((steps > 0 && data->last_gain_correction < 0) ||
(steps < 0 && data->last_gain_correction > 0))
steps /= 2;
for (x = 0; x < abs(steps); x++) {
if (avg_lum > target) {
if (exposure > expoctrl.default_value)
exposure--;
else if (gain > gainctrl.default_value)
gain--;
else if (exposure > exposure_low)
exposure--;
else if (gain > gainctrl.minimum)
gain--;
else if (exposure > expoctrl.minimum)
exposure--;
else
break;
} else {
if (exposure < exposure_low)
exposure++;
else if (gain < gainctrl.default_value)
gain++;
else if (exposure < expoctrl.default_value)
exposure++;
else if (gain < gainctrl.maximum)
gain++;
else if (exposure < expoctrl.maximum)
exposure++;
else
break;
}
}
if (steps)
data->last_gain_correction = steps;
if (gain != orig_gain) {
ctrl.id = V4L2_CID_GAIN;
ctrl.value = gain;
SYS_IOCTL(data->fd, VIDIOC_S_CTRL, &ctrl);
}
if (exposure != orig_exposure) {
ctrl.id = V4L2_CID_EXPOSURE;
ctrl.value = exposure;
SYS_IOCTL(data->fd, VIDIOC_S_CTRL, &ctrl);
}
return 0;
}
/* auto gain and exposure algorithm based on the knee algorithm described here:
http://ytse.tricolour.net/docs/LowLightOptimization.html */
static int autogain_calculate_lookup_tables(
struct v4lprocessing_data *data,
unsigned char *buf, const struct v4l2_format *fmt)
{
int x, y, target, steps, avg_lum = 0;
int gain, exposure, orig_gain, orig_exposure, exposure_low;
struct v4l2_control ctrl;
struct v4l2_queryctrl gainctrl, expoctrl;
const int deadzone = 6;
ctrl.id = V4L2_CID_EXPOSURE;
expoctrl.id = V4L2_CID_EXPOSURE;
if (SYS_IOCTL(data->fd, VIDIOC_QUERYCTRL, &expoctrl) ||
SYS_IOCTL(data->fd, VIDIOC_G_CTRL, &ctrl))
return 0;
exposure = orig_exposure = ctrl.value;
/* Determine a value below which we try to not lower the exposure,
as most exposure controls tend to jump with big steps in the low
range, causing oscilation, so we prefer to use gain when exposure
has hit this value */
exposure_low = expoctrl.maximum / 10;
/* If we have a fine grained exposure control only avoid the last 10 steps */
steps = exposure_low / expoctrl.step;
if (steps > 10)
steps = 10;
exposure_low = steps * expoctrl.step + expoctrl.minimum;
ctrl.id = V4L2_CID_GAIN;
gainctrl.id = V4L2_CID_GAIN;
if (SYS_IOCTL(data->fd, VIDIOC_QUERYCTRL, &gainctrl) ||
SYS_IOCTL(data->fd, VIDIOC_G_CTRL, &ctrl))
return 0;
gain = orig_gain = ctrl.value;
switch (fmt->fmt.pix.pixelformat) {
case V4L2_PIX_FMT_SGBRG8:
case V4L2_PIX_FMT_SGRBG8:
case V4L2_PIX_FMT_SBGGR8:
case V4L2_PIX_FMT_SRGGB8:
buf += fmt->fmt.pix.height * fmt->fmt.pix.bytesperline / 4 +
fmt->fmt.pix.width / 4;
for (y = 0; y < fmt->fmt.pix.height / 2; y++) {
for (x = 0; x < fmt->fmt.pix.width / 2; x++) {
avg_lum += *buf++;
}
buf += fmt->fmt.pix.bytesperline - fmt->fmt.pix.width / 2;
}
avg_lum /= fmt->fmt.pix.height * fmt->fmt.pix.width / 4;
break;
case V4L2_PIX_FMT_RGB24:
case V4L2_PIX_FMT_BGR24:
buf += fmt->fmt.pix.height * fmt->fmt.pix.bytesperline / 4 +
fmt->fmt.pix.width * 3 / 4;
for (y = 0; y < fmt->fmt.pix.height / 2; y++) {
for (x = 0; x < fmt->fmt.pix.width / 2; x++) {
avg_lum += *buf++;
avg_lum += *buf++;
avg_lum += *buf++;
}
buf += fmt->fmt.pix.bytesperline - fmt->fmt.pix.width * 3 / 2;
}
avg_lum /= fmt->fmt.pix.height * fmt->fmt.pix.width * 3 / 4;
break;
}
/* If we are off a multiple of deadzone, do multiple steps to reach the
desired lumination fast (with the risc of a slight overshoot) */
target = v4lcontrol_get_ctrl(data->control, V4LCONTROL_AUTOGAIN_TARGET);
steps = (target - avg_lum) / deadzone;
/* If we were decreasing and are now increasing, or vica versa, half the
number of steps to avoid overshooting and oscilating */
if ((steps > 0 && data->last_gain_correction < 0) ||
(steps < 0 && data->last_gain_correction > 0))
steps /= 2;
if (steps == 0)
return 0; /* Nothing to do */
if (steps < 0) {
if (exposure > expoctrl.default_value)
autogain_adjust(&expoctrl, &exposure, steps, expoctrl.default_value);
else if (gain > gainctrl.default_value)
autogain_adjust(&gainctrl, &gain, steps, gainctrl.default_value);
else if (exposure > exposure_low)
autogain_adjust(&expoctrl, &exposure, steps, exposure_low);
else if (gain > gainctrl.minimum)
autogain_adjust(&gainctrl, &gain, steps, gainctrl.minimum);
else if (exposure > expoctrl.minimum)
autogain_adjust(&expoctrl, &exposure, steps, expoctrl.minimum);
else
steps = 0;
} else {
if (exposure < exposure_low)
autogain_adjust(&expoctrl, &exposure, steps, exposure_low);
else if (gain < gainctrl.default_value)
autogain_adjust(&gainctrl, &gain, steps, gainctrl.default_value);
else if (exposure < expoctrl.default_value)
autogain_adjust(&expoctrl, &exposure, steps, expoctrl.default_value);
else if (gain < gainctrl.maximum)
autogain_adjust(&gainctrl, &gain, steps, gainctrl.maximum);
else if (exposure < expoctrl.maximum)
autogain_adjust(&expoctrl, &exposure, steps, expoctrl.maximum);
else
steps = 0;
}
if (steps) {
data->last_gain_correction = steps;
/* We are still settling down, force the next update sooner. Note we
skip the next frame as that is still captured with the old settings,
and another one just to be sure (because if we re-adjust based
on the old settings we might overshoot). */
data->lookup_table_update_counter = V4L2PROCESSING_UPDATE_RATE - 2;
}
if (gain != orig_gain) {
ctrl.id = V4L2_CID_GAIN;
ctrl.value = gain;
SYS_IOCTL(data->fd, VIDIOC_S_CTRL, &ctrl);
}
if (exposure != orig_exposure) {
ctrl.id = V4L2_CID_EXPOSURE;
ctrl.value = exposure;
SYS_IOCTL(data->fd, VIDIOC_S_CTRL, &ctrl);
}
return 0;
}
static int whitebalance_active(struct v4lprocessing_data *data) {
return v4lcontrol_get_ctrl(data->control, V4LCONTROL_WHITEBALANCE);
}
