1 /*
2 # (C) 2008-2009 Hans de Goede <hdegoede@redhat.com>
3
4 # This program is free software; you can redistribute it and/or modify
5 # it under the terms of the GNU Lesser General Public License as published by
6 # the Free Software Foundation; either version 2.1 of the License, or
7 # (at your option) any later version.
8 #
9 # This program is distributed in the hope that it will be useful,
10 # but WITHOUT ANY WARRANTY; without even the implied warranty of
11 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 # GNU Lesser General Public License for more details.
13 #
14 # You should have received a copy of the GNU Lesser General Public License
15 # along with this program; if not, write to the Free Software
16 # Foundation, Inc., 51 Franklin Street, Suite 500, Boston, MA 02110-1335 USA
17 */
18
19 #include <errno.h>
20 #include <string.h>
21 #include <stdio.h>
22 #include <stdlib.h>
23 #include <unistd.h>
24
25 #include "libv4lprocessing.h"
26 #include "libv4lprocessing-priv.h"
27 #include "../libv4lconvert-priv.h" /* for PIX_FMT defines */
28 #include "../libv4lsyscall-priv.h"
29
autogain_active(struct v4lprocessing_data * data)30 static int autogain_active(struct v4lprocessing_data *data)
31 {
32 int autogain;
33
34 autogain = v4lcontrol_get_ctrl(data->control, V4LCONTROL_AUTOGAIN);
35 if (!autogain) {
36 /* Reset last_correction val */
37 data->last_gain_correction = 0;
38 }
39
40 return autogain;
41 }
42
43 /* Adjust ctrl value with steps steps, while not crossing limit */
autogain_adjust(struct v4l2_queryctrl * ctrl,int * value,int steps,int limit,int accel)44 static void autogain_adjust(struct v4l2_queryctrl *ctrl, int *value,
45 int steps, int limit, int accel)
46 {
47 int ctrl_range = (ctrl->maximum - ctrl->minimum) / ctrl->step;
48
49 /* If we are of 3 * deadzone or more, and we have a fine grained
50 control, take larger steps, otherwise we take ages to get to the
51 right setting point. We use 256 as tripping point for determining
52 fine grained controls here, as avg_lum has a range of 0 - 255. */
53 if (accel && abs(steps) >= 3 && ctrl_range > 256)
54 *value += steps * ctrl->step * (ctrl_range / 256);
55 /* If we are of by less than 3, but have a very finegrained control
56 still speed things up a bit */
57 else if (accel && ctrl_range > 1024)
58 *value += steps * ctrl->step * (ctrl_range / 1024);
59 else
60 *value += steps * ctrl->step;
61
62 if (steps > 0) {
63 if (*value > limit)
64 *value = limit;
65 } else {
66 if (*value < limit)
67 *value = limit;
68 }
69 }
70
71 /* auto gain and exposure algorithm based on the knee algorithm described here:
72 http://ytse.tricolour.net/docs/LowLightOptimization.html */
autogain_calculate_lookup_tables(struct v4lprocessing_data * data,unsigned char * buf,const struct v4l2_format * fmt)73 static int autogain_calculate_lookup_tables(
74 struct v4lprocessing_data *data,
75 unsigned char *buf, const struct v4l2_format *fmt)
76 {
77 int x, y, target, steps, avg_lum = 0;
78 int gain, exposure, orig_gain, orig_exposure, exposure_low;
79 struct v4l2_control ctrl;
80 struct v4l2_queryctrl gainctrl, expoctrl;
81 const int deadzone = 6;
82
83 ctrl.id = V4L2_CID_EXPOSURE;
84 expoctrl.id = V4L2_CID_EXPOSURE;
85 if (SYS_IOCTL(data->fd, VIDIOC_QUERYCTRL, &expoctrl) ||
86 SYS_IOCTL(data->fd, VIDIOC_G_CTRL, &ctrl))
87 return 0;
88
89 exposure = orig_exposure = ctrl.value;
90 /* Determine a value below which we try to not lower the exposure,
91 as most exposure controls tend to jump with big steps in the low
92 range, causing oscilation, so we prefer to use gain when exposure
93 has hit this value */
94 exposure_low = (expoctrl.maximum - expoctrl.minimum) / 10;
95 /* If we have a fine grained exposure control only avoid the last 10 steps */
96 steps = exposure_low / expoctrl.step;
97 if (steps > 10)
98 steps = 10;
99 exposure_low = steps * expoctrl.step + expoctrl.minimum;
100
101 ctrl.id = V4L2_CID_GAIN;
102 gainctrl.id = V4L2_CID_GAIN;
103 if (SYS_IOCTL(data->fd, VIDIOC_QUERYCTRL, &gainctrl) ||
104 SYS_IOCTL(data->fd, VIDIOC_G_CTRL, &ctrl))
105 return 0;
106 gain = orig_gain = ctrl.value;
107
108 switch (fmt->fmt.pix.pixelformat) {
109 case V4L2_PIX_FMT_SGBRG8:
110 case V4L2_PIX_FMT_SGRBG8:
111 case V4L2_PIX_FMT_SBGGR8:
112 case V4L2_PIX_FMT_SRGGB8:
113 buf += fmt->fmt.pix.height * fmt->fmt.pix.bytesperline / 4 +
114 fmt->fmt.pix.width / 4;
115
116 for (y = 0; y < fmt->fmt.pix.height / 2; y++) {
117 for (x = 0; x < fmt->fmt.pix.width / 2; x++)
118 avg_lum += *buf++;
119 buf += fmt->fmt.pix.bytesperline - fmt->fmt.pix.width / 2;
120 }
121 avg_lum /= fmt->fmt.pix.height * fmt->fmt.pix.width / 4;
122 break;
123
124 case V4L2_PIX_FMT_RGB24:
125 case V4L2_PIX_FMT_BGR24:
126 buf += fmt->fmt.pix.height * fmt->fmt.pix.bytesperline / 4 +
127 fmt->fmt.pix.width * 3 / 4;
128
129 for (y = 0; y < fmt->fmt.pix.height / 2; y++) {
130 for (x = 0; x < fmt->fmt.pix.width / 2; x++) {
131 avg_lum += *buf++;
132 avg_lum += *buf++;
133 avg_lum += *buf++;
134 }
135 buf += fmt->fmt.pix.bytesperline - fmt->fmt.pix.width * 3 / 2;
136 }
137 avg_lum /= fmt->fmt.pix.height * fmt->fmt.pix.width * 3 / 4;
138 break;
139 }
140
141 /* If we are off a multiple of deadzone, do multiple steps to reach the
142 desired lumination fast (with the risc of a slight overshoot) */
143 target = v4lcontrol_get_ctrl(data->control, V4LCONTROL_AUTOGAIN_TARGET);
144 steps = (target - avg_lum) / deadzone;
145
146 /* If we were decreasing and are now increasing, or vica versa, half the
147 number of steps to avoid overshooting and oscilating */
148 if ((steps > 0 && data->last_gain_correction < 0) ||
149 (steps < 0 && data->last_gain_correction > 0))
150 steps /= 2;
151
152 if (steps == 0)
153 return 0; /* Nothing to do */
154
155 if (steps < 0) {
156 if (exposure > expoctrl.default_value)
157 autogain_adjust(&expoctrl, &exposure, steps,
158 expoctrl.default_value, 1);
159 else if (gain > gainctrl.default_value)
160 autogain_adjust(&gainctrl, &gain, steps,
161 gainctrl.default_value, 1);
162 else if (exposure > exposure_low)
163 autogain_adjust(&expoctrl, &exposure, steps,
164 exposure_low, 1);
165 else if (gain > gainctrl.minimum)
166 autogain_adjust(&gainctrl, &gain, steps,
167 gainctrl.minimum, 1);
168 else if (exposure > expoctrl.minimum)
169 autogain_adjust(&expoctrl, &exposure, steps,
170 expoctrl.minimum, 0);
171 else
172 steps = 0;
173 } else {
174 if (exposure < exposure_low)
175 autogain_adjust(&expoctrl, &exposure, steps,
176 exposure_low, 0);
177 else if (gain < gainctrl.default_value)
178 autogain_adjust(&gainctrl, &gain, steps,
179 gainctrl.default_value, 1);
180 else if (exposure < expoctrl.default_value)
181 autogain_adjust(&expoctrl, &exposure, steps,
182 expoctrl.default_value, 1);
183 else if (gain < gainctrl.maximum)
184 autogain_adjust(&gainctrl, &gain, steps,
185 gainctrl.maximum, 1);
186 else if (exposure < expoctrl.maximum)
187 autogain_adjust(&expoctrl, &exposure, steps,
188 expoctrl.maximum, 1);
189 else
190 steps = 0;
191 }
192
193 if (steps) {
194 data->last_gain_correction = steps;
195 /* We are still settling down, force the next update sooner. Note we
196 skip the next frame as that is still captured with the old settings,
197 and another one just to be sure (because if we re-adjust based
198 on the old settings we might overshoot). */
199 data->lookup_table_update_counter = V4L2PROCESSING_UPDATE_RATE - 2;
200 }
201
202 if (gain != orig_gain) {
203 ctrl.id = V4L2_CID_GAIN;
204 ctrl.value = gain;
205 SYS_IOCTL(data->fd, VIDIOC_S_CTRL, &ctrl);
206 }
207 if (exposure != orig_exposure) {
208 ctrl.id = V4L2_CID_EXPOSURE;
209 ctrl.value = exposure;
210 SYS_IOCTL(data->fd, VIDIOC_S_CTRL, &ctrl);
211 }
212
213 return 0;
214 }
215
216 const struct v4lprocessing_filter autogain_filter = {
217 autogain_active, autogain_calculate_lookup_tables
218 };
219