C++ cabsf函数代码示例

/* subroutines for fwi */
float genadjsrc_fwi(sf_complex ***obs,
                    sf_complex ***syn,
                    sf_complex ***adj,
                    float **recloc,
                    const int n1, 
                    const int n2,
                    const int ns)
/*< generate adjoint source for fwi >*/
{
    int i, j, k;
    sf_complex temp;
    float misfit;

    misfit = 0.0; 
    for ( k = 0; k < ns; k++ ){ 
    for ( j = 0; j < n2; j++) {
    for ( i = 0; i < n1; i++ ){ 
        adj[k][j][i] = sf_cmplx(0.0,0.0); 

        if ( recloc[j][i] > 0.0 ) { 
            temp = obs[k][j][i] - syn[k][j][i];
            misfit += cabsf(temp) * cabsf(temp); 
            adj[k][j][i] = temp;
        }
    }  /* for i  */ 
    }  /* for j  */
    }  /* for ns */

    return misfit;
}

/* Performs 1st order linear interpolation with out-of-bound interpolation */
void interp_linear_offset(cf_t *input, cf_t *output, int M, int len, int off_st, int off_end) {
	int i, j;
	float mag0, mag1, arg0, arg1, mag, arg;

	for (i=0;i<len-1;i++) {
		mag0 = cabsf(input[i]);
		mag1 = cabsf(input[i+1]);
		arg0 = cargf(input[i]);
		arg1 = cargf(input[i+1]);
		if (i==0) {
			for (j=0;j<off_st;j++) {
				mag = mag0 - (j+1)*(mag1-mag0)/M;
				arg = arg0 - (j+1)*(arg1-arg0)/M;
				output[j] = mag * cexpf(I * arg);
			}
		}
		for (j=0;j<M;j++) {
			mag = mag0 + j*(mag1-mag0)/M;
			arg = arg0 + j*(arg1-arg0)/M;
			output[i*M+j+off_st] = mag * cexpf(I * arg);
		}
	}
	if (len > 1) {
		for (j=0;j<off_end;j++) {
			mag = mag1 + j*(mag1-mag0)/M;
			arg = arg1 + j*(arg1-arg0)/M;
			output[i*M+j+off_st] = mag * cexpf(I * arg);
		}
	}
}

