Ulf@538: #include "AGC.h"
pfh1976@554: #include <cmath>
pfh1976@554: #include <stdlib.h>
pfh1976@554: #include <stdio.h>  
Ulf@538: 
Ulf@545: AGC_coefficients::AGC_coefficients(AGC_parameters* AGC_params,
Ulf@545:                                    float fs, int n_ch){
pfh1976@554:   float decim = 1.0;
pfh1976@554:   float total_DC_gain = 0.0;
pfh1976@554:   float tau, ntimes, delay, spread_sq, u, p, dp;
pfh1976@554:   int n_taps = 0, n_iterations = 1;
pfh1976@554:   bool FIR_OK = false;
pfh1976@554:   
pfh1976@554:   n_ch_ = n_ch;
pfh1976@554:   n_agc_stages_ = AGC_params->n_stages_;
pfh1976@554:   agc_stage_gain_ = AGC_params->agc_stage_gain_;
pfh1976@554:   
pfh1976@554:   // FloatArray initialization using assign method - dont know if this is good enough
pfh1976@554:   agc_epsilon_.assign(n_agc_stages_, 0.0); //the 1/(tau*fs) roughly
pfh1976@554:   agc_polez1_ = agc_epsilon_;
pfh1976@554:   agc_polez2_ = agc_epsilon_;
pfh1976@554:   agc_spatial_iterations_ = agc_epsilon_;
pfh1976@554:   agc_spatial_n_taps_ = agc_epsilon_;
pfh1976@554:   agc_mix_coeffs_ = agc_epsilon_;
pfh1976@554:   FloatArray agc1_scales = AGC_params->agc1_scales_;
pfh1976@554:   FloatArray agc2_scales = AGC_params->agc2_scales_;
pfh1976@554:   FloatArray agc_spatial_FIR;
pfh1976@554:   decimation_ = AGC_params->decimation_;
pfh1976@554:   
pfh1976@554:   for(int stage=0; stage < n_agc_stages_; stage++){
pfh1976@554:     tau = AGC_params->time_constants_[stage];
pfh1976@554:     decim *= AGC_params->decimation_[stage];
pfh1976@554:     agc_epsilon_[stage] = 1.0 - exp(-decim/(tau*fs));
pfh1976@554:     ntimes = tau * (fs/decim);
pfh1976@554:     delay = (agc2_scales[stage]-agc1_scales[stage])/ntimes;
pfh1976@554:     spread_sq = (agc1_scales[stage]*agc1_scales[stage] + agc2_scales[stage]*agc2_scales[stage])/ntimes;
pfh1976@554:     
pfh1976@554:     u = 1.0 + 1.0/spread_sq;
pfh1976@554:     p = u - sqrt(u*u-1);
pfh1976@554:     dp = delay*(1 - 2*p + p*p)*0.5;
pfh1976@554:     agc_polez1_[stage] = p - dp;
pfh1976@554:     agc_polez2_[stage] = p + dp;
pfh1976@554:     
pfh1976@554:     while(!FIR_OK){
pfh1976@554:       switch(n_taps){
pfh1976@554:         case 0:
pfh1976@554:           n_taps = 3;
pfh1976@554:           break;
pfh1976@554:         case 3:
pfh1976@554:           n_taps = 5;
pfh1976@554:           break;
pfh1976@554:         case 5:
pfh1976@554:           n_iterations++;
pfh1976@554:           if(n_iterations > 16){
pfh1976@554:             printf("Too many n_iterations in CARFAC_DesignAGC\n");
pfh1976@554:             exit(1);
pfh1976@554:           }
pfh1976@554:           break;
pfh1976@554:         default:
pfh1976@554:           printf("Bad n_taps in CARFAC_DesignAGC\n");
pfh1976@554:           exit(1);
pfh1976@554:       }
pfh1976@554:       agc_spatial_FIR = FIR_coeffs(n_taps, spread_sq, delay, n_iterations, &FIR_OK);
pfh1976@554:     }
pfh1976@554:     agc_spatial_iterations_[stage] = (float) n_iterations;
pfh1976@554:     agc_spatial_n_taps_[stage] = (float) n_taps;
pfh1976@554:     agc_spatial_fir_.push_back(FloatArray());
pfh1976@554:     
pfh1976@554:     for(int i =0; i < 3; i++)
pfh1976@554:       agc_spatial_fir_[stage].push_back(agc_spatial_FIR[i]);
pfh1976@554:       
pfh1976@554:     total_DC_gain += pow(AGC_params->agc_stage_gain_,stage);
pfh1976@554:     
pfh1976@554:     if(stage == 0)
pfh1976@554:       agc_mix_coeffs_[stage] = 0.0;
pfh1976@554:     else
pfh1976@554:       agc_mix_coeffs_[stage] = AGC_params->agc_mix_coeff_/(tau * (fs/decim));
pfh1976@554:   }
pfh1976@554:   agc_gain_ = total_DC_gain;
pfh1976@554:   
pfh1976@554:   // TODO Computation of the detect_scale_ member
Ulf@538: }
Ulf@545: AGC_coefficients::~AGC_coefficients(){
Ulf@544:   // TODO Auto-generated destructor stub
Ulf@538: }
pfh1976@554: 
pfh1976@554: FloatArray  AGC_coefficients::FIR_coeffs(int n_taps, float var, float mn, int n_iter, bool* ptr_FIR_OK)
pfh1976@554: {
pfh1976@554:   float a, b;
pfh1976@554:   FloatArray FIR(3);
pfh1976@554:   mn /= n_iter;
pfh1976@554:   var /= n_iter;
pfh1976@554:   
pfh1976@554:   switch(n_taps){
pfh1976@554:     case 3:
pfh1976@554:       a = (var + mn*mn - mn)/2;
pfh1976@554:       b = (var + mn*mn + mn)/2;
pfh1976@554:       FIR[0] = a;
pfh1976@554:       FIR[1] = 1.0 - a - b;
pfh1976@554:       FIR[2] = b;
pfh1976@554:       if(FIR[1] >= 0.2)
pfh1976@554:         *ptr_FIR_OK = true;
pfh1976@554:       break;
pfh1976@554:     case 5:
pfh1976@554:       a = ((var + mn*mn)*2/5 - mn*2/3)/2;
pfh1976@554:       b = ((var + mn*mn)*2/5 + mn*2/3)/2;
pfh1976@554:       FIR[0] = a/2;
pfh1976@554:       FIR[1] = 1.0 - a - b;
pfh1976@554:       FIR[2] = b;
pfh1976@554:       if(FIR[1] >= 0.1)
pfh1976@554:         *ptr_FIR_OK = true;
pfh1976@554:       break;
pfh1976@554:     default:
pfh1976@554:       printf("Bad n_taps in AGC_spatial_FIR\n");
pfh1976@554:       exit(1);
pfh1976@554:   }
pfh1976@554:   
pfh1976@554:   return FIR;
pfh1976@554: }