libv4l2: better auto-gain

index c6866d6..a2c69f4 100644
@@ -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
 };
+