elphel: apps/autoexposure/white_balance.c Source File

00001 /*!***************************************************************************
00002 *! FILE NAME  : white_balance.c
00003 *! DESCRIPTION: White balance part of the autoexposure/white balance/hdr daemon
00004 *! Copyright (C) 2008 Elphel, Inc.
00005 *! -----------------------------------------------------------------------------**
00006 *!  This program is free software: you can redistribute it and/or modify
00007 *!  it under the terms of the GNU General Public License as published by
00008 *!  the Free Software Foundation, either version 3 of the License, or
00009 *!  (at your option) any later version.
00010 *!
00011 *!  This program is distributed in the hope that it will be useful,
00012 *!  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 *!  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 *!  GNU General Public License for more details.
00015 *!
00016 *!  You should have received a copy of the GNU General Public License
00017 *!  along with this program.  If not, see <http://www.gnu.org/licenses/>.
00018 *! -----------------------------------------------------------------------------**
00019 *!
00020 *!  $Log: white_balance.c,v $
00021 *!  Revision 1.1.1.1  2008/11/27 20:04:01  elphel
00022 *!
00023 *!
00024 *!  Revision 1.5  2008/11/18 19:30:16  elphel
00025 *!  Added initialization of the "next" frame - otherwise it wait _very_ long if the camera frame number is reset
00026 *!
00027 *!  Revision 1.4  2008/11/18 07:36:55  elphel
00028 *!  added TODO
00029 *!
00030 *!  Revision 1.3  2008/11/17 06:49:48  elphel
00031 *!  synchronized greens analog gains when color HDR is not used
00032 *!
00033 *!  Revision 1.2  2008/11/16 17:35:06  elphel
00034 *!  improved control of analog gains
00035 *!
00036 *!  Revision 1.1  2008/11/15 23:08:24  elphel
00037 *!  8.0.alpha17 - split autoexposure source file
00038 *!
00039 *!
00040 */ 
00041 
00042 #include "autoexposure.h"
00043 void initWhiteBalanceCorr(void) {
00044   GLOBALPARS(G_NEXT_WB_FRAME)=0;
00045 }
00046 
00055 
00056 
00057 
00058 int whiteBalanceCorr(int frame, int target_frame, int ae_rslt) {
00059    int rslt;
00060    int colors;
00061    unsigned long write_data[18];
00062    unsigned long hash32[4];
00066    int color;
00067    int wb_whitelev,wb_whitefrac,wb_whitepix, wb_whiteindex;
00068    int brightest_color=-1; 
00069    int max_white_pixels;   
00070    int num_pixels,white_pixels,nonwhite_frac,nonwhite_pixels;
00071    int perc,delta,perc_frac;
00072    unsigned long * hist_cumul;      
00073    unsigned char * hist_percentile; 
00074    int levels[4];      
00075    int corr[4];        
00076    int target_frame8=target_frame & PARS_FRAMES_MASK;
00077    unsigned long min_scale; 
00078    int i;
00079    int wb_period_change=   framePars[target_frame8].pars[P_WB_PERIOD] & 0xff; 
00080    int wb_period_nochange=(framePars[target_frame8].pars[P_WB_PERIOD] >> 8 ) & 0xff; 
00081    int wb_dont_sync=      (framePars[target_frame8].pars[P_WB_PERIOD] & 0x10000); 
00082 
00083    if (!wb_period_change)   wb_period_change=  DEFAULT_WB_PERIOD_CHANGE;
00084    if (!wb_period_nochange) wb_period_nochange=DEFAULT_WB_PERIOD_NOCHANGE;
00085 
00086    if (!((colors=framePars[target_frame8].pars[P_WB_MASK]))) {
00087      GLOBALPARS(G_NEXT_WB_FRAME)=frame+wb_period_change;
00088      return 0; 
00089    }
00090    if (GLOBALPARS(G_NEXT_WB_FRAME) > frame) return 0; 
00091    MDF3(fprintf(stderr,"G_WB_INTEGERR=0x%08lx\n",GLOBALPARS(G_WB_INTEGERR))); 
00092    colors |= (1 << COLOR_Y_NUMBER);
00094    lseek(fd_histogram_cache, LSEEK_HIST_WAIT_C, SEEK_END); 
00095    lseek(fd_histogram_cache, LSEEK_HIST_NEEDED + (colors << 8), SEEK_END);      
00096    hist_index=lseek(fd_histogram_cache, frame-1, SEEK_SET);                     
00097    if(hist_index <0) {
00098      ELP_FERR(fprintf(stderr, "Requested histograms for frame %d (0x%x) are not available\n",frame-1,frame-1));
00099      return -1;
00100    }
00101 
00102 
00103    if (histogram_cache[hist_index].frame < (frame-1)) { 
00104      GLOBALPARS(G_NEXT_WB_FRAME)=frame+1;
00105      if (wb_dont_sync) return 0; 
00106 
00107      for (i=0; i<8; i++) {
00108        frame++;
00109        MDF3(fprintf(stderr,"Skipping frame trying to synchronize, frame will be 0x%x\n",frame));
00110        lseek(fd_fparmsall, frame +LSEEK_FRAME_WAIT_ABS, SEEK_END);
00111        this_frame=frame;
00112        hist_index=lseek(fd_histogram_cache, frame-1, SEEK_SET);                     
00113        if (histogram_cache[hist_index].frame == (frame-1)) break;
00114      }
00115      if (histogram_cache[hist_index].frame < (frame-1)) { 
00116        GLOBALPARS(G_NEXT_WB_FRAME)=frame+1;
00117        return 0; 
00118      }
00119    }
00120 
00121    wb_whitelev=framePars[target_frame8].pars[P_WB_WHITELEV];
00122    wb_whitefrac=framePars[target_frame8].pars[P_WB_WHITEFRAC];
00123    if (!wb_whitelev  || (wb_whitelev  > 0xffff))  wb_whitelev =DEFAULT_WB_WHITELEV;  
00124    if (!wb_whitefrac || (wb_whitefrac > 0xffff)) wb_whitefrac=DEFAULT_WB_WHITEFRAC; 
00125    MDF4(fprintf(stderr,"colors=0x%x wb_whitelev=0x%x, wb_whitefrac=0x%x\n",colors,wb_whitelev,wb_whitefrac));
00126    corr[0]=framePars[target_frame8].pars[P_WB_SCALE_R];
00127    corr[1]=0x10000;
00128    corr[2]=framePars[target_frame8].pars[P_WB_SCALE_GB];
00129    corr[3]=framePars[target_frame8].pars[P_WB_SCALE_B];
00130    int same_greens= (corr[COLOR_GREEN2]==corr[COLOR_GREEN1]) ||
00131                     ((corr[COLOR_GREEN2]>(corr[COLOR_GREEN1]-0x800)) && (corr[COLOR_GREEN2]<(corr[COLOR_GREEN1]+0x800))) ; 
00132 
00133 
00134    for (color=0;color<4;color++) if (!corr[color]) corr[color]=0x10000;
00135    MDF4(fprintf(stderr,"corr[]= 0x%04x 0x%04x 0x%04x 0x%04x\n",corr[0],corr[1],corr[2],corr[3]));
00137    max_white_pixels=0;
00138    num_pixels=histogram_cache[hist_index].cumul_hist[255+ (COLOR_Y_NUMBER<<8)]; 
00139    wb_whitepix= (((long long) num_pixels) * wb_whitefrac)>>16;
00140    wb_whiteindex=(wb_whitelev>>8) & 0xff;
00141    MDF4(fprintf(stderr,"num_pixels=0x%04x wb_whitepix=0x%04x wb_whiteindex=0x%04x\n",num_pixels,wb_whitepix,wb_whiteindex));
00143    for (color=0;color<4;color++) if (colors & (1 << color)) {
00144      hist_cumul=     &(histogram_cache[hist_index].cumul_hist[(color << 8)]);
00145      if ( ((white_pixels= (num_pixels-hist_cumul[wb_whiteindex])))> max_white_pixels) {
00146        max_white_pixels=white_pixels;
00147        brightest_color=color;
00148      }
00149    }
00150    MDF4(fprintf(stderr,"brightest_color=%d, max_white_pixels= 0x%04x, wb_whiteindex=0x%x\n",brightest_color,max_white_pixels,wb_whiteindex));
00151    while (max_white_pixels < wb_whitepix)  { 
00152      for (brightest_color=0;brightest_color<4;brightest_color++) if (colors & (1 << brightest_color)) {
00153        if ( ((max_white_pixels= (num_pixels-histogram_cache[hist_index].cumul_hist[(brightest_color << 8) +wb_whiteindex])))>=wb_whitepix) break;
00154      }
00155      wb_whiteindex--;
00156    }
00157    MDF4(fprintf(stderr,"brightest_color=%d, max_white_pixels= 0x%04x, wb_whiteindex=0x%x\n",brightest_color,max_white_pixels,wb_whiteindex));
00158    hist_cumul=&(histogram_cache[hist_index].cumul_hist[brightest_color << 8]);
00160    if (max_white_pixels < wb_whitepix) { 
00161      while (wb_whiteindex && (hist_cumul[wb_whiteindex] > (num_pixels-wb_whitepix))) wb_whiteindex--;
00162    }
00163    wb_whitelev=wb_whiteindex<<8;
00165    nonwhite_pixels=hist_cumul[wb_whiteindex];  
00166    if (nonwhite_pixels<0) nonwhite_pixels=0; 
00167    nonwhite_frac= (((long long) nonwhite_pixels)<<16)/num_pixels;
00168    white_pixels=num_pixels-nonwhite_pixels;
00169 
00170    levels[brightest_color]=wb_whitelev;
00171    MDF4(fprintf(stderr,"wb_whitelev=0x%x white_pixels=0x%04x, nonwhite_pixels=0x%04x, nonwhite_frac= 0x%04x\n",wb_whitelev, white_pixels,nonwhite_pixels,nonwhite_frac));
00172    for (color=0;color<4;color++) if ((colors & (1 << color)) && (color != brightest_color)){
00174      hist_cumul=     &(histogram_cache[hist_index].cumul_hist[color<<8]);
00175      hist_percentile=&(histogram_cache[hist_index].percentile[color<<8]);
00176      perc=hist_percentile[nonwhite_frac >> (PERCENTILE_SHIFT-8)]; 
00177      MDF4(fprintf(stderr,"perc=0x%04x nonwhite_frac >> (PERCENTILE_SHIFT-8)= 0x%04x\n",perc, nonwhite_frac >> (PERCENTILE_SHIFT-8)));
00178      if (perc>0) perc--;                                              
00179      while ((perc>0) &&  (hist_cumul[perc] > nonwhite_pixels)) perc--;    
00180      perc++;
00181      while ((perc<255) &&  (hist_cumul[perc] <= nonwhite_pixels)) perc++; 
00182      perc--;
00183      delta=hist_cumul[perc+1] - hist_cumul[perc];
00184      MDF4(fprintf(stderr,"perc=0x%04x delta= 0x%04x\n",perc, delta));
00185      if (delta) {
00186        perc_frac=((nonwhite_pixels-hist_cumul[perc]) << (PERCENTILE_SHIFT-8))/delta;
00187      } else perc_frac=0;
00188      levels[color]=(perc << (PERCENTILE_SHIFT-8)) + perc_frac;
00189      MDF4(fprintf(stderr,"hist_cumul[0x%x]=0x%lx hist_cumul[0x%x]=0x%lx, delta=0x%x, levels[%d]=0x%x\n",perc,hist_cumul[perc],perc+1,hist_cumul[perc+1],delta,color,levels[color]));
00190    }
00191 
00193 
00194    for (color=0;color<4;color++) if (colors & (1 << color)){
00195      hash32[color]=get_imageParamsThat  (P_GTAB_R+color, frame-1);
00196      if (hash32[color] == 0xffffffff) return -1;
00198      MDF4(fprintf(stderr,"hash32=0x%lx\n",hash32[color]));
00199    }
00203    MDF4(fprintf(stderr,"levels[]=      0x%04x,0x%04x,0x%04x,0x%04x\n",levels[0],levels[1],levels[2],levels[3]));
00204    MDF4(fprintf(stderr,"hash32[]=      0x%04lx,0x%04lx,0x%04lx,0x%04lx\n",hash32[0],hash32[1],hash32[2],hash32[3]));
00205    MDF4(fprintf(stderr,"corr[]=        0x%04x,0x%04x,0x%04x,0x%04x\n",corr[0],corr[1],corr[2],corr[3]));
00206    min_scale=0xffffffff; 
00207    for (color=0;color<4;color++) if (colors & (1 << color)){
00208      levels[color]= ((hash32[color] & 0xffff) * ((unsigned long) corr[color])) / ((unsigned long) levels[color]);
00209      if (levels[color]<min_scale) min_scale= levels[color];
00210    }
00211    MDF4(fprintf(stderr,"levels[]=      0x%04x,0x%04x,0x%04x,0x%04x\n",levels[0],levels[1],levels[2],levels[3]));
00212    MDF4(fprintf(stderr,"min_scale=   0x%lx\n",min_scale));
00213    for (color=0;color<4;color++) if (colors & (1 << color)){
00214      levels[color]= (levels[color] << GAMMA_SCALE_SHIFT)/min_scale;
00215      if (levels[color]>0xffff) levels[color] = 0xffff;
00216      hash32[color]=(hash32[color] & 0xffff0000) | levels[color];
00217    }
00218    MDF4(fprintf(stderr,"levels[]=      0x%04x,0x%04x,0x%04x,0x%04x\n",levels[0],levels[1],levels[2],levels[3]));
00219    MDF4(fprintf(stderr,"hash32[]=      0x%04lx,0x%04lx,0x%04lx,0x%04lx\n",hash32[0],hash32[1],hash32[2],hash32[3]));
00220 
00226 //unsigned long gammaReverse (unsigned long x)
00227 //unsigned long gammaDirect (unsigned long x);
00230    int ana_corr=(framePars[target_frame8].pars[P_GAIN_CTRL] & (1<<GAIN_BIT_ENABLE))?1:0;
00231    int gain_min_limit=framePars[target_frame8].pars[P_GAIN_MIN];
00232    int gain_max_limit=framePars[target_frame8].pars[P_GAIN_MAX];
00233    int gain_step=framePars[target_frame8].pars[P_GAIN_CTRL] & 0xffff;
00234    int gains[4];
00235    int ungamma_Y_level;
00236    int gain, mod_gains;
00237    int satLevels[4]; 
00238    mod_gains=0;
00239    if (ana_corr) { 
00240      MDF4(fprintf(stderr,"levels[]=  0x%04x,0x%04x,0x%04x,0x%04x, same_greens=%d\n",levels[0],levels[1],levels[2],levels[3],same_greens));
00241      MDF4(fprintf(stderr,"gains[]=   0x%04lx,0x%04lx,0x%04lx,0x%04lx\n",get_imageParamsThat(P_GAINR+0, frame-1),get_imageParamsThat(P_GAINR+1, frame-1),get_imageParamsThat(P_GAINR+2, frame-1),get_imageParamsThat  (P_GAINR+3, frame-1)));
00242      satLevels[COLOR_Y_NUMBER]=gammaReverse((1<<(GAMMA_SCALE_SHIFT+16))/levels[COLOR_Y_NUMBER])>>2; 
00243      ungamma_Y_level=satLevels[COLOR_Y_NUMBER]>>2; 
00244      for (color=0;color<4;color++) if (colors & (1 << color)) {
00245        gains[color]=get_imageParamsThat  (P_GAINR+color, frame-1);
00246        if (gains[color] == 0xffffffff) break;
00247        if (color != COLOR_Y_NUMBER) {
00248          satLevels[color]=gammaReverse((1<<(GAMMA_SCALE_SHIFT+16))/levels[color]) >>2; 
00249 
00250 //         gain=(gains[color]*levels[color]) / levels[COLOR_Y_NUMBER];
00251          gain=(gains[color]* satLevels[COLOR_Y_NUMBER])/satLevels[color]; 
00252          if (gain>gain_max_limit) gain=gain_max_limit;
00253          if (gain<gain_min_limit) gain=gain_min_limit;
00255          if (same_greens && (color==COLOR_GREEN2)) gain =gains[COLOR_GREEN1];
00257          if ((gain != gains[color])  &&
00258              ((same_greens && (color==COLOR_GREEN2)) ||
00259               ((gain > gains[color]) && (gain > (((gains[color])*(0x100+gain_step))>>8) )) ||
00260               ((gain < gains[color]) && (gain < (((gains[color])*(0x100-gain_step))>>8) )))) {
00261             levels[color]= (1<<(GAMMA_SCALE_SHIFT+16))/ gammaDirect(((satLevels[color]*gain)/gains[color])<<2); 
00262             hash32[color]=(hash32[color] & 0xffff0000) | levels[color];
00263             gains[color]=gain;
00264             mod_gains |= (1<<color);
00265          }
00266        }
00267      }
00268      MDF4(fprintf(stderr,"satLevels[]=  0x%04x,0x%04x,0x%04x,0x%04x\n",satLevels[0],satLevels[1],satLevels[2],satLevels[3]));
00269      MDF4(fprintf(stderr,"levels[]=     0x%04x,0x%04x,0x%04x,0x%04x\n",levels[0],levels[1],levels[2],levels[3]));
00270      MDF4(fprintf(stderr,"gains[]=      0x%04x,0x%04x,0x%04x,0x%04x\n",gains[0],gains[1],gains[2],gains[3]));
00272      int minScale=levels[COLOR_Y_NUMBER];
00273      for (color=0;color<4;color++) if ((colors & (1 << color)) && (levels[color] < minScale)) minScale=levels[color];
00274      if (minScale > (1<<GAMMA_SCALE_SHIFT)) {
00275        MDF4(fprintf(stderr,"minScale= 0x%x - reducing all scales\n",minScale));
00276        for (color=0;color<4;color++) if (colors & (1 << color)) {
00277          levels[color]=(levels[color] << GAMMA_SCALE_SHIFT)/minScale;
00278          hash32[color]=(hash32[color] & 0xffff0000) | levels[color];
00279        }
00280      }
00281      MDF4(fprintf(stderr,"levels[]=     0x%04x,0x%04x,0x%04x,0x%04x\n",levels[0],levels[1],levels[2],levels[3]));
00282    }
00283 
00285   i=0;
00286   write_data[i++]= FRAMEPARS_SETFRAME;
00287   write_data[i++]= target_frame;
00288   for (color=0;color<4;color++) if (colors & (1 << color)){
00289     write_data[i++]= P_GTAB_R+color;
00290     write_data[i++]= hash32[color];
00291     if (mod_gains & (1 << color)) {
00292       write_data[i++]= P_GAINR+color;
00293       write_data[i++]= gains[color];
00294     }
00295   }
00296   rslt=write(fd_fparmsall, write_data, (i << 2));
00297   if (rslt < (i << 2)) return -errno;
00298   GLOBALPARS(G_NEXT_WB_FRAME)=frame+framePars[target_frame8].pars[P_WB_PERIOD]; 
00299   return 1;
00300 }