void fftUpdate(Fft *fft, float complex *inbuf, int count, FftOutputFunc *func, void *context)
{
    float complex *in = inbuf;
    fftw_complex  *fftwin = fft->in;
    int N     = fft->N;
    int inPtr = fft->inPtr;
    while (count--)
        {
        if ((fft->skipCounter++) < fft->threshold)
            continue;
        fftwin[inPtr++] = *in++;
        if (inPtr >= N)
            {
            inPtr = 0;
            fftw_execute(fft->plan);
            unsigned int *ps = fft->spectrum;
            int half = N>>1;
            fftw_complex *lower = fft->out;
            fftw_complex *upper = fft->out + half;
            int count = half;
            while (count--)
                {
                *ps++ = (unsigned int)(20.0 * fasterlog2(1.0 + cabsf(*upper++)));
                }
            count = half;
            while (count--)
                {
                *ps++ = (unsigned int)(20.0 * fasterlog2(1.0 + cabsf(*lower++)));
                }
            func(fft->spectrum, N, context);
            fft->skipCounter = 0;
            }
        }

float calc_misfit(sf_complex ***obs,
                  sf_complex ***syn,
                  float **recloc,
                  const int n1, 
                  const int n2, 
                  const int ns)
/*< calculate misfit value >*/
{
    int i, j, k;
    float misfit; 
    sf_complex temp;

    misfit = 0.0; 
    for ( k = 0; k < ns; k++ ){ 
    for ( j = 0; j < n2; j++) {
    for ( i = 0; i < n1; i++ ){ 
        if ( recloc[j][i] > 0.0 ) { 
            temp = obs[k][j][i] - syn[k][j][i];
            misfit += cabsf(temp) * cabsf(temp); 
        }
    }  /* for i */
    }  /* for j */
    }  /* for ns */
    return misfit;
}

/* Performs 1st order linear interpolation with out-of-bound interpolation */
void srslte_interp_linear_offset_cabs(cf_t *input, cf_t *output, 
                               uint32_t M, uint32_t len, 
                               uint32_t off_st, uint32_t off_end) 
{
  uint32_t i, j;
  float mag0=0, mag1=0, arg0=0, arg1=0, mag=0, arg=0;

  for (i=0;i<len-1;i++) {
    mag0 = cabsf(input[i]);
    mag1 = cabsf(input[i+1]);
    arg0 = cargf(input[i]);
    arg1 = cargf(input[i+1]);
    if (i==0) {
      for (j=0;j<off_st;j++) {
        mag = mag0 - (j+1)*(mag1-mag0)/M;
        arg = arg0 - (j+1)*(arg1-arg0)/M;
        output[j] = mag * cexpf(I * arg);
      }
    }
    for (j=0;j<M;j++) {
      mag = mag0 + j*(mag1-mag0)/M;
      arg = arg0 + j*(arg1-arg0)/M;
      output[i*M+j+off_st] = mag * cexpf(I * arg);
    }
  }
  if (len > 1) {
    for (j=0;j<off_end;j++) {
      mag = mag1 + j*(mag1-mag0)/M;
      arg = arg1 + j*(arg1-arg0)/M;
      output[i*M+j+off_st] = mag * cexpf(I * arg);
    }
  }
}

void ofdmoqamframe64sync_execute_plcplong1(ofdmoqamframe64sync _q, float complex _x)
{
    // cross-correlator
    float complex rxy;
    firfilt_cccf_push(_q->crosscorr, _x);
    firfilt_cccf_execute(_q->crosscorr, &rxy);

    rxy *= _q->g;

#if DEBUG_OFDMOQAMFRAME64SYNC
    windowcf_push(_q->debug_rxy, rxy);
#endif

    _q->timer++;
    if (_q->timer < _q->num_subcarriers-8) {
        return;
    } else if (_q->timer > _q->num_subcarriers+8) {
#if DEBUG_OFDMOQAMFRAME64SYNC_PRINT
        printf("warning: ofdmoqamframe64sync could not find second PLCP long sequence; resetting synchronizer\n");
#endif
        ofdmoqamframe64sync_reset(_q);
        return;
    }

    if (cabsf(rxy) > 0.7f*(_q->rxy_thresh)*(_q->num_subcarriers)) {
#if DEBUG_OFDMOQAMFRAME64SYNC_PRINT
        printf("rxy[1] : %12.8f at input[%3u]\n", cabsf(rxy), _q->num_samples);
#endif

        // 
        float complex * rc;
        windowcf_read(_q->input_buffer, &rc);
        memmove(_q->S1b, rc, 64*sizeof(float complex));

        // estimate frequency offset
        float complex rxy_hat=0.0f;
        unsigned int j;
        for (j=0; j<64; j++) {
            rxy_hat += _q->S1a[j] * conjf(_q->S1b[j]) * hamming(j,64);
        }
        float nu_hat1 = -cargf(rxy_hat);
        if (nu_hat1 >  M_PI) nu_hat1 -= 2.0f*M_PI;
        if (nu_hat1 < -M_PI) nu_hat1 += 2.0f*M_PI;
        nu_hat1 /= 64.0f;
#if DEBUG_OFDMOQAMFRAME64SYNC_PRINT
        printf("nu_hat[0] = %12.8f\n", _q->nu_hat);
        printf("nu_hat[1] = %12.8f\n", nu_hat1);
#endif
        nco_crcf_adjust_frequency(_q->nco_rx, nu_hat1);

        /*
        printf("exiting prematurely\n");
        ofdmoqamframe64sync_destroy(_q);
        exit(1);
        */
        _q->state = OFDMOQAMFRAME64SYNC_STATE_RXSYMBOLS;
    }

}

void wlanframesync_execute_rxlong0(wlanframesync _q)
{
    // set timer to 16, wait for phase to be relatively small
    
    _q->timer++;
    if (_q->timer < 16)
        return;

    // reset timer
    _q->timer = 0;

    // run fft
    float complex * rc;
    windowcf_read(_q->input_buffer, &rc);

    // estimate S1 gain, adding backoff in gain estimation
    wlanframesync_estimate_gain_S1(_q, &rc[16-2], _q->G1a);

    // compute S1 metrics
    float complex s_hat;
    wlanframesync_S1_metrics(_q, _q->G1a, &s_hat);
    s_hat *= _q->g0;    // scale output by raw gain estimate

    // rotate by complex phasor relative to timing backoff
    //s_hat *= cexpf(_Complex_I * 2.0f * 2.0f * M_PI / 64.0f);
    s_hat *= cexpf(_Complex_I * 0.19635f);

    // save first 'long' symbol statistic
    _q->s1a_hat = s_hat;

#if DEBUG_WLANFRAMESYNC_PRINT
    printf("    s_hat   :   %12.8f <%12.8f>\n", cabsf(s_hat), cargf(s_hat));
#endif

    float s_hat_abs = cabsf(s_hat);
    float s_hat_arg = cargf(s_hat);
    if (s_hat_arg >  M_PI) s_hat_arg -= 2.0f*M_PI;
    if (s_hat_arg < -M_PI) s_hat_arg += 2.0f*M_PI;
    
    // check conditions for s_hat:
    //  1. magnitude should be large (near unity) when aligned
    //  2. phase should be very near zero (time aligned)
    if (s_hat_abs        > WLANFRAMESYNC_S1A_ABS_THRESH &&
        fabsf(s_hat_arg) < WLANFRAMESYNC_S1A_ARG_THRESH)
    {
#if DEBUG_WLANFRAMESYNC_PRINT
        printf("    acquisition S1[a]\n");
#endif
        
        // set state
        _q->state = WLANFRAMESYNC_STATE_RXLONG1;

        // reset timer
        _q->timer = 0;
    }

}

void ofdmoqamframe64sync_execute_plcpshort(ofdmoqamframe64sync _q, float complex _x)
{
    // run AGC, clip output
    float complex y;
    agc_crcf_execute(_q->sigdet, _x, &y);
    //if (agc_crcf_get_signal_level(_q->sigdet) < -15.0f)
    //    return;
    if (cabsf(y) > 2.0f)
        y = 2.0f*liquid_cexpjf(cargf(y));

    // auto-correlators
    //autocorr_cccf_push(_q->autocorr, _x);
    autocorr_cccf_push(_q->autocorr, y);
    autocorr_cccf_execute(_q->autocorr, &_q->rxx);

#if DEBUG_OFDMOQAMFRAME64SYNC
    windowcf_push(_q->debug_rxx, _q->rxx);

    windowf_push(_q->debug_rssi, agc_crcf_get_signal_level(_q->sigdet));
#endif
    float rxx_mag = cabsf(_q->rxx);

    float threshold = (_q->rxx_thresh)*(_q->autocorr_length);

    if (rxx_mag > threshold) {
        // wait for auto-correlation to peak before changing state
        if (rxx_mag > _q->rxx_mag_max) {
            _q->rxx_mag_max = rxx_mag;
            return;
        }

        // estimate CFO
        _q->nu_hat = cargf(_q->rxx);
        if (_q->nu_hat >  M_PI/2.0f) _q->nu_hat -= M_PI;
        if (_q->nu_hat < -M_PI/2.0f) _q->nu_hat += M_PI;
        _q->nu_hat *= 4.0f / (float)(_q->num_subcarriers);

#if DEBUG_OFDMOQAMFRAME64SYNC_PRINT
        printf("rxx = |%12.8f| arg{%12.8f}\n", cabsf(_q->rxx),cargf(_q->rxx));
        printf("nu_hat =  %12.8f\n", _q->nu_hat);
#endif

        nco_crcf_set_frequency(_q->nco_rx, _q->nu_hat);
        _q->state = OFDMOQAMFRAME64SYNC_STATE_PLCPLONG0;

        _q->g = agc_crcf_get_gain(_q->sigdet);

#if DEBUG_OFDMOQAMFRAME64SYNC_PRINT
        printf("gain : %f\n", _q->g);
#endif
        _q->timer=0;
    }
}

// Help function to keep code base small
//  _kf     :   modulation factor
//  _type   :   demodulation type {LIQUID_FREQDEM_DELAYCONJ, LIQUID_FREQDEM_PLL}
void freqmodem_test(float               _kf,
                    liquid_freqdem_type _type)
{
    // options
    unsigned int num_samples = 1024;
    //float tol = 1e-2f;

    unsigned int i;

    // create mod/demod objects
    freqmod mod = freqmod_create(_kf);          // modulator
    freqdem dem = freqdem_create(_kf,_type);    // demodulator

    // allocate arrays
    float         m[num_samples];   // message signal
    float complex r[num_samples];   // received signal (complex baseband)
    float         y[num_samples];   // demodulator output

    // generate message signal (single-frequency sine)
    for (i=0; i<num_samples; i++)
        m[i] = 0.7f*cosf(2*M_PI*0.013f*i + 0.0f);

    // modulate/demodulate signal
    for (i=0; i<num_samples; i++) {
        // modulate
        freqmod_modulate(mod, m[i], &r[i]);

        // demodulate
        freqdem_demodulate(dem, r[i], &y[i]);
    }

    // delete modem objects
    freqmod_destroy(mod);
    freqdem_destroy(dem);

#if 0
    // compute power spectral densities and compare
    float complex mcf[num_samples];
    float complex ycf[num_samples];
    float complex M[num_samples];
    float complex Y[num_samples];
    for (i=0; i<num_samples; i++) {
        mcf[i] = m[i] * hamming(i,num_samples);
        ycf[i] = y[i] * hamming(i,num_samples);
    }
    fft_run(num_samples, mcf, M, LIQUID_FFT_FORWARD, 0);
    fft_run(num_samples, ycf, Y, LIQUID_FFT_FORWARD, 0);

    // run test: compare spectral magnitude
    for (i=0; i<num_samples; i++)
        CONTEND_DELTA( cabsf(Y[i]), cabsf(M[i]), tol );
#endif
}

// get demodulator frequency error
float fskdem_get_frequency_error(fskdem _q)
{
    // get index of peak bin
    //unsigned int index = _q->buf_freq[ _q->s_demod ];

    // extract peak value of previous, post FFT index
    float vm = cabsf(_q->buf_freq[(_q->s_demod+_q->K-1)%_q->K]);  // previous
    float v0 = cabsf(_q->buf_freq[ _q->s_demod               ]);  // peak
    float vp = cabsf(_q->buf_freq[(_q->s_demod+      1)%_q->K]);  // post

    // compute derivative
    // TODO: compensate for bin spacing
    // TODO: just find peak using polynomial interpolation
    return (vp - vm) / v0;
}

// frame detection
void wlanframesync_execute_rxshort0(wlanframesync _q)
{
    _q->timer++;
    if (_q->timer < 16)
        return;

    // reset timer
    _q->timer = 0;

    // read contents of input buffer
    float complex * rc;
    windowcf_read(_q->input_buffer, &rc);

    // re-estimate S0 gain
    wlanframesync_estimate_gain_S0(_q, &rc[16], _q->G0a);

    float complex s_hat;
    wlanframesync_S0_metrics(_q, _q->G0a, &s_hat);
    //float g = agc_crcf_get_gain(_q->agc_rx);
    s_hat *= _q->g0;

    // save first 'short' symbol statistic
    _q->s0a_hat = s_hat;

#if DEBUG_WLANFRAMESYNC_PRINT
    float tau_hat  = cargf(s_hat) * 16.0f / (2*M_PI);
    printf("********** S0[a] received ************\n");
    printf("    s_hat   :   %12.8f <%12.8f>\n", cabsf(s_hat), cargf(s_hat));
    printf("  tau_hat   :   %12.8f\n", tau_hat);
#endif

    _q->state = WLANFRAMESYNC_STATE_RXSHORT1;
}

// demodulate symbol, assuming perfect symbol timing
//  _q      :   fskdem object
//  _y      :   input sample array [size: _k x 1]
unsigned int fskdem_demodulate(fskdem          _q,
                               float complex * _y)
{
    // copy input to internal time buffer
    memmove(_q->buf_time, _y, _q->k*sizeof(float complex));

    // compute transform, storing result in 'buf_freq'
    FFT_EXECUTE(_q->fft);

    // find maximum by looking at particular bins
    float        vmax  = 0;
    unsigned int s     = 0;

    // run search
    for (s=0; s<_q->M; s++) {
        float v = cabsf( _q->buf_freq[_q->demod_map[s]] );
        if (s==0 || v > vmax) {
            // save optimal output symbol
            _q->s_demod = s;

            // save peak FFT bin value
            vmax = v;
        }
    }

    // save best result
    return _q->s_demod;
}

//  _p      : polynomial,     [size: _order+1 x 1]
//  _r      : roots (sorted), [size: _order   x 1]
//  _ordre  : polynomial order
void polyf_findroots_testbench(float *         _p,
                               float complex * _r,
                               unsigned int    _order)
{
    float tol=1e-6f;

    float complex roots[_order];
    polyf_findroots(_p,_order+1,roots);

    unsigned int i;
    if (liquid_autotest_verbose) {
        printf("poly:\n");
        for (i=0; i<=_order; i++)
            printf("  p[%3u] = %12.8f\n", i, _p[i]);

        printf("roots:\n");
        for (i=0; i<_order; i++) {
            float e = cabsf(roots[i] - _r[i]);
            printf("  r[%3u] = %12.8f + %12.8fj (%12.8f + %12.8fj) %12.4e%s\n",
                    i,
                    crealf(roots[i]), cimagf(roots[i]),
                    crealf(   _r[i]), cimagf(   _r[i]),
                    e, e < tol ? "" : " *");
        }
    }

    // check to see if roots match within relative tolerance
    for (i=0; i<_order; i++) {
        CONTEND_DELTA(crealf(roots[i]), crealf(_r[i]), tol);
        CONTEND_DELTA(cimagf(roots[i]), cimagf(_r[i]), tol);
    }
}

static void crop_weight(const long dims[DIMS], complex float* ptr, weight_function fun, float crth, const complex float* map)
{
	long xx = dims[0];
	long yy = dims[1];
	long zz = dims[2];
	long cc = dims[3];
	long mm = dims[4];

	assert(DIMS >= 5);
	assert(1 == md_calc_size(DIMS - 5, dims + 5));



	for (long m = 0; m < mm; m++) {
#pragma omp parallel for
		for (long k = 0; k < zz; k++) {
			for (long i = 0; i < yy; i++) {
				for (long j = 0; j < xx; j++) {

					float val = cabsf(map[((m * zz + k) * yy + i) * xx + j]);

					for (long c = 0; c < cc; c++)
						ptr[(((m * cc + c) * zz + k) * yy + i) * xx + j] *= fun(crth, val);
				}
			}
		}
	}
}

float complex clogf(float complex z) {
  float r, phi;

  r = cabsf(z);
  phi = cargf(z);
  return CMPLXF(logf(r), phi);
}