155 static uint32_t prev_time = 0;
156 float dt = (img->pprz_ts - prev_time) * 1e-6f;
158 prev_time = img->pprz_ts;
161 if (ae_set.prev_active != ae_set.active) {
162 mt9f002_reset_exposure(&mt9f002);
163 ae_set.prev_active = ae_set.active;
167 mt9f002_reset_color(&mt9f002);
168 awb_set.prev_active = awb_set.active;
175 static float prev_real_exposure = 0.f;
176 static uint8_t prev_exposure_stuck = 0;
179 static uint32_t cdf[MAX_HIST_Y];
180 cdf[MIN_HIST_Y - 1] = 0;
182 for (int i = MIN_HIST_Y; i < MAX_HIST_Y; i++) {
183 cdf[i] = cdf[i - 1] + yuv_stats.ae_histogram_Y[i];
192 if (bright_index < MIN_HIST_Y || bright_index >
MAX_HIST_Y) {
193 bright_index = 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]))
205 while (current_pixels < median_pixels && ae_current_ind <
MAX_HIST_Y) {
207 if (ae_current_ind <= dark_index) {
210 }
else if (ae_current_ind >= bright_index) {
219 float ae_current_level = (float)ae_current_ind;
222 ae_current_level -= (current_pixels - median_pixels) / (
float)yuv_stats.
ae_histogram_Y[ae_current_ind];
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);
239 Bound(
mt9f002.target_exposure, 1 / 128.f, 80.f);
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) {
248 mt9f002.target_exposure *= 1.125f;
250 mt9f002.target_exposure /= 1.125f;
253 prev_exposure_stuck = 0;
255 prev_real_exposure =
mt9f002.real_exposure;
258 VERBOSE_PRINT(
"AE lvl: target %d, actual %3.2f (%d pixels)\n",
ae_set.middle_index, ae_current_level,
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);
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;
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;
302 if (
mt9f002.target_exposure > (
awb_set.gain_scheduling_target +
awb_set.gain_scheduling_tolerance)) {
304 }
else if (
mt9f002.target_exposure < (
awb_set.gain_scheduling_target -
awb_set.gain_scheduling_tolerance)) {
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;