Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
bebop_ae_awb.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Freek van Tienen
3  *
4  * This file is part of paparazzi
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
26 #include "bebop_ae_awb.h"
27 #include "boards/bebop.h"
28 #include "boards/bebop/mt9f002.h"
31 
32 #define MAX_HIST_Y 255
33 #define MIN_HIST_Y 1
34 
35 #ifndef BEBOP_AE_AWB_VERBOSE
36 #define BEBOP_AE_AWB_VERBOSE 0
37 #endif
38 PRINT_CONFIG_VAR(BEBOP_AE_AWB_VERBOSE)
39 
40 #define PRINT(string,...) fprintf(stderr, "[bebop_ae_awb->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
41 
42 #if BEBOP_AE_AWB_VERBOSE
43 #include "stdio.h"
44 #define VERBOSE_PRINT PRINT
45 #else
46 #define VERBOSE_PRINT(...)
47 #endif
48 
49 // thread nice setting
50 #ifndef BEBOP_AE_AWB_NICE
51 #define BEBOP_AE_AWB_NICE 5
52 #endif
53 
54 #ifndef BEBOP_AUTO_EXPOSURE
55 #define BEBOP_AUTO_EXPOSURE true
56 #endif
57 
58 // Gain to apply to autoexposure change
59 #ifndef BEBOP_AE_EXPOSURE_GAIN
60 #define BEBOP_AE_EXPOSURE_GAIN 0.8
61 #endif
62 PRINT_CONFIG_VAR(BEBOP_AE_EXPOSURE_GAIN)
63 
64 // Bin index to be considered as the center of the brightness bins
65 #ifndef BEBOP_AE_MIDDLE_INDEX
66 #define BEBOP_AE_MIDDLE_INDEX 160
67 #endif
68 PRINT_CONFIG_VAR(BEBOP_AE_MIDDLE_INDEX)
69 
70 // Proportion of dark bins to ignore in the computation of the autoexposure
71 #ifndef BEBOP_AE_DARK_IGNORE
72 #define BEBOP_AE_DARK_IGNORE 0.4
73 #endif
74 PRINT_CONFIG_VAR(BEBOP_AE_DARK_IGNORE)
75 
76 // Proportion of bright bins to ignore in the computation of the autoexposure
77 #ifndef BEBOP_AE_BRIGHT_IGNORE
78 #define BEBOP_AE_BRIGHT_IGNORE 0.2
79 #endif
80 PRINT_CONFIG_VAR(BEBOP_AE_BRIGHT_IGNORE)
81 
82 // Number of dark bins to consider as dark in the autoexposure
83 #ifndef BEBOP_AE_DARK_BINS
84 #define BEBOP_AE_DARK_BINS 80
85 #endif
86 PRINT_CONFIG_VAR(BEBOP_AE_DARK_BINS)
87 
88 // Number of bright bins to consider as bright in autoexposure
89 #ifndef BEBOP_AE_BRIGHT_BINS
90 #define BEBOP_AE_BRIGHT_BINS 20
91 #endif
92 PRINT_CONFIG_VAR(BEBOP_AE_BRIGHT_BINS)
93 
94 #ifndef BEBOP_AUTO_WHITE_BALANCE
95 #define BEBOP_AUTO_WHITE_BALANCE true
96 #endif
97 
98 // Minimum gain for AWB
99 #ifndef BEBOP_AWB_MIN_GAINS
100 #define BEBOP_AWB_MIN_GAINS 1.0
101 #endif
102 PRINT_CONFIG_VAR(BEBOP_AE_MIN_GAINS)
103 
104 // Maximum gain for AWB
105 #ifndef BEBOP_AWB_MAX_GAINS
106 #define BEBOP_AWB_MAX_GAINS 63.5
107 #endif
108 PRINT_CONFIG_VAR(BEBOP_AWB_MAX_GAINS)
109 
110 //Gain to apply to AWB changes
111 #ifndef BEBOP_AWB_GAIN
112 #define BEBOP_AWB_GAIN 0.5
113 #endif
114 PRINT_CONFIG_VAR(BEBOP_AWB_GAIN)
115 
116 // Activates gain scheduling for the AWB, helps to handle fast changes in brightness
117 #ifndef BEBOP_AWB_GAIN_SCHEDULING
118 #define BEBOP_AWB_GAIN_SCHEDULING false
119 #endif
120 PRINT_CONFIG_VAR(BEBOP_AWB_GAIN_SCHEDULING)
121 
122 // Target white balance
123 #ifndef BEBOP_AWB_GAIN_SCHEDULING_TARGET
124 #define BEBOP_AWB_GAIN_SCHEDULING_TARGET 10.0
125 #endif
126 PRINT_CONFIG_VAR(BEBOP_AWB_GAIN_SCHEDULING_TARGET)
127 
128 // Tolerance around the target exposure outside of which gain scheduling is activated
129 #ifndef BEBOP_AWB_GAIN_SCHEDULING_TOLERANCE
130 #define BEBOP_AWB_GAIN_SCHEDULING_TOLERANCE 7.5
131 #endif
132 PRINT_CONFIG_VAR(BEBOP_AWB_GAIN_SCHEDULING_TOLERANCE)
133 
134 // Step size of the AWB gain scheduling correction
135 #ifndef BEBOP_AWB_GAIN_SCHEDULING_STEP
136 #define BEBOP_AWB_GAIN_SCHEDULING_STEP 0.02
137 #endif
138 PRINT_CONFIG_VAR(BEBOP_AWB_GAIN_SCHEDULING_STEP)
139 
140 // Minimum number of grey pixels in image, when lower, the AWB is reset
141 #ifndef BEBOP_AWB_MIN_GREY_PIXELS
142 #define BEBOP_AWB_MIN_GREY_PIXELS 1000
143 #endif
144 PRINT_CONFIG_VAR(BEBOP_AWB_MIN_GREY_PIXELS)
145 
146 struct ae_setting_t ae_set;
147 struct awb_setting_t awb_set;
148 
149 static struct image_t *update_ae_awb(struct image_t *img, uint8_t camera_id)
150 {
151  static struct isp_yuv_stats_t yuv_stats;
152 
153  if (isp_get_statistics_yuv(&yuv_stats) == 0 && yuv_stats.nb_valid_Y > 1) {
154  // get timing stats
155  static uint32_t prev_time = 0;
156  float dt = (img->pprz_ts - prev_time) * 1e-6f;
157  Bound(dt, 0.f, 1.f);
158  prev_time = img->pprz_ts;
159 
160  // handle any change in active setting
161  if (ae_set.prev_active != ae_set.active) {
164  }
165 
169  }
170 
171  /*
172  * Auto-exposure (Histogram median centering)
173  */
174  if (ae_set.active) {
175  static float prev_real_exposure = 0.f;
176  static uint8_t prev_exposure_stuck = 0;
177 
178  // Calculate the cummulative histogram based on the histogram
179  static uint32_t cdf[MAX_HIST_Y];
180  cdf[MIN_HIST_Y - 1] = 0;
181 
182  for (int i = MIN_HIST_Y; i < MAX_HIST_Y; i++) {
183  cdf[i] = cdf[i - 1] + yuv_stats.ae_histogram_Y[i];
184  }
185 
186  // Calculate the indices of the dark and bright bins
187  uint8_t dark_index = MIN_HIST_Y + ae_set.dark_bins;
188  if (dark_index > MAX_HIST_Y) {
189  dark_index = MAX_HIST_Y;
190  }
191  uint8_t bright_index = MAX_HIST_Y - ae_set.bright_bins;
192  if (bright_index < MIN_HIST_Y || bright_index > MAX_HIST_Y) {
193  bright_index = dark_index;
194  }
195 
196  // Calculate the median number of pixels ignoring the dark_ignore % and bright-ignore %
197  uint32_t median_pixels = (uint32_t) roundf(((1.f - ae_set.dark_ignore) * cdf[dark_index] +
198  cdf[bright_index - 1] - cdf[dark_index] +
199  (1.f - ae_set.bright_ignore) * (cdf[MAX_HIST_Y - 1] - cdf[bright_index - 1]))
200  / 2.0f);
201 
202  // Find the level that contains the median
203  uint32_t current_pixels = 0;
204  uint8_t ae_current_ind = MIN_HIST_Y - 1;
205  while (current_pixels < median_pixels && ae_current_ind < MAX_HIST_Y) {
206  ae_current_ind++;
207  if (ae_current_ind <= dark_index) {
208  // In dark bin - ignore dark_ignore %
209  current_pixels += (1 - ae_set.dark_ignore) * yuv_stats.ae_histogram_Y[ae_current_ind];
210  } else if (ae_current_ind >= bright_index) {
211  // In bright bin - ignore bright_ignore %
212  current_pixels += (1 - ae_set.bright_ignore) * yuv_stats.ae_histogram_Y[ae_current_ind];
213  } else {
214  // In centre bin - 100%
215  current_pixels += yuv_stats.ae_histogram_Y[ae_current_ind];
216  }
217  }
218 
219  float ae_current_level = (float)ae_current_ind;
220  if (yuv_stats.ae_histogram_Y[ae_current_ind] > 0) {
221  // Calculate decimal level
222  ae_current_level -= (current_pixels - median_pixels) / (float)yuv_stats.ae_histogram_Y[ae_current_ind];
223  }
224 
225  if (yuv_stats.awb_sum_Y > ae_current_level * yuv_stats.awb_nb_grey_pixels) {
226  // too many saturated pixels so median no longer good estimate
227  ae_current_level = (float)yuv_stats.awb_sum_Y / yuv_stats.awb_nb_grey_pixels;
228  }
229 
230  // that level is supposed to be 'middle_index'
231  float adjustment = 1.f;
232  if (ae_current_level > 1e-5) {
233  adjustment = 1.f + ae_set.exposure_gain * (ae_set.middle_index / ae_current_level - 1.f) * dt;
234  Bound(adjustment, 1 / 16.f, 16.f);
235  }
236 
237  // Calculate exposure based on adjustment
239  Bound(mt9f002.target_exposure, 1 / 128.f, 80.f);
240 
241  // Sometimes the exposure seems to freeze and it isn't updated. Try this hack to fix it
242  // 10 consecutive YUV statistics leading to the exact same real_exposure is highly unlikely
243  if (prev_real_exposure == mt9f002.real_exposure && prev_exposure_stuck < 10) {
244  prev_exposure_stuck++;
245  } else if (prev_real_exposure == mt9f002.real_exposure && prev_exposure_stuck >= 10) {
246  // Bump the target exposure in order to unfreeze the exposure
248  mt9f002.target_exposure *= 1.125f;
249  } else {
250  mt9f002.target_exposure /= 1.125f;
251  }
252  } else {
253  prev_exposure_stuck = 0;
254  }
255  prev_real_exposure = mt9f002.real_exposure;
256 
257  // Verbose prints
258  VERBOSE_PRINT("AE lvl: target %d, actual %3.2f (%d pixels)\n", ae_set.middle_index, ae_current_level,
259  yuv_stats.nb_valid_Y);
260  VERBOSE_PRINT("AE exp: target %5.2f ms, real %5.2f ms (%f)\n", mt9f002.target_exposure, mt9f002.real_exposure,
261  adjustment);
262 
264  }
265 
266  /*
267  * Auto white-balance (Robust Automatic White Balance Algorithm using Gray Color Points in Images - Huo et al.)
268  */
269  if (awb_set.active) {
270  if (yuv_stats.awb_nb_grey_pixels) {
271  VERBOSE_PRINT("avgU = %d / %d = %d\n", yuv_stats.awb_sum_U, yuv_stats.awb_nb_grey_pixels,
272  yuv_stats.awb_sum_U / yuv_stats.awb_nb_grey_pixels);
273  VERBOSE_PRINT("avgV = %d / %d = %d\n", yuv_stats.awb_sum_V, yuv_stats.awb_nb_grey_pixels,
274  yuv_stats.awb_sum_V / yuv_stats.awb_nb_grey_pixels);
275  }
276  // |B| = |Y| + |U| && |R| = |Y| + |V|
277  // ideal:
278  // |U| = |V| = 0 && |B| = |R| = |Y|
279  // so:
280  // gain_blue *= |Y| / (|Y| + |U|) --> ^0.25 in order to make less aggressive updates
281  // gain_red *= |Y| / (|Y| + |V|) --> ^0.25 in order to make less aggressive updates
282  float blue_adj = ((float)(yuv_stats.awb_sum_Y)) / ((float)(yuv_stats.awb_sum_Y + yuv_stats.awb_sum_U - 128.f *
283  yuv_stats.awb_nb_grey_pixels)) - 1.f;
284  mt9f002.gain_blue *= 1 + awb_set.gain * blue_adj * dt;
287  }
288 
289  float red_adj = ((float)(yuv_stats.awb_sum_Y)) / ((float)(yuv_stats.awb_sum_Y + yuv_stats.awb_sum_V - 128.f *
290  yuv_stats.awb_nb_grey_pixels)) - 1.f;
291  mt9f002.gain_red *= 1 + awb_set.gain * red_adj * dt;
294  }
295 
296  /*
297  * Gain scheduling
298  */
299  if (awb_set.gain_scheduling) {
300  int8_t gs_adjustDir = 0;
301  // Let's try to center the exposure, that way we'll be able to account for fast changes in brightness
303  gs_adjustDir = +1;
305  gs_adjustDir = -1;
306  }
307  mt9f002.gain_blue *= 1 + gs_adjustDir * awb_set.gain_scheduling_step;
308  mt9f002.gain_red *= 1 + gs_adjustDir * awb_set.gain_scheduling_step;
309  mt9f002.gain_green1 *= 1 + gs_adjustDir * awb_set.gain_scheduling_step;
310  mt9f002.gain_green2 *= 1 + gs_adjustDir * awb_set.gain_scheduling_step;
311  }
312 
313  // Bound gains
318 
319  // check that gains have not exploded
324  } else {
325  VERBOSE_PRINT("blue %f, red %f, green 1 %f green 2 %f\n", mt9f002.gain_blue, mt9f002.gain_red, mt9f002.gain_green1,
327  // Set gains
329  }
330  }
331  }
332 
333  return NULL;
334 }
335 
337 {
346 
354 
356 }
#define VERBOSE_PRINT(...)
Definition: bebop_ae_awb.c:46
#define BEBOP_AWB_GAIN
Definition: bebop_ae_awb.c:112
#define BEBOP_AE_BRIGHT_IGNORE
Definition: bebop_ae_awb.c:78
#define BEBOP_AUTO_EXPOSURE
Definition: bebop_ae_awb.c:55
#define BEBOP_AE_AWB_NICE
Definition: bebop_ae_awb.c:51
#define BEBOP_AE_DARK_IGNORE
Definition: bebop_ae_awb.c:72
#define BEBOP_AWB_MIN_GAINS
Definition: bebop_ae_awb.c:100
#define BEBOP_AWB_MIN_GREY_PIXELS
Definition: bebop_ae_awb.c:142
void bebop_ae_awb_init(void)
Definition: bebop_ae_awb.c:336
struct ae_setting_t ae_set
Definition: bebop_ae_awb.c:146
#define BEBOP_AUTO_WHITE_BALANCE
Definition: bebop_ae_awb.c:95
#define BEBOP_AE_AWB_VERBOSE
Definition: bebop_ae_awb.c:36
#define BEBOP_AWB_GAIN_SCHEDULING_TARGET
Definition: bebop_ae_awb.c:124
#define BEBOP_AE_DARK_BINS
Definition: bebop_ae_awb.c:84
static struct image_t * update_ae_awb(struct image_t *img, uint8_t camera_id)
Definition: bebop_ae_awb.c:149
struct awb_setting_t awb_set
Definition: bebop_ae_awb.c:147
#define MAX_HIST_Y
Definition: bebop_ae_awb.c:32
#define BEBOP_AWB_GAIN_SCHEDULING_STEP
Definition: bebop_ae_awb.c:136
#define BEBOP_AWB_GAIN_SCHEDULING_TOLERANCE
Definition: bebop_ae_awb.c:130
#define BEBOP_AE_MIDDLE_INDEX
Definition: bebop_ae_awb.c:66
#define BEBOP_AWB_MAX_GAINS
Definition: bebop_ae_awb.c:106
#define BEBOP_AWB_GAIN_SCHEDULING
Definition: bebop_ae_awb.c:118
#define BEBOP_AE_BRIGHT_BINS
Definition: bebop_ae_awb.c:90
#define MIN_HIST_Y
Definition: bebop_ae_awb.c:33
#define BEBOP_AE_EXPOSURE_GAIN
Definition: bebop_ae_awb.c:60
bool gain_scheduling
Definition: bebop_ae_awb.h:47
float gain_scheduling_tolerance
Definition: bebop_ae_awb.h:49
uint8_t dark_bins
Definition: bebop_ae_awb.h:38
uint8_t middle_index
Definition: bebop_ae_awb.h:37
bool prev_active
Definition: bebop_ae_awb.h:33
float gain_scheduling_target
Definition: bebop_ae_awb.h:48
float exposure_gain
Definition: bebop_ae_awb.h:34
float dark_ignore
Definition: bebop_ae_awb.h:36
float bright_ignore
Definition: bebop_ae_awb.h:35
float gain_scheduling_step
Definition: bebop_ae_awb.h:50
uint8_t bright_bins
Definition: bebop_ae_awb.h:39
struct video_listener * cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level, uint16_t fps, uint8_t id)
Definition: cv.c:83
Computer vision framework for onboard processing.
uint32_t pprz_ts
The timestamp in us since system startup.
Definition: image.h:50
Definition: image.h:44
int isp_get_statistics_yuv(struct isp_yuv_stats_t *yuv_stats)
Definition: libisp.c:261
uint32_t awb_nb_grey_pixels
Definition: libisp.h:95
uint32_t ae_histogram_Y[256]
Definition: libisp.h:97
uint32_t awb_sum_Y
Definition: libisp.h:92
uint32_t awb_sum_V
Definition: libisp.h:94
uint32_t nb_valid_Y
Definition: libisp.h:96
uint32_t awb_sum_U
Definition: libisp.h:93
struct mt9f002_t mt9f002
Definition: mt9f002.c:123
void mt9f002_reset_exposure(struct mt9f002_t *mt)
Definition: mt9f002.c:1108
void mt9f002_set_gains(struct mt9f002_t *mt)
Sets the GreenR, Blue, Red and GreenB gains.
Definition: mt9f002.c:912
void mt9f002_set_exposure(struct mt9f002_t *mt)
Set the exposure configuration Depends on the blanking (and therefore the FPS)
Definition: mt9f002.c:799
void mt9f002_reset_color(struct mt9f002_t *mt)
Definition: mt9f002.c:1099
Initialization and configuration of the MT9F002 CMOS Chip.
float target_exposure
Target exposure time in ms.
Definition: mt9f002.h:152
float gain_red
Gain for the Red pixels [1., 63.50].
Definition: mt9f002.h:157
float gain_green2
Gain for the GreenB pixels [1., 63.50].
Definition: mt9f002.h:158
float gain_green1
Gain for the GreenR pixels [1., 63.50].
Definition: mt9f002.h:155
float gain_blue
Gain for the Blue pixels [1., 63.50].
Definition: mt9f002.h:156
float real_exposure
Real exposure time in ms.
Definition: mt9f002.h:153
#define front_camera
Definition: mt9f002_nps.c:3
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103