@@ -21,6 +21,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
+#include <math.h>
#include "libv4lprocessing.h"
#include "libv4lprocessing-priv.h"
@@ -40,179 +41,136 @@ static int autogain_active(struct v4lprocessing_data *data)
return autogain;
}
-/* Adjust ctrl value with steps steps, while not crossing limit */
-static void autogain_adjust(struct v4l2_queryctrl *ctrl, int *value,
- int steps, int limit, int accel)
+#define BUCKETS 20
+
+static void v4l2_histogram_bayer10(unsigned short *buf, int cdf[], const struct v4l2_format *fmt)
{
- int ctrl_range = (ctrl->maximum - ctrl->minimum) / ctrl->step;
-
- /* If we are of 3 * deadzone or more, and we have a fine grained
- control, take larger steps, otherwise we take ages to get to the
- right setting point. We use 256 as tripping point for determining
- fine grained controls here, as avg_lum has a range of 0 - 255. */
- if (accel && abs(steps) >= 3 && ctrl_range > 256)
- *value += steps * ctrl->step * (ctrl_range / 256);
- /* If we are of by less then 3, but have a very finegrained control
- still speed things up a bit */
- else if (accel && ctrl_range > 1024)
- *value += steps * ctrl->step * (ctrl_range / 1024);
- else
- *value += steps * ctrl->step;
-
- if (steps > 0) {
- if (*value > limit)
- *value = limit;
- } else {
- if (*value < limit)
- *value = limit;
- }
+ for (int y = 0; y < fmt->fmt.pix.height; y+=19)
+ for (int x = 0; x < fmt->fmt.pix.width; x+=19) {
+ int b;
+ b = buf[fmt->fmt.pix.width*y + x];
+ b = (b * BUCKETS)/(1024);
+ cdf[b]++;
+ }
}
-/* 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)
+static int v4l2_s_ctrl(int fd, long id, long value)
{
- int x, y, target, steps, avg_lum = 0;
- int gain, exposure, orig_gain, orig_exposure, exposure_low;
+ int res;
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 - expoctrl.minimum) / 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;
- }
+ ctrl.id = id;
+ ctrl.value = value;
+ /* FIXME: we'd like to do v4l2_ioctl here, but headers
+ prevent that */
+ res = SYS_IOCTL(fd, VIDIOC_S_CTRL, &ctrl);
+ if (res < 0)
+ printf("Set control %lx %ld failed\n", id, value);
+ return res;
+}
- /* 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, 1);
- else if (gain > gainctrl.default_value)
- autogain_adjust(&gainctrl, &gain, steps,
- gainctrl.default_value, 1);
- else if (exposure > exposure_low)
- autogain_adjust(&expoctrl, &exposure, steps,
- exposure_low, 1);
- else if (gain > gainctrl.minimum)
- autogain_adjust(&gainctrl, &gain, steps,
- gainctrl.minimum, 1);
- else if (exposure > expoctrl.minimum)
- autogain_adjust(&expoctrl, &exposure, steps,
- expoctrl.minimum, 0);
- else
- steps = 0;
- } else {
- if (exposure < exposure_low)
- autogain_adjust(&expoctrl, &exposure, steps,
- exposure_low, 0);
- else if (gain < gainctrl.default_value)
- autogain_adjust(&gainctrl, &gain, steps,
- gainctrl.default_value, 1);
- else if (exposure < expoctrl.default_value)
- autogain_adjust(&expoctrl, &exposure, steps,
- expoctrl.default_value, 1);
- else if (gain < gainctrl.maximum)
- autogain_adjust(&gainctrl, &gain, steps,
- gainctrl.maximum, 1);
- else if (exposure < expoctrl.maximum)
- autogain_adjust(&expoctrl, &exposure, steps,
- expoctrl.maximum, 1);
- else
- steps = 0;
+static int v4l2_set_exposure(struct v4lprocessing_data *data, double exposure)
+{
+ double exp, gain; /* microseconds */
+ int exp_, gain_;
+ int fd = data->fd;
+
+ gain = 1;
+ exp = exposure / gain;
+ if (exp > 10000) {
+ exp = 10000;
+ gain = exposure / exp;
}
-
- 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 > 16) {
+ gain = 16;
+ exp = exposure / gain;
}
- if (gain != orig_gain) {
- ctrl.id = V4L2_CID_GAIN;
- ctrl.value = gain;
- SYS_IOCTL(data->fd, VIDIOC_S_CTRL, &ctrl);
+ exp_ = exp;
+ gain_ = 10*log(gain)/log(2);
+ printf("Exposure %f %d, gain %f %d\n", exp, exp_, gain, gain_);
+
+ /* gain | ISO | gain_
+ * 1. | 100 | 0
+ * 2. | 200 | 10
+ * ...
+ * 16. | 1600 | 40
+ */
+
+ if (v4l2_s_ctrl(fd, V4L2_CID_GAIN, gain_) < 0) {
+ printf("Could not set gain\n");
}
- if (exposure != orig_exposure) {
- ctrl.id = V4L2_CID_EXPOSURE;
- ctrl.value = exposure;
- SYS_IOCTL(data->fd, VIDIOC_S_CTRL, &ctrl);
+ if (v4l2_s_ctrl(fd, V4L2_CID_EXPOSURE_ABSOLUTE, exp_) < 0) {
+ printf("Could not set exposure\n");
}
+ return 0;
+}
+
+struct exposure_data {
+ double exposure;
+};
+static int autogain_calculate_lookup_tables_exp(
+ struct v4lprocessing_data *data,
+ unsigned char *buf, const struct v4l2_format *fmt)
+{
+ int cdf[BUCKETS] = { 0, };
+ static struct exposure_data e_data;
+ static struct exposure_data *exp = &e_data;
+
+ v4l2_histogram_bayer10((void *) buf, cdf, fmt);
+
+ for (int i=1; i<BUCKETS; i++)
+ cdf[i] += cdf[i-1];
+
+ int b = BUCKETS;
+ int brightPixels = cdf[b-1] - cdf[b-8];
+ int targetBrightPixels = cdf[b-1]/50;
+ int maxSaturatedPixels = cdf[b-1]/200;
+ int saturatedPixels = cdf[b-1] - cdf[b-2];
+ /* how much should I change brightness by */
+ float adjustment = 1.0f;
+
+ if (saturatedPixels > maxSaturatedPixels) {
+ /* first don't let things saturate too much */
+ adjustment = 1.0f - ((float)(saturatedPixels - maxSaturatedPixels))/cdf[b-1];
+ } else if (brightPixels < (targetBrightPixels - (saturatedPixels * 4))) {
+ /* increase brightness to try and hit the desired
+ number of well exposed pixels
+ */
+ int l = b-6;
+ while (brightPixels < targetBrightPixels && l > 0) {
+ brightPixels += cdf[l];
+ brightPixels -= cdf[l-1];
+ l--;
+ }
+
+ adjustment = ((float) (b-6+1))/(l+1);
+ }
+ /* else we're not oversaturated, and we have enough bright pixels.
+ Do nothing.
+ */
+
+ float limit = 4;
+ if (adjustment > limit) { adjustment = limit; }
+ if (adjustment < 1/limit) { adjustment = 1/limit; }
+
+ exp->exposure *= adjustment;
+ if (exp->exposure < 1)
+ exp->exposure = 1;
+
+ float elimit = 64000000;
+ if (exp->exposure > elimit)
+ exp->exposure = elimit;
+
+ if (adjustment != 1.)
+ printf("AutoExposure: adjustment: %f exposure %f\n",
+ adjustment, exp->exposure);
+
+ v4l2_set_exposure(data, exp->exposure);
return 0;
}
struct v4lprocessing_filter autogain_filter = {
- autogain_active, autogain_calculate_lookup_tables
+ autogain_active, autogain_calculate_lookup_tables_exp
};
+