/* 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;
}
static
void
BLAS_csy_norm(enum blas_order_type order, enum blas_norm_type norm,
enum blas_uplo_type uplo, int n, const PLASMA_Complex32_t *a, int lda, float *res) {
int i, j; float anorm, v;
char rname[] = "BLAS_csy_norm";
if (order != blas_colmajor) BLAS_error( rname, -1, order, 0 );
if (norm == blas_inf_norm) {
anorm = 0.0;
if (blas_upper == uplo) {
for (i = 0; i < n; ++i) {
v = 0.0;
for (j = 0; j < i; ++j) {
v += cabsf( a[j + i * lda] );
}
for (j = i; j < n; ++j) {
v += cabsf( a[i + j * lda] );
}
if (v > anorm)
anorm = v;
}
} else {
BLAS_error( rname, -3, norm, 0 );
return;
}
} else {
BLAS_error( rname, -2, norm, 0 );
return;
}
if (res) *res = anorm;
}
/* 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);
}
static void
compare(complex float *a, complex float *b, int logN)
{
int i;
/* Scan all values */
for (i=0; i<(1<<logN); i++)
{
if (cabsf(a[i] - b[i]) / cabsf(a[i]) > 2e-5)
printf("%4d | %10f %10f | %10f %10f | %f\n", i,
crealf(a[i]), cimagf(a[i]),
crealf(b[i]), cimagf(b[i]),
cabsf(a[i] - b[i])
);
}
}
// checks stability of iir filter
// _b : feed-forward coefficients [size: _n x 1]
// _a : feed-back coefficients [size: _n x 1]
// _n : number of coefficients
int iirdes_isstable(float * _b,
float * _a,
unsigned int _n)
{
// validate input
if (_n < 2) {
fprintf(stderr,"error: iirdes_isstable(), filter order too low\n");
exit(1);
}
unsigned int i;
// flip denominator, left to right
float a_hat[_n];
for (i=0; i<_n; i++)
a_hat[i] = _a[_n-i-1];
// compute poles (roots of denominator)
float complex roots[_n-1];
polyf_findroots_bairstow(a_hat, _n, roots);
#if 0
// print roots
printf("\nroots:\n");
for (i=0; i<_n-1; i++)
printf(" r[%3u] = %12.8f + j *%12.8f\n", i, crealf(roots[i]), cimagf(roots[i]));
#endif
// compute magnitude of poles
for (i=0; i<_n-1; i++) {
if (cabsf(roots[i]) > 1.0)
return 0;
}
return 1;
}
static inline void ctop(float *dst, const float complex *src, int c)
{
for (int k = 0; k < c; k++)
{
dst[k ] = cabsf(src[k]);
dst[k + c] = cargf(src[k]);
}
}
// frame detection
void ofdmframesync_execute_S0a(ofdmframesync _q)
{
//printf("t : %u\n", _q->timer);
_q->timer++;
if (_q->timer < _q->M2)
return;
// reset timer
_q->timer = 0;
//
float complex * rc;
windowcf_read(_q->input_buffer, &rc);
// TODO : re-estimate nominal gain
// estimate S0 gain
ofdmframesync_estimate_gain_S0(_q, &rc[_q->cp_len], _q->G0);
float complex s_hat;
ofdmframesync_S0_metrics(_q, _q->G0, &s_hat);
s_hat *= _q->g0;
_q->s_hat_0 = s_hat;
#if DEBUG_OFDMFRAMESYNC_PRINT
float tau_hat = cargf(s_hat) * (float)(_q->M2) / (2*M_PI);
printf("********** S0[0] 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
#if 0
// TODO : also check for phase of s_hat (should be small)
if (cabsf(s_hat) < 0.3f) {
// false alarm
#if DEBUG_OFDMFRAMESYNC_PRINT
printf("false alarm S0[0]\n");
#endif
ofdmframesync_reset(_q);
return;
}
#endif
_q->state = OFDMFRAMESYNC_STATE_PLCPSHORT1;
}
float complex
clog10f (float complex z)
{
float complex v;
COMPLEX_ASSIGN (v, log10f (cabsf (z)), cargf (z));
return v;
}
/* log10(z) = log10 (cabs(z)) + i*carg(z) */
GFC_COMPLEX_4
clog10f (GFC_COMPLEX_4 z)
{
GFC_COMPLEX_4 v;
COMPLEX_ASSIGN (v, log10f (cabsf (z)), cargf (z));
return v;
}
// frame detection
void wlanframesync_execute_rxshort1(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);
// estimate S0 gain
wlanframesync_estimate_gain_S0(_q, &rc[16], _q->G0b);
float complex s_hat;
wlanframesync_S0_metrics(_q, _q->G0b, &s_hat);
//float g = agc_crcf_get_gain(_q->agc_rx);
s_hat *= _q->g0;
// save second 'short' symbol statistic
_q->s0b_hat = s_hat;
#if DEBUG_WLANFRAMESYNC_PRINT
float tau_hat = cargf(s_hat) * 16.0f / (2*M_PI);
printf("********** S0[b] received ************\n");
printf(" s_hat : %12.8f <%12.8f>\n", cabsf(s_hat), cargf(s_hat));
printf(" tau_hat : %12.8f\n", tau_hat);
// new timing offset estimate
tau_hat = cargf(_q->s0a_hat + _q->s0b_hat) * 16.0f / (2*M_PI);
printf(" tau_hat * : %12.8f\n", tau_hat);
#endif
#if 0
// compute carrier frequency offset estimate using ML method
float complex t0 = 0.0f;
for (i=0; i<48; i++) {
t0 += conjf(rc[i]) * wlanframe_s0[i] *
rc[i+16] * conjf(wlanframe_s0[i+16]);
}
float nu_hat = cargf(t0) / (float)(_q->M2);
#else
// compute carrier frequency offset estimate using freq. domain method
float nu_hat = wlanframesync_estimate_cfo_S0(_q->G0a, _q->G0b);
#endif
// set NCO frequency
nco_crcf_set_frequency(_q->nco_rx, nu_hat);
#if DEBUG_WLANFRAMESYNC_PRINT
printf(" nu_hat[0]: %12.8f\n", nu_hat);
#endif
// set state
_q->state = WLANFRAMESYNC_STATE_RXLONG0;
}
// estimate complex equalizer gain from G0 and G1
// _q : ofdmframesync object
// _ntaps : number of time-domain taps for smoothing
void ofdmframesync_estimate_eqgain(ofdmframesync _q,
unsigned int _ntaps)
{
#if DEBUG_OFDMFRAMESYNC
if (_q->debug_enabled) {
// copy pre-smoothed gain
memmove(_q->G_hat, _q->G, _q->M*sizeof(float complex));
}
#endif
// validate input
if (_ntaps == 0 || _ntaps > _q->M) {
fprintf(stderr, "error: ofdmframesync_estimate_eqgain(), ntaps must be in [1,M]\n");
exit(1);
}
unsigned int i;
// generate smoothing window (fft of temporal window)
for (i=0; i<_q->M; i++)
_q->x[i] = (i < _ntaps) ? 1.0f : 0.0f;
FFT_EXECUTE(_q->fft);
memmove(_q->G0, _q->G, _q->M*sizeof(float complex));
// smooth complex equalizer gains
for (i=0; i<_q->M; i++) {
// set gain to zero for null subcarriers
if (_q->p[i] == OFDMFRAME_SCTYPE_NULL) {
_q->G[i] = 0.0f;
continue;
}
float complex w;
float complex w0 = 0.0f;
float complex G_hat = 0.0f;
unsigned int j;
for (j=0; j<_q->M; j++) {
if (_q->p[j] == OFDMFRAME_SCTYPE_NULL) continue;
// select window sample from array
w = _q->X[(i + _q->M - j) % _q->M];
// accumulate gain
//G_hat += w * 0.5f * (_q->G0[j] + _q->G1[j]);
G_hat += w * _q->G0[j];
w0 += w;
}
// eliminate divide-by-zero issues
if (cabsf(w0) < 1e-4f) {
fprintf(stderr,"error: ofdmframesync_estimate_eqgain(), weighting factor is zero\n");
w0 = 1.0f;
}
_q->G[i] = G_hat / w0;
}
}
int
main (void)
{
/* For each type, test both runtime and compile time (constant folding)
optimization. */
float _Complex fc = 3.0F + 4.0iF;
double _Complex dc = 3.0 + 4.0i;
long double _Complex ldc = 3.0L + 4.0iL;
#ifdef HAVE_C99_RUNTIME
/* Test floats. */
if (cabsf (fc) != 5.0F)
link_error ();
if (__builtin_cabsf (fc) != 5.0F)
link_error ();
if (cabsf (3.0F + 4.0iF) != 5.0F)
link_error ();
if (__builtin_cabsf (3.0F + 4.0iF) != 5.0F)
link_error ();
#endif
/* Test doubles. */
if (cabs (dc) != 5.0)
link_error ();
if (__builtin_cabs (dc) != 5.0)
link_error ();
if (cabs (3.0 + 4.0i) != 5.0)
link_error ();
if (__builtin_cabs (3.0 + 4.0i) != 5.0)
link_error ();
#ifdef HAVE_C99_RUNTIME
/* Test long doubles. */
if (cabsl (ldc) != 5.0L)
link_error ();
if (__builtin_cabsl (ldc) != 5.0L)
link_error ();
if (cabsl (3.0L + 4.0iL) != 5.0L)
link_error ();
if (__builtin_cabsl (3.0L + 4.0iL) != 5.0L)
link_error ();
#endif
return 0;
}
void ofdmoqamframe64sync_execute_plcplong0(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 > 10*(_q->num_subcarriers)) {
#if DEBUG_OFDMOQAMFRAME64SYNC_PRINT
printf("warning: ofdmoqamframe64sync could not find first PLCP long sequence; resetting synchronizer\n");
#endif
ofdmoqamframe64sync_reset(_q);
return;
}
if (cabsf(rxy) > (_q->rxy_thresh)*(_q->num_subcarriers)) {
#if DEBUG_OFDMOQAMFRAME64SYNC_PRINT
printf("rxy[0] : %12.8f at input[%3u]\n", cabsf(rxy), _q->num_samples);
#endif
// run analyzers
//firpfbch_analyzer_run(_q->ca0, _q->S1a);
//firpfbch_analyzer_run(_q->ca1, _q->S1b);
_q->sample_phase = (_q->num_samples + (_q->num_subcarriers)/2) % _q->num_subcarriers;
//_q->sample_phase = (_q->num_samples) % _q->num_subcarriers;
#if DEBUG_OFDMOQAMFRAME64SYNC_PRINT
printf("sample phase : %u\n",_q->sample_phase);
#endif
//
float complex * rc;
windowcf_read(_q->input_buffer, &rc);
memmove(_q->S1a, rc, 64*sizeof(float complex));
_q->state = OFDMOQAMFRAME64SYNC_STATE_PLCPLONG1;
_q->timer = 0;
}
}
void liquid_doc_compute_psdcf(float complex * _x,
unsigned int _n,
float complex * _X,
unsigned int _nfft,
liquid_doc_psdwindow _wtype,
int _normalize)
{
unsigned int i;
// compute window and norm
float w[_n];
float wnorm=0.0f;
for (i=0; i<_n; i++) {
switch (_wtype) {
case LIQUID_DOC_PSDWINDOW_NONE: w[i] = 1.0f; break;
case LIQUID_DOC_PSDWINDOW_HANN: w[i] = hann(i,_n); break;
case LIQUID_DOC_PSDWINDOW_HAMMING: w[i] = hamming(i,_n); break;
break;
default:
fprintf(stderr,"error: liquid_doc_compute_psd(), invalid window type\n");
exit(1);
}
wnorm += w[i];
}
wnorm /= (float)(_n);
float complex x[_nfft];
fftplan fft = fft_create_plan(_nfft,x,_X,FFT_FORWARD,0);
for (i=0; i<_nfft; i++) {
x[i] = i < _n ? _x[i] * w[i] / wnorm : 0.0f;
}
fft_execute(fft);
fft_destroy_plan(fft);
// normalize spectrum by maximum
if (_normalize) {
float X_max = 0.0f;
for (i=0; i<_nfft; i++)
X_max = cabsf(_X[i]) > X_max ? cabsf(_X[i]) : X_max;
for (i=0; i<_nfft; i++)
_X[i] /= X_max;
}
}
/*
* Nuclear norm calculation for arbitrary block sizes
*/
float lrnucnorm(const struct operator_p_s* op, const complex float* src)
{
struct lrthresh_data_s* data = (struct lrthresh_data_s*)operator_p_get_data(op);
long strs1[DIMS];
md_calc_strides(DIMS, strs1, data->dims_decom, 1);
float nnorm = 0.;
for (int l = 0; l < data->levels; l++) {
const complex float* srcl = src + l * strs1[LEVEL_DIM];
// Initialize
long blkdims[DIMS];
long blksize = 1;
for (unsigned int i = 0; i < DIMS; i++) {
blkdims[i] = data->blkdims[l][i];
blksize *= blkdims[i];
}
// Special case if blocksize is 1
if (blksize == 1) {
for (long j = 0; j < md_calc_size(DIMS, data->dims); j++)
nnorm += 2 * cabsf(srcl[j]);
continue;
}
// Initialize data
struct svthresh_blockproc_data* svdata = svthresh_blockproc_create(data->mflags, 0., 0);
// Initialize tmp
complex float* tmp;
#ifdef USE_CUDA
tmp = (data->use_gpu ? md_alloc_gpu : md_alloc)(DIMS, data->dims, CFL_SIZE);
#else
tmp = md_alloc(DIMS, data->dims, CFL_SIZE);
#endif
// Copy to tmp
//debug_print_dims(DP_DEBUG1, DIMS, data->dims);
md_copy(DIMS, data->dims, tmp, srcl, CFL_SIZE);
// Block SVD Threshold
nnorm = blockproc(DIMS, data->dims, blkdims, (void*)svdata, nucnorm_blockproc, tmp, tmp);
// Free tmp
free(svdata);
md_free(tmp);
}
return nnorm;
}
static void zsoftthresh(long N, float lambda, complex float* d, const complex float* x)
{
for (long i = 0; i < N; i++) {
float norm = cabsf(x[i]);
float red = norm - lambda;
d[i] = (red > 0.) ? (red / norm) * x[i]: 0.;
}
}
/**
* Compute l1 norm of complex vec
*
* @param N vector length
* @param vec vector
*/
static double zl1norm(long N, const complex float* vec)
{
double res = 0.;
for (long i = 0; i < N; i++)
res += cabsf(vec[i]);
return res;
}
