Ulf@538: #include "AGC.h"
pfh1976@554: #include <cmath>
pfh1976@554: #include <stdlib.h>
pfh1976@554: #include <stdio.h>  
Ulf@538: 
Ulf@586: AGC_parameters::AGC_parameters():
Ulf@586: n_stages_(4),
Ulf@586: time_constants_({0.002*1, 0.002*4, 0.002*16, 0.002*64}),
Ulf@586: agc_stage_gain_(2),
Ulf@586: decimation_({8, 2, 2, 2}),
Ulf@586: agc1_scales_({1.0, 1.4,  2.0, 2.8}),
Ulf@586: agc2_scales_({1.6, 2.25, 3.2, 4.5}),
Ulf@586: detect_scale_(0.25),
Ulf@586: agc_mix_coeff_(0.5)
Ulf@586: {
Ulf@586:   // do nothing more
Ulf@586: }
Ulf@586: 
Ulf@564: AGC_coefficients::AGC_coefficients(AGC_parameters* AGC_params_p,
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;
Ulf@557: 
pfh1976@554:   n_ch_ = n_ch;
Ulf@564:   n_agc_stages_ = AGC_params_p->n_stages_;
Ulf@564:   agc_stage_gain_ = AGC_params_p->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_;
Ulf@564:   FloatArray agc1_scales = AGC_params_p->agc1_scales_;
Ulf@564:   FloatArray agc2_scales = AGC_params_p->agc2_scales_;
pfh1976@554:   FloatArray agc_spatial_FIR;
Ulf@564:   decimation_ = AGC_params_p->decimation_;
pfh1976@554:   
pfh1976@554:   for(int stage=0; stage < n_agc_stages_; stage++){
Ulf@564:     tau = AGC_params_p->time_constants_[stage];
Ulf@564:     decim *= AGC_params_p->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;
Ulf@557: 
Ulf@557:     agc_spatial_FIR = Build_FIR_coeffs(spread_sq, delay, &n_iterations, &n_taps);
Ulf@557: 
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:       
Ulf@564:     total_DC_gain += pow(AGC_params_p->agc_stage_gain_,stage);
pfh1976@554:     
pfh1976@554:     if(stage == 0)
pfh1976@554:       agc_mix_coeffs_[stage] = 0.0;
pfh1976@554:     else
Ulf@564:       agc_mix_coeffs_[stage] = AGC_params_p->agc_mix_coeff_/(tau * (fs/decim));
pfh1976@554:   }
pfh1976@554:   agc_gain_ = total_DC_gain;
Ulf@564:   detect_scale_ = AGC_params_p->detect_scale_/total_DC_gain;
Ulf@538: }
pfh1976@554: 
Ulf@557: FloatArray AGC_coefficients::Build_FIR_coeffs(float var, float mn, int* ptr_iters, int* ptr_taps){
pfh1976@554:   float a, b;
pfh1976@554:   FloatArray FIR(3);
Ulf@557: 
Ulf@557:   // Try with 3 FIR taps
Ulf@557:   a = (var + mn*mn - mn)/2;
Ulf@557:   b = (var + mn*mn + mn)/2;
Ulf@557:   FIR[0] = a;
Ulf@557:   FIR[1] = 1.0 - a - b;
Ulf@557:   FIR[2] = b;
Ulf@557: 
Ulf@557:   if(FIR[1] >= 0.2){
Ulf@557:     *ptr_taps = 3;
Ulf@557:     return FIR;
pfh1976@554:   }
Ulf@557:   else //Try with 5 FIR taps
Ulf@557:     {
Ulf@557:       for(int n_iter = 1; n_iter<16; n_iter++){
Ulf@557:         mn /= n_iter;
Ulf@557:         var /= n_iter;
Ulf@557: 
Ulf@557:         a = ((var + mn*mn)*2/5 - mn*2/3)/2;
Ulf@557:         b = ((var + mn*mn)*2/5 + mn*2/3)/2;
Ulf@557:         FIR[0] = a/2;
Ulf@557:         FIR[1] = 1.0 - a - b;
Ulf@557:         FIR[2] = b;
Ulf@557: 
Ulf@557:         *ptr_iters = n_iter;
Ulf@557: 
Ulf@557:         if(FIR[1] >= 0.1){
Ulf@557:             *ptr_taps = 5;
Ulf@557:             return FIR;
Ulf@557:         }
Ulf@557:       }
Ulf@557:       //TODO: discuss how we handle errors
Ulf@557:       printf("Too many iterations in FIR_coeffs\n");
Ulf@557:       FIR = {0, 0, 0};
Ulf@557:       return FIR;
Ulf@557:    }
pfh1976@554: }