// Compute spectral response
void liquid_doc_freqz(float * _b,
unsigned int _nb,
float * _a,
unsigned int _na,
unsigned int _nfft,
float complex * _H)
{
unsigned int i;
float complex x[_nfft];
float complex B[_nfft];
float complex A[_nfft];
float complex X[_nfft];
fftplan fft = fft_create_plan(_nfft,x,X,FFT_FORWARD,0);
// numerator
for (i=0; i<_nfft; i++)
x[i] = i < _nb ? _b[i] : 0.0f;
fft_execute(fft);
memmove(B,X,_nfft*sizeof(float complex));
// denominator
for (i=0; i<_nfft; i++)
x[i] = i < _na ? _a[i] : 0.0f;
fft_execute(fft);
memmove(A,X,_nfft*sizeof(float complex));
fft_destroy_plan(fft);
for (i=0; i<_nfft; i++)
X[i] = B[i] / A[i];
memmove(_H, X, _nfft*sizeof(float complex));
}
// autotest helper function
void fft_r2r_test(float * _x,
float * _test,
unsigned int _n,
unsigned int _kind)
{
int _flags = 0;
float tol=1e-4f;
unsigned int i;
float y[_n];
// compute real even/odd FFT
fftplan q = fft_create_plan_r2r_1d(_n, _x, y, _kind, _flags);
fft_execute(q);
// print results
if (liquid_autotest_verbose) {
printf("%12s %12s\n", "expected", "actual");
for (i=0; i<_n; i++)
printf("%12.8f %12.8f\n", _test[i], y[i]);
}
// validate results
for (i=0; i<_n; i++)
CONTEND_DELTA( y[i], _test[i], tol);
// destroy plans
fft_destroy_plan(q);
}
void processSound() {
uint8_t i, x, L, *data, nBins, binNum;
uint16_t minLvl, maxLvl;
int level, sum;
fft_input(capture, bfly_buff); // Samples -> complex #s
samplePos = 0; // Reset sample counter
ADCSRA |= _BV(ADIE); // Resume sampling interrupt
fft_execute(bfly_buff); // Process complex data
fft_output(bfly_buff, spectrum); // Complex -> spectrum
// Remove noise and apply EQ levels
for (x=0; x < FFT_N / 2; x++) {
L = pgm_read_byte(&noise[x]);
spectrum[x] = (spectrum[x] <= L) ? 0 :
(((spectrum[x] - L) * (256L - pgm_read_byte(&eq[x]))) >> 8);
}
colCount = (colCount + 1) % 10;
// Downsample spectrum output to 8 columns:
for(x = 0; x < NUM_COLUMNS; x++) {
data = (uint8_t *)pgm_read_word(&colData[x]);
nBins = pgm_read_byte(&data[0]);
binNum = pgm_read_byte(&data[1]);
for(sum = 0, i = 0; i < nBins; i++)
sum += spectrum[binNum++] * pgm_read_byte(&data[i + 2]); // Weighted
col[x][colCount] = sum / colDiv[x]; // Average
minLvl = maxLvl = col[x][0];
for(i = 1; i < 10; i++) { // Get range of prior 10 frames
if(col[x][i] < minLvl) minLvl = col[x][i];
else if(col[x][i] > maxLvl) maxLvl = col[x][i];
}
// minLvl and maxLvl indicate the extents of the FFT output, used
// for vertically scaling the output graph (so it looks interesting
// regardless of volume level). If they're too close together though
// (e.g. at very low volume levels) the graph becomes super coarse
// and 'jumpy'...so keep some minimum distance between them (this
// also lets the graph go to zero when no sound is playing):
if((maxLvl - minLvl) < 8) maxLvl = minLvl + 8;
minLvlAvg[x] = (minLvlAvg[x] * 7 + minLvl) >> 3; // Dampen min/max levels
maxLvlAvg[x] = (maxLvlAvg[x] * 7 + maxLvl) >> 3; // (fake rolling average)
// Second fixed-point scale based on dynamic min/max levels:
level = 10L * (col[x][colCount] - minLvlAvg[x]) /
(long)(maxLvlAvg[x] - minLvlAvg[x]);
// Clip output and convert to byte:
if(level < 0L) colLeveled[x] = 0;
else if(level > 10) colLeveled[x] = 10; // Allow dot to go a couple pixels off top
else colLeveled[x] = (uint8_t)level;
// XXX - The leveled columns could probably be improved
}
}
int main (void)
{
char *cp;
uint16_t m, n, s;
uint16_t t1,t2,t3;
DDRE = 0b00000010; /* PE1:<conout>, PE0:<conin> in N81 38.4kbps */
TCCR1B = 3; /* clk/64 */
xmitstr(PSTR("\r\nFFT sample program\r\n"));
for(;;) {
xmitstr(PSTR("\r\n>")); /* Prompt */
rcvrstr(pool, sizeof(pool)); /* Console input */
cp = pool;
switch (*cp++) { /* Pick a header char (command) */
case '\0' : /* Blank line */
break;
case 'w' : /* w: show waveform */
capture_wave(capture, FFT_N);
for (n = 0; n < FFT_N; n++) {
s = capture[n];
xmitf(PSTR("\r\n%4u:%6d "), n, s);
s = (s + 32768) / 1024;
for (m = 0; m < s; m++) xmit(' ');
xmit('*');
}
break;
case 's' : /* s: show spectrum */
capture_wave(capture, FFT_N);
TCNT1 = 0; /* performance counter */
fft_input(capture, bfly_buff);
t1 = TCNT1; TCNT1 = 0;
fft_execute(bfly_buff);
t2 = TCNT1; TCNT1 = 0;
fft_output(bfly_buff, spektrum);
t3 = TCNT1;
for (n = 0; n < FFT_N / 2; n++) {
s = spektrum[n];
xmitf(PSTR("\r\n%4u:%5u "), n, s);
s /= 512;
for (m = 0; m < s; m++) xmit('*');
}
xmitf(PSTR("\r\ninput=%u, execute=%u, output=%u (x64clk)"), t1,t2,t3);
break;
default : /* Unknown command */
xmitstr(PSTR("\n???"));
}
}
}
void GetMinimumPhaseSpectrum(const MinimumPhaseAnalysis *minimum_phase) {
// Mirroring
for (int i = minimum_phase->fft_size / 2 + 1;
i < minimum_phase->fft_size; ++i)
minimum_phase->log_spectrum[i] =
minimum_phase->log_spectrum[minimum_phase->fft_size - i];
// This fft_plan carries out "forward" FFT.
// To carriy out the Inverse FFT, the sign of imaginary part
// is inverted after FFT.
fft_execute(minimum_phase->inverse_fft);
minimum_phase->cepstrum[0][1] *= -1.0;
for (int i = 1; i < minimum_phase->fft_size / 2; ++i) {
minimum_phase->cepstrum[i][0] *= 2.0;
minimum_phase->cepstrum[i][1] *= -2.0;
}
minimum_phase->cepstrum[minimum_phase->fft_size / 2][1] *= -1.0;
for (int i = minimum_phase->fft_size / 2 + 1;
i < minimum_phase->fft_size; ++i) {
minimum_phase->cepstrum[i][0] = 0.0;
minimum_phase->cepstrum[i][1] = 0.0;
}
fft_execute(minimum_phase->forward_fft);
// Since x is complex number, calculation of exp(x) is as following.
// Note: This FFT library does not keep the aliasing.
double tmp;
for (int i = 0; i <= minimum_phase->fft_size / 2; ++i) {
tmp = exp(minimum_phase->minimum_phase_spectrum[i][0] /
minimum_phase->fft_size);
minimum_phase->minimum_phase_spectrum[i][0] = tmp *
cos(minimum_phase->minimum_phase_spectrum[i][1] /
minimum_phase->fft_size);
minimum_phase->minimum_phase_spectrum[i][1] = tmp *
sin(minimum_phase->minimum_phase_spectrum[i][1] /
minimum_phase->fft_size);
}
}
// Helper function to keep code base small
void fft_runbench(struct rusage * _start,
struct rusage * _finish,
unsigned long int * _num_iterations,
unsigned int _nfft,
int _direction)
{
// initialize arrays, plan
float complex * x = (float complex *) malloc(_nfft*sizeof(float complex));
float complex * y = (float complex *) malloc(_nfft*sizeof(float complex));
int _method = 0;
fftplan q = fft_create_plan(_nfft, x, y, _direction, _method);
unsigned long int i;
// initialize input with random values
for (i=0; i<_nfft; i++)
x[i] = randnf() + randnf()*_Complex_I;
// scale number of iterations to keep execution time
// relatively linear
*_num_iterations /= _nfft;
// start trials
getrusage(RUSAGE_SELF, _start);
for (i=0; i<(*_num_iterations); i++) {
fft_execute(q);
fft_execute(q);
fft_execute(q);
fft_execute(q);
}
getrusage(RUSAGE_SELF, _finish);
*_num_iterations *= 4;
fft_destroy_plan(q);
free(x);
free(y);
}
// Helper function to keep code base small
void fft_r2r_bench(struct rusage *_start,
struct rusage *_finish,
unsigned long int *_num_iterations,
unsigned int _n,
int _kind)
{
// initialize arrays, plan
float x[_n], y[_n];
int _flags = 0;
fftplan p = fft_create_plan_r2r_1d(_n, x, y, _kind, _flags);
unsigned long int i;
// initialize input with random values
for (i=0; i<_n; i++)
x[i] = randnf();
// scale number of iterations to keep execution time
// relatively linear
*_num_iterations /= _n * _n;
*_num_iterations *= 10;
*_num_iterations += 1;
// start trials
getrusage(RUSAGE_SELF, _start);
for (i=0; i<(*_num_iterations); i++) {
fft_execute(p);
fft_execute(p);
fft_execute(p);
fft_execute(p);
}
getrusage(RUSAGE_SELF, _finish);
*_num_iterations *= 4;
fft_destroy_plan(p);
}
// autotest helper function
// _x : fft input array
// _test : expected fft output
// _n : fft size
void fft_test(float complex * _x,
float complex * _test,
unsigned int _n)
{
int _method = 0;
float tol=2e-4f;
unsigned int i;
float complex y[_n], z[_n];
// compute FFT
fftplan pf = fft_create_plan(_n, _x, y, LIQUID_FFT_FORWARD, _method);
fft_execute(pf);
// compute IFFT
fftplan pr = fft_create_plan(_n, y, z, LIQUID_FFT_BACKWARD, _method);
fft_execute(pr);
// normalize inverse
for (i=0; i<_n; i++)
z[i] /= (float) _n;
// validate results
float fft_error, ifft_error;
for (i=0; i<_n; i++) {
fft_error = cabsf( y[i] - _test[i] );
ifft_error = cabsf( _x[i] - z[i] );
CONTEND_DELTA( fft_error, 0, tol);
CONTEND_DELTA( ifft_error, 0, tol);
}
// destroy plans
fft_destroy_plan(pf);
fft_destroy_plan(pr);
}
void apply_fir_fft_cc(FFT_PLAN_T* plan, FFT_PLAN_T* plan_inverse, complexf* taps_fft, complexf* last_overlap, int overlap_size)
{
//use the overlap & add method for filtering
//calculate FFT on input buffer
fft_execute(plan);
//multiply the filter and the input
complexf* in = plan->output;
complexf* out = plan_inverse->input;
for(int i=0;i<plan->size;i++) //@apply_fir_fft_cc: multiplication
{
iof(out,i)=iof(in,i)*iof(taps_fft,i)-qof(in,i)*qof(taps_fft,i);
qof(out,i)=iof(in,i)*qof(taps_fft,i)+qof(in,i)*iof(taps_fft,i);
}
//calculate inverse FFT on multiplied buffer
fft_execute(plan_inverse);
//add the overlap of the previous segment
complexf* result = plan_inverse->output;
for(int i=0;i<plan->size;i++) //@apply_fir_fft_cc: normalize by fft_size
{
iof(result,i)/=plan->size;
qof(result,i)/=plan->size;
}
for(int i=0;i<overlap_size;i++) //@apply_fir_fft_cc: add overlap
{
iof(result,i)=iof(result,i)+iof(last_overlap,i);
qof(result,i)=qof(result,i)+qof(last_overlap,i);
}
}
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;
}
}
int main (void)
{
initTimer();
initLeds();
loggerInit();
loggerWriteToMarker((LogMesT)"\r\nFFT sample program\r\n*", '*');
loggerWriteToMarker((LogMesT)"\r\n>*", '*'); /* Prompt */
for(;;) {
capture_wave(capture, FFT_N);
fft_input(capture, bfly_buff);
fft_execute(bfly_buff);
fft_output(bfly_buff, spektrum);
_delay_ms(50);
}
}
void do_spectrum() {
std::complex<float> *data_in;
windowcf_read(w, &data_in);
fftplan q = fft_create_plan(SPECTRUM_FFT_LENGTH,data_in,spectrum_fft_output,LIQUID_FFT_FORWARD,0);
fft_execute(q);
fft_destroy_plan(q);
spectrum_fft_magnitude[0] = 0.0; //Do not consider DC comp
int maxIdx = 0;
float maxVal = 0;
double total_power = 0;
double psd = 0;
for (int i = 1; i < SPECTRUM_FFT_LENGTH/2; i++) {
spectrum_fft_magnitude[i] = std::norm(spectrum_fft_output[i]);
total_power += spectrum_fft_magnitude[i];
if (spectrum_fft_magnitude[i] > maxVal) {
maxIdx = i;
maxVal = spectrum_fft_magnitude[i];
}
}
// TODO normalize so window size does not affect this threshold.
psd = maxVal / total_power;
if (shm_settings.auto_magnitude_thresholding == 0) {
if (psd > shm_settings.spectrum_magnitude_threshold && (current_sample_count - old_spectrum_sample_count > shm_settings.spectrum_cooldown_samples)) {
shm_results_spectrum.most_recent_ping_magnitude = maxVal;
shm_results_spectrum.most_recent_ping_frequency = (double) SAMPLING_FREQUENCY / (double) SPECTRUM_FFT_LENGTH * (double) maxIdx;
shm_results_spectrum.most_recent_ping_count++;
old_spectrum_sample_count = current_sample_count;
shm_setg(hydrophones_results_spectrum, shm_results_spectrum);
}
prev_psd = psd;
}
else {
//IMPLEMENTE AUTO THRESHOLDING AND ASSOCIATED PING STUFF
}
}
void WorldSynthesis::synthesize(double *dst, int length, int fftLength, const double *spectrum, const double *residual)
{
// 長さが違うときはバッファ再生成
if(fftLength!= _fftLength)
{
_setFftLength(fftLength);
}
// エルミート対称なので半分でよろしい.
int lastIndex = fftLength / 2;
// パワースペクトル→最小位相応答スペクトル
for (int i = 0; i <= lastIndex; i++)
{
_minimumPhase->log_spectrum[i] = log(spectrum[i]) / 2.0;
}
GetMinimumPhaseSpectrum(_minimumPhase);
// 最小位相応答と励起信号を畳み込みする.
fft_complex *minimum = _minimumPhase->minimum_phase_spectrum;
_spectrum[0][0] = minimum[0][0] * residual[0];
_spectrum[0][1] = 0.0;
for(int i = 1; i < lastIndex; i++)
{
_spectrum[i][0] = minimum[i][0] * residual[i*2-1] - minimum[i][1] * residual[i*2];
_spectrum[i][1] = minimum[i][1] * residual[i*2-1] + minimum[i][0] * residual[i*2];
}
_spectrum[lastIndex][0] = minimum[lastIndex][0] * residual[fftLength - 1];
_spectrum[lastIndex][1] = 0.0;
// 逆 FFT したら
fft_execute(*_plan);
// バッファに足しこんで終了.
for(int i = 0; i < _fftLength * 3 / 4 && i < length; i++) // ループ回数は本家に合わせた.
{
dst[i] += _impulse[i] / _fftLength; // FFT の補正項はここ.
}
}
int main(int argc, char*argv[])
{
// options
unsigned int num_channels=6; // number of channels (must be even)
unsigned int m=4; // filter delay
unsigned int num_symbols=4*m; // number of symbols
// validate input
if (num_channels%2) {
fprintf(stderr,"error: %s, number of channels must be even\n", argv[0]);
exit(1);
}
// derived values
unsigned int num_samples = num_channels * num_symbols;
unsigned int i;
unsigned int j;
// generate filter
// NOTE : these coefficients can be random; the purpose of this
// exercise is to demonstrate mathematical equivalence
#if 0
unsigned int h_len = 2*m*num_channels;
float h[h_len];
for (i=0; i<h_len; i++) h[i] = randnf();
#else
unsigned int h_len = 2*m*num_channels+1;
float h[h_len];
// NOTE: 81.29528 dB > beta = 8.00000 (6 channels, m=4)
liquid_firdes_kaiser(h_len, 1.0f/(float)num_channels, 81.29528f, 0.0f, h);
#endif
// normalize
float hsum = 0.0f;
for (i=0; i<h_len; i++) hsum += h[i];
for (i=0; i<h_len; i++) h[i] = h[i] * num_channels / hsum;
// sub-sampled filters for M=6 channels, m=4, beta=8.0
// -3.2069e-19 -6.7542e-04 -1.3201e-03 2.2878e-18 3.7613e-03 5.8033e-03
// -7.2899e-18 -1.2305e-02 -1.7147e-02 1.6510e-17 3.1187e-02 4.0974e-02
// -3.0032e-17 -6.8026e-02 -8.6399e-02 4.6273e-17 1.3732e-01 1.7307e-01
// -6.2097e-17 -2.8265e-01 -3.7403e-01 7.3699e-17 8.0663e-01 1.6438e+00
// 2.0001e+00 1.6438e+00 8.0663e-01 7.3699e-17 -3.7403e-01 -2.8265e-01
// -6.2097e-17 1.7307e-01 1.3732e-01 4.6273e-17 -8.6399e-02 -6.8026e-02
// -3.0032e-17 4.0974e-02 3.1187e-02 1.6510e-17 -1.7147e-02 -1.2305e-02
// -7.2899e-18 5.8033e-03 3.7613e-03 2.2878e-18 -1.3201e-03 -6.7542e-04
// create filterbank manually
dotprod_crcf dp[num_channels]; // vector dot products
windowcf w[num_channels]; // window buffers
#if DEBUG
// print coefficients
printf("h_prototype:\n");
for (i=0; i<h_len; i++)
printf(" h[%3u] = %12.8f\n", i, h[i]);
#endif
// create objects
unsigned int h_sub_len = 2*m;
float h_sub[h_sub_len];
for (i=0; i<num_channels; i++) {
// sub-sample prototype filter
#if 0
for (j=0; j<h_sub_len; j++)
h_sub[j] = h[j*num_channels+i];
#else
// load coefficients in reverse order
for (j=0; j<h_sub_len; j++)
h_sub[h_sub_len-j-1] = h[j*num_channels+i];
#endif
// create window buffer and dotprod objects
dp[i] = dotprod_crcf_create(h_sub, h_sub_len);
w[i] = windowcf_create(h_sub_len);
#if DEBUG
printf("h_sub[%u] : \n", i);
for (j=0; j<h_sub_len; j++)
printf(" h[%3u] = %12.8f\n", j, h_sub[j]);
#endif
}
// generate DFT object
float complex x[num_channels]; // time-domain buffer
float complex X[num_channels]; // freq-domain buffer
#if 1
fftplan fft = fft_create_plan(num_channels, X, x, LIQUID_FFT_BACKWARD, 0);
#else
fftplan fft = fft_create_plan(num_channels, X, x, LIQUID_FFT_FORWARD, 0);
#endif
float complex y[num_samples]; // time-domain input
float complex Y0[2*num_symbols][num_channels]; // channelizer output
float complex Y1[2*num_symbols][num_channels]; // conventional output
// generate input sequence
for (i=0; i<num_samples; i++) {
//y[i] = randnf() * cexpf(_Complex_I*randf()*2*M_PI);
y[i] = (i==0) ? 1.0f : 0.0f;
y[i] = cexpf(_Complex_I*sqrtf(2.0f)*i*i);
printf("y[%3u] = %12.8f + %12.8fj\n", i, crealf(y[i]), cimagf(y[i]));
}
//
// run analysis filter bank
//
#if 0
unsigned int filter_index = 0;
#else
unsigned int filter_index = num_channels/2-1;
#endif
float complex y_hat; // input sample
float complex * r; // buffer read pointer
int toggle = 0; // flag indicating buffer/filter alignment
//
for (i=0; i<2*num_symbols; i++) {
// load buffers in blocks of num_channels/2
for (j=0; j<num_channels/2; j++) {
// grab sample
y_hat = y[i*num_channels/2 + j];
// push sample into buffer at filter index
windowcf_push(w[filter_index], y_hat);
// decrement filter index
filter_index = (filter_index + num_channels - 1) % num_channels;
//filter_index = (filter_index + 1) % num_channels;
}
// execute filter outputs
// reversing order of output (not sure why this is necessary)
unsigned int offset = toggle ? num_channels/2 : 0;
toggle = 1-toggle;
for (j=0; j<num_channels; j++) {
unsigned int buffer_index = (offset+j)%num_channels;
unsigned int dotprod_index = j;
windowcf_read(w[buffer_index], &r);
//dotprod_crcf_execute(dp[dotprod_index], r, &X[num_channels-j-1]);
dotprod_crcf_execute(dp[dotprod_index], r, &X[buffer_index]);
}
printf("***** i = %u\n", i);
for (j=0; j<num_channels; j++)
printf(" v2[%4u] = %12.8f + %12.8fj\n", j, crealf(X[j]), cimagf(X[j]));
// execute DFT, store result in buffer 'x'
fft_execute(fft);
// scale fft output
for (j=0; j<num_channels; j++)
x[j] *= 1.0f / (num_channels);
// move to output array
for (j=0; j<num_channels; j++)
Y0[i][j] = x[j];
}
// destroy objects
for (i=0; i<num_channels; i++) {
dotprod_crcf_destroy(dp[i]);
windowcf_destroy(w[i]);
}
fft_destroy_plan(fft);
//
// run traditional down-converter (inefficient)
//
// generate filter object
firfilt_crcf f = firfilt_crcf_create(h, h_len);
float dphi; // carrier frequency
unsigned int n=0;
for (i=0; i<num_channels; i++) {
// reset filter
firfilt_crcf_clear(f);
// set center frequency
dphi = 2.0f * M_PI * (float)i / (float)num_channels;
// reset symbol counter
n=0;
for (j=0; j<num_samples; j++) {
// push down-converted sample into filter
firfilt_crcf_push(f, y[j]*cexpf(-_Complex_I*j*dphi));
// compute output at the appropriate sample time
assert(n<2*num_symbols);
if ( ((j+1)%(num_channels/2))==0 ) {
firfilt_crcf_execute(f, &Y1[n][i]);
n++;
}
}
assert(n==2*num_symbols);
}
firfilt_crcf_destroy(f);
// print filterbank channelizer
printf("\n");
printf("filterbank channelizer:\n");
for (i=0; i<2*num_symbols; i++) {
printf("%2u:", i);
for (j=0; j<num_channels; j++) {
printf("%6.3f+%6.3fj, ", crealf(Y0[i][j]), cimagf(Y0[i][j]));
}
printf("\n");
}
#if 0
// print traditional channelizer
printf("\n");
printf("traditional channelizer:\n");
for (i=0; i<2*num_symbols; i++) {
printf("%2u:", i);
for (j=0; j<num_channels; j++) {
printf("%6.3f+%6.3fj, ", crealf(Y1[i][j]), cimagf(Y1[i][j]));
}
printf("\n");
}
//
// compare results
//
float mse[num_channels];
float complex d;
for (i=0; i<num_channels; i++) {
mse[i] = 0.0f;
for (j=0; j<2*num_symbols; j++) {
d = Y0[j][i] - Y1[j][i];
mse[i] += crealf(d*conjf(d));
}
mse[i] /= num_symbols;
}
printf("\n");
printf(" e:");
for (i=0; i<num_channels; i++)
printf("%12.4e ", sqrt(mse[i]));
printf("\n");
#endif
printf("done.\n");
return 0;
}
int main(void)
{
memset( spectrum, 0, sizeof(spectrum) );
memset( spectrum_history, 0, sizeof(spectrum) );
init();
uint16_t cycles_till_reset_x = LCD_RESET_ADDR_CYCLES;
while(1)
{
if( sleeping )
{
sleepcycles++;
if( backlight_task_scaler++ >= 75 )
{
backlight_task(-1);
backlight_task_scaler = 0;
}
if( sleeping == 3 )
go_to_sleep();
else if( sleeping == 2 )
wake_up();
else // sleeping == 1
{
SMCR = _BV(SM0) | _BV(SE);
asm volatile( "sleep\n\t" );
}
}
else
{
backlight_task_scaler = 0;
backlight_task(-1);
// Apply window function and store in butterfly array
fft_input( capture, bfly );
// Execute forier transform
fft_execute( bfly );
// Bit reversal algorithm from butterfly array to output array
fft_output( bfly, spectrum );
// Do exponential/FIR filtering with history data
exp_average( spectrum, spectrum_history );
#ifdef DISPLAY_TEST_PATTERN
uint8_t *sp = spectrum;
uint8_t v = 5;
uint8_t k = sizeof(spectrum)/sizeof(uint8_t);
while( k-- )
{
*sp = v;
v++;
if( v > 38 )
v = 5;
sp++;
}
long t = 100000;
while(t--)
{
asm volatile("nop");
}
#else
#ifdef BLANK_LEFT_TWO_BARS
spectrum[0] = spectrum[1] = 0;
#endif
#endif
avc_task( spectrum );
if( --cycles_till_reset_x <= 0 )
{
cycles_till_reset_x = LCD_RESET_ADDR_CYCLES;
lcd_write_instruction( LCD_ADDR | 0, CHIP1 );
lcd_write_instruction( LCD_ADDR | 0, CHIP2 );
}
fastlcd( spectrum );
loopnum++;
}
}
decimating_shift_addition_status_t fastddc_inv_cc(complexf* input, complexf* output, fastddc_t* ddc, FFT_PLAN_T* plan_inverse, complexf* taps_fft, decimating_shift_addition_status_t shift_stat)
{
//implements DDC by using the overlap & scrap method
//TODO: +/-1s on overlap_size et al
//input shoud have ddc->fft_size number of elements
complexf* inv_input = plan_inverse->input;
complexf* inv_output = plan_inverse->output;
//Initialize buffers for inverse FFT to zero
for(int i=0;i<plan_inverse->size;i++)
{
iof(inv_input,i)=0;
qof(inv_input,i)=0;
}
//Alias & shift & filter at once
fft_swap_sides(input, ddc->fft_size); //TODO this is not very optimal, but now we stick with this slow solution until we got the algorithm working
//fprintf(stderr, " === fastddc_inv_cc() ===\n");
//The problem is, we have to say that the output_index should be the _center_ of the spectrum when i is at startbin! (startbin is at the _center_ of the input to downconvert, not at its first bin!)
for(int i=0;i<ddc->fft_size;i++)
{
int output_index = (ddc->fft_size+i-ddc->offsetbin+(ddc->fft_inv_size/2))%plan_inverse->size;
int tap_index = i;
//fprintf(stderr, "output_index = %d , tap_index = %d, input index = %d\n", output_index, tap_index, i);
//cmultadd(inv_input+output_index, input+i, taps_fft+tap_index); //cmultadd(output, input1, input2): complex output += complex input1 * complex input 2
// (a+b*i)*(c+d*i) = (ac-bd)+(ad+bc)*i
// a = iof(input,i)
// b = qof(input,i)
// c = iof(taps_fft,i)
// d = qof(taps_fft,i)
iof(inv_input,output_index) += iof(input,i) * iof(taps_fft,i) - qof(input,i) * qof(taps_fft,i);
qof(inv_input,output_index) += iof(input,i) * qof(taps_fft,i) + qof(input,i) * iof(taps_fft,i);
//iof(inv_input,output_index) += iof(input,i); //no filter
//qof(inv_input,output_index) += qof(input,i);
}
//Normalize inv fft bins (now our output level is not higher than the input... but we may optimize this into the later loop when we normalize by size)
for(int i=0;i<plan_inverse->size;i++)
{
iof(inv_input,i)/=ddc->pre_decimation;
qof(inv_input,i)/=ddc->pre_decimation;
}
fft_swap_sides(inv_input,plan_inverse->size);
fft_execute(plan_inverse);
//Normalize data
for(int i=0;i<plan_inverse->size;i++) //@fastddc_inv_cc: normalize by size
{
iof(inv_output,i)/=plan_inverse->size;
qof(inv_output,i)/=plan_inverse->size;
}
//Overlap is scrapped, not added
//Shift correction
shift_stat=decimating_shift_addition_cc(inv_output+ddc->scrap, output, ddc->post_input_size, ddc->dsadata, ddc->post_decimation, shift_stat);
//shift_stat.output_size = ddc->post_input_size; //bypass shift correction
//memcpy(output, inv_output+ddc->scrap, sizeof(complexf)*ddc->post_input_size);
return shift_stat;
}
// create FFT-based FIR filter using external coefficients
// _h : filter coefficients [size: _h_len x 1]
// _h_len : filter length, _h_len > 0
// _n : block size = nfft/2, at least _h_len-1
FFTFILT() FFTFILT(_create)(TC * _h,
unsigned int _h_len,
unsigned int _n)
{
// validate input
if (_h_len == 0) {
fprintf(stderr,"error: fftfilt_%s_create(), filter length must be greater than zero\n",
EXTENSION_FULL);
exit(1);
} else if (_n < _h_len-1) {
fprintf(stderr,"error: fftfilt_%s_create(), block length must be greater than _h_len-1 (%u)\n",
EXTENSION_FULL,
_h_len-1);
exit(1);
}
// create filter object and initialize
FFTFILT() q = (FFTFILT()) malloc(sizeof(struct FFTFILT(_s)));
q->h_len = _h_len;
q->n = _n;
// copy filter coefficients
q->h = (TC *) malloc((q->h_len)*sizeof(TC));
memmove(q->h, _h, _h_len*sizeof(TC));
// allocate internal memory arrays
q->time_buf = (float complex *) malloc((2*q->n)* sizeof(float complex)); // time buffer
q->freq_buf = (float complex *) malloc((2*q->n)* sizeof(float complex)); // frequency buffer
q->H = (float complex *) malloc((2*q->n)* sizeof(float complex)); // FFT{ h }
q->w = (float complex *) malloc(( q->n)* sizeof(float complex)); // delay buffer
// create internal FFT objects
#ifdef LIQUID_FFTOVERRIDE
q->fft = fft_create_plan(2*q->n, q->time_buf, q->freq_buf, LIQUID_FFT_FORWARD, 0);
q->ifft = fft_create_plan(2*q->n, q->freq_buf, q->time_buf, LIQUID_FFT_BACKWARD, 0);
#else
q->fft = FFT_CREATE_PLAN(2*q->n, q->time_buf, q->freq_buf, FFT_DIR_FORWARD, FFT_METHOD);
q->ifft = FFT_CREATE_PLAN(2*q->n, q->freq_buf, q->time_buf, FFT_DIR_BACKWARD, FFT_METHOD);
#endif
// compute FFT of filter coefficients and copy to internal H array
unsigned int i;
for (i=0; i<2*q->n; i++)
q->time_buf[i] = (i < q->h_len) ? q->h[i] : 0;
// time_buf > {FFT} > freq_buf
#ifdef LIQUID_FFTOVERRIDE
fft_execute(q->fft);
#else
FFT_EXECUTE(q->fft);
#endif
memmove(q->H, q->freq_buf, 2*q->n*sizeof(float complex));
// set default scaling
FFTFILT(_set_scale)(q, 1);
// reset filter state (clear buffer)
FFTFILT(_reset)(q);
// return object
return q;
}
int main(int argc, char*argv[])
{
// options
unsigned int m = 3; // number of bits/symbol
unsigned int k = 0; // filter samples/symbol
unsigned int num_symbols = 200; // number of data symbols
float SNRdB = 40.0f; // signal-to-noise ratio [dB]
float cfo = 0.0f; // carrier frequency offset
float cpo = 0.0f; // carrier phase offset
float tau = 0.0f; // fractional symbol timing offset
float bandwidth = 0.20; // frequency spacing
int dopt;
while ((dopt = getopt(argc,argv,"hm:k:b:n:s:F:P:T:")) != EOF) {
switch (dopt) {
case 'h': usage(); return 0;
case 'm': m = atoi(optarg); break;
case 'k': k = atoi(optarg); break;
case 'b': bandwidth = atof(optarg); break;
case 'n': num_symbols = atoi(optarg); break;
case 's': SNRdB = atof(optarg); break;
case 'F': cfo = atof(optarg); break;
case 'P': cpo = atof(optarg); break;
case 'T': tau = atof(optarg); break;
default:
exit(1);
}
}
unsigned int i;
unsigned int j;
// derived values
if (k == 0) k = 2 << m; // set samples per symbol if not otherwise specified
unsigned int num_samples = k*num_symbols;
unsigned int M = 1 << m;
float nstd = powf(10.0f, -SNRdB/20.0f);
float M2 = 0.5f*(float)(M-1);
// validate input
if (k < M) {
fprintf(stderr,"errors: %s, samples/symbol must be at least modulation size (M=%u)\n", __FILE__,M);
exit(1);
} else if (k > 2048) {
fprintf(stderr,"errors: %s, samples/symbol exceeds maximum (2048)\n", __FILE__);
exit(1);
} else if (M > 1024) {
fprintf(stderr,"errors: %s, modulation size (M=%u) exceeds maximum (1024)\n", __FILE__, M);
exit(1);
} else if (bandwidth <= 0.0f || bandwidth >= 0.5f) {
fprintf(stderr,"errors: %s, bandwidht must be in (0,0.5)\n", __FILE__);
exit(1);
}
// compute demodulation FFT size such that FFT output bin frequencies are
// as close to modulated frequencies as possible
unsigned int K = 0; // demodulation FFT size
float df = bandwidth / M2; // frequency spacing
float err_min = 1e9f;
unsigned int K_min = k; // minimum FFT size
unsigned int K_max = k*4 < 16 ? 16 : k*4; // maximum FFT size
unsigned int K_hat;
for (K_hat=K_min; K_hat<=K_max; K_hat++) {
// compute candidate FFT size
float v = 0.5f*df * (float)K_hat; // bin spacing
float err = fabsf( roundf(v) - v ); // fractional bin spacing
// print results
printf(" K_hat = %4u : v = %12.8f, err=%12.8f %s\n", K_hat, v, err, err < err_min ? "*" : "");
// save best result
if (K_hat==K_min || err < err_min) {
K = K_hat;
err_min = err;
}
// perfect match; no need to continue searching
if (err < 1e-6f)
break;
}
// arrays
unsigned int sym_in[num_symbols]; // input symbols
float complex x[num_samples]; // transmitted signal
float complex y[num_samples]; // received signal
unsigned int sym_out[num_symbols]; // output symbols
// determine demodulation mapping between tones and frequency bins
// TODO: use gray coding
unsigned int demod_map[M];
for (i=0; i<M; i++) {
// print frequency bins
float freq = ((float)i - M2) * bandwidth / M2;
float idx = freq * (float)K;
unsigned int index = (unsigned int) (idx < 0 ? roundf(idx + K) : roundf(idx));
demod_map[i] = index;
printf(" s=%3u, f = %12.8f, index=%3u\n", i, freq, index);
}
// check for uniqueness
for (i=1; i<M; i++) {
if (demod_map[i] == demod_map[i-1]) {
fprintf(stderr,"warning: demod map is not unique; consider increasing bandwidth\n");
break;
}
}
// generate message symbols and modulate
// TODO: use gray coding
for (i=0; i<num_symbols; i++) {
// generate random symbol
sym_in[i] = rand() % M;
// compute frequency
float dphi = 2*M_PI*((float)sym_in[i] - M2) * bandwidth / M2;
// generate random phase
float phi = randf() * 2 * M_PI;
// modulate symbol
for (j=0; j<k; j++)
x[i*k+j] = cexpf(_Complex_I*phi + _Complex_I*j*dphi);
}
// push through channel
for (i=0; i<num_samples; i++)
y[i] = x[i] + nstd*(randnf() + _Complex_I*randnf())*M_SQRT1_2;
#if 0
// demodulate signal: high SNR method
float complex buf_time[k];
unsigned int n = 0;
j = 0;
for (i=0; i<num_samples; i++) {
// start filling time buffer with samples (assume perfect symbol timing)
buf_time[n++] = y[i];
// demodulate symbol
if (n==k) {
// reset counter
n = 0;
// estimate frequency
float complex metric = 0;
unsigned int s;
for (s=1; s<k; s++)
metric += buf_time[s] * conjf(buf_time[s-1]);
float dphi_hat = cargf(metric) / (2*M_PI);
unsigned int v=( (unsigned int) roundf(dphi_hat*M2/bandwidth + M2) ) % M;
sym_out[j++] = v;
printf("%3u : %12.8f : %u\n", j, dphi_hat, v);
}
}
#else
// demodulate signal: least-squares method
float complex buf_time[K];
float complex buf_freq[K];
fftplan fft = fft_create_plan(K, buf_time, buf_freq, LIQUID_FFT_FORWARD, 0);
for (i=0; i<K; i++)
buf_time[i] = 0.0f;
unsigned int n = 0;
j = 0;
for (i=0; i<num_samples; i++) {
// start filling time buffer with samples (assume perfect symbol timing)
buf_time[n++] = y[i];
// demodulate symbol
if (n==k) {
// reset counter
n = 0;
// compute transform, storing result in 'buf_freq'
fft_execute(fft);
// find maximum by looking at particular bins
float vmax = 0;
unsigned int s;
unsigned int s_opt = 0;
for (s=0; s<M; s++) {
float v = cabsf( buf_freq[demod_map[s]] );
if (s==0 || v > vmax) {
s_opt = s;
vmax =v;
}
}
// save best result
sym_out[j++] = s_opt;
}
}
// destroy fft object
fft_destroy_plan(fft);
#endif
// count errors
unsigned int num_symbol_errors = 0;
for (i=0; i<num_symbols; i++)
num_symbol_errors += (sym_in[i] == sym_out[i]) ? 0 : 1;
printf("symbol errors: %u / %u\n", num_symbol_errors, num_symbols);
// compute power spectral density of received signal
unsigned int nfft = 1200;
float psd[nfft];
spgramcf_estimate_psd(nfft, y, num_samples, psd);
//
// export results
//
// truncate to at most 10 symbols
if (num_symbols > 10)
num_symbols = 10;
num_samples = k*num_symbols;
FILE * fid = fopen(OUTPUT_FILENAME,"w");
fprintf(fid,"%% %s : auto-generated file\n", OUTPUT_FILENAME);
fprintf(fid,"clear all\n");
fprintf(fid,"close all\n");
fprintf(fid,"k = %u;\n", k);
fprintf(fid,"M = %u;\n", M);
fprintf(fid,"num_symbols = %u;\n", num_symbols);
fprintf(fid,"num_samples = %u;\n", num_samples);
fprintf(fid,"nfft = %u;\n", nfft);
fprintf(fid,"x = zeros(1,num_samples);\n");
fprintf(fid,"y = zeros(1,num_samples);\n");
for (i=0; i<num_samples; i++) {
fprintf(fid,"x(%4u) = %12.8f + j*%12.8f;\n", i+1, crealf(x[i]), cimagf(x[i]));
fprintf(fid,"y(%4u) = %12.8f + j*%12.8f;\n", i+1, crealf(y[i]), cimagf(y[i]));
}
// save power spectral density
fprintf(fid,"psd = zeros(1,nfft);\n");
for (i=0; i<nfft; i++)
fprintf(fid,"psd(%4u) = %12.8f;\n", i+1, psd[i]);
fprintf(fid,"t=[0:(num_samples-1)]/k;\n");
fprintf(fid,"i = 1:k:num_samples;\n");
fprintf(fid,"figure;\n");
// plot time signal
fprintf(fid,"subplot(2,1,1),\n");
fprintf(fid,"hold on;\n");
fprintf(fid," plot(t,real(y),'-', 'Color',[0 0.3 0.5]);\n");
fprintf(fid," plot(t,imag(y),'-', 'Color',[0 0.5 0.3]);\n");
fprintf(fid,"hold off;\n");
fprintf(fid,"ymax = ceil(max(abs(y))*5)/5;\n");
fprintf(fid,"axis([0 num_symbols -ymax ymax]);\n");
fprintf(fid,"xlabel('time');\n");
fprintf(fid,"ylabel('x(t)');\n");
fprintf(fid,"grid on;\n");
// plot PSD
fprintf(fid,"subplot(2,1,2),\n");
fprintf(fid,"f = [0:(nfft-1)]/nfft - 0.5;\n");
fprintf(fid,"plot(f,psd,'LineWidth',1.5,'Color',[0.5 0 0]);\n");
fprintf(fid,"axis([-0.5 0.5 -40 40]);\n");
fprintf(fid,"xlabel('Normalized Frequency [f/F_s]');\n");
fprintf(fid,"ylabel('PSD [dB]');\n");
fprintf(fid,"grid on;\n");
fclose(fid);
printf("results written to '%s'\n", OUTPUT_FILENAME);
return 0;
}
int main() {
// options
unsigned int num_channels=4; // number of channels
unsigned int m=5; // filter delay
unsigned int num_symbols=12; // number of symbols
// derived values
unsigned int num_samples = num_channels * num_symbols;
unsigned int i;
unsigned int j;
// generate filter
// NOTE : these coefficients can be random; the purpose of this
// exercise is to demonstrate mathematical equivalence
unsigned int h_len = 2*m*num_channels;
float h[h_len];
for (i=0; i<h_len; i++) h[i] = randnf();
//for (i=0; i<h_len; i++) h[i] = 0.1f*i;
//for (i=0; i<h_len; i++) h[i] = (i<=m) ? 1.0f : 0.0f;
//for (i=0; i<h_len; i++) h[i] = 1.0f;
// create filterbank manually
dotprod_crcf dp[num_channels]; // vector dot products
windowcf w[num_channels]; // window buffers
#if DEBUG
// print coefficients
printf("h_prototype:\n");
for (i=0; i<h_len; i++)
printf(" h[%3u] = %12.8f\n", i, h[i]);
#endif
// create objects
unsigned int h_sub_len = 2*m;
float h_sub[h_sub_len];
for (i=0; i<num_channels; i++) {
// sub-sample prototype filter, loading coefficients in
// reverse order
#if 0
for (j=0; j<h_sub_len; j++)
h_sub[j] = h[j*num_channels+i];
#else
for (j=0; j<h_sub_len; j++)
h_sub[h_sub_len-j-1] = h[j*num_channels+i];
#endif
// create window buffer and dotprod objects
dp[i] = dotprod_crcf_create(h_sub, h_sub_len);
w[i] = windowcf_create(h_sub_len);
#if DEBUG
printf("h_sub[%u] : \n", i);
for (j=0; j<h_sub_len; j++)
printf(" h[%3u] = %12.8f\n", j, h_sub[j]);
#endif
}
// generate DFT object
float complex x[num_channels]; // time-domain buffer
float complex X[num_channels]; // freq-domain buffer
#if 0
fftplan fft = fft_create_plan(num_channels, X, x, LIQUID_FFT_BACKWARD, 0);
#else
fftplan fft = fft_create_plan(num_channels, X, x, LIQUID_FFT_FORWARD, 0);
#endif
// generate filter object
firfilt_crcf f = firfilt_crcf_create(h, h_len);
float complex y[num_samples]; // time-domain input
float complex Y0[num_symbols][num_channels]; // channelized output
float complex Y1[num_symbols][num_channels]; // channelized output
// generate input sequence (complex noise)
for (i=0; i<num_samples; i++)
y[i] = randnf() * cexpf(_Complex_I*randf()*2*M_PI);
//
// run analysis filter bank
//
#if 0
unsigned int filter_index = 0;
#else
unsigned int filter_index = num_channels-1;
#endif
float complex y_hat; // input sample
float complex * r; // read pointer
for (i=0; i<num_symbols; i++) {
// load buffers
for (j=0; j<num_channels; j++) {
// grab sample
y_hat = y[i*num_channels + j];
// push sample into buffer at filter index
windowcf_push(w[filter_index], y_hat);
// decrement filter index
filter_index = (filter_index + num_channels - 1) % num_channels;
//filter_index = (filter_index + 1) % num_channels;
}
// execute filter outputs, reversing order of output (not
// sure why this is necessary)
for (j=0; j<num_channels; j++) {
windowcf_read(w[j], &r);
dotprod_crcf_execute(dp[j], r, &X[num_channels-j-1]);
}
// execute DFT, store result in buffer 'x'
fft_execute(fft);
// move to output array
for (j=0; j<num_channels; j++)
Y0[i][j] = x[j];
}
//
// run traditional down-converter (inefficient)
//
float dphi; // carrier frequency
unsigned int n=0;
for (i=0; i<num_channels; i++) {
// reset filter
firfilt_crcf_reset(f);
// set center frequency
dphi = 2.0f * M_PI * (float)i / (float)num_channels;
// reset symbol counter
n=0;
for (j=0; j<num_samples; j++) {
// push down-converted sample into filter
firfilt_crcf_push(f, y[j]*cexpf(-_Complex_I*j*dphi));
// compute output at the appropriate sample time
assert(n<num_symbols);
if ( ((j+1)%num_channels)==0 ) {
firfilt_crcf_execute(f, &Y1[n][i]);
n++;
}
}
assert(n==num_symbols);
}
// destroy objects
for (i=0; i<num_channels; i++) {
dotprod_crcf_destroy(dp[i]);
windowcf_destroy(w[i]);
}
fft_destroy_plan(fft);
firfilt_crcf_destroy(f);
// print filterbank channelizer
printf("\n");
printf("filterbank channelizer:\n");
for (i=0; i<num_symbols; i++) {
printf("%3u: ", i);
for (j=0; j<num_channels; j++) {
printf(" %8.5f+j%8.5f, ", crealf(Y0[i][j]), cimagf(Y0[i][j]));
}
printf("\n");
}
// print traditional channelizer
printf("\n");
printf("traditional channelizer:\n");
for (i=0; i<num_symbols; i++) {
printf("%3u: ", i);
for (j=0; j<num_channels; j++) {
printf(" %8.5f+j%8.5f, ", crealf(Y1[i][j]), cimagf(Y1[i][j]));
}
printf("\n");
}
//
// compare results
//
float mse[num_channels];
float complex d;
for (i=0; i<num_channels; i++) {
mse[i] = 0.0f;
for (j=0; j<num_symbols; j++) {
d = Y0[j][i] - Y1[j][i];
mse[i] += crealf(d*conjf(d));
}
mse[i] /= num_symbols;
}
printf("\n");
printf("rmse: ");
for (i=0; i<num_channels; i++)
printf("%12.4e ", sqrt(mse[i]));
printf("\n");
printf("done.\n");
return 0;
}
int main() {
// options
unsigned int num_channels=64; // must be even number
unsigned int num_symbols=16; // number of symbols
unsigned int m=3; // filter delay (symbols)
float beta = 0.9f; // filter excess bandwidth factor
float phi = 0.0f; // carrier phase offset;
float dphi = 0.04f; // carrier frequency offset
// number of frames (compensate for filter delay)
unsigned int num_frames = num_symbols + 2*m;
unsigned int num_samples = num_channels * num_frames;
unsigned int i;
unsigned int j;
// create filter prototype
unsigned int h_len = 2*num_channels*m + 1;
float h[h_len];
float complex hc[h_len];
float complex gc[h_len];
liquid_firdes_rkaiser(num_channels, m, beta, 0.0f, h);
unsigned int g_len = 2*num_channels*m;
for (i=0; i<g_len; i++) {
hc[i] = h[i];
gc[i] = h[g_len-i-1] * cexpf(_Complex_I*dphi*i);
}
// data arrays
float complex s[num_channels]; // input symbols
float complex y[num_samples]; // time-domain samples
float complex Y0[num_frames][num_channels]; // channelized output
float complex Y1[num_frames][num_channels]; // channelized output
// create ofdm/oqam generator object and generate data
ofdmoqam qs = ofdmoqam_create(num_channels, m, beta, 0.0f, LIQUID_SYNTHESIZER, 0);
for (i=0; i<num_frames; i++) {
for (j=0; j<num_channels; j++) {
if (i<num_symbols) {
#if 0
// QPSK on all subcarriers
s[j] = (rand() % 2 ? 1.0f : -1.0f) +
(rand() % 2 ? 1.0f : -1.0f) * _Complex_I;
s[j] *= 1.0f / sqrtf(2.0f);
#else
// BPSK on even subcarriers
s[j] = rand() % 2 ? 1.0f : -1.0f;
s[j] *= (j%2)==0 ? 1.0f : 0.0f;
#endif
} else {
s[j] = 0.0f;
}
}
// run synthesizer
ofdmoqam_execute(qs, s, &y[i*num_channels]);
}
ofdmoqam_destroy(qs);
// channel
for (i=0; i<num_samples; i++)
y[i] *= cexpf(_Complex_I*(phi + dphi*i));
//
// analysis filterbank (receiver)
//
// create filterbank manually
dotprod_cccf dp[num_channels]; // vector dot products
windowcf w[num_channels]; // window buffers
#if DEBUG
// print coefficients
printf("h_prototype:\n");
for (i=0; i<h_len; i++)
printf(" h[%3u] = %12.8f\n", i, h[i]);
#endif
// create objects
unsigned int gc_sub_len = 2*m;
float complex gc_sub[gc_sub_len];
for (i=0; i<num_channels; i++) {
// sub-sample prototype filter, loading coefficients in
// reverse order
#if 0
for (j=0; j<gc_sub_len; j++)
gc_sub[j] = h[j*num_channels+i];
#else
for (j=0; j<gc_sub_len; j++)
gc_sub[gc_sub_len-j-1] = gc[j*num_channels+i];
#endif
// create window buffer and dotprod objects
dp[i] = dotprod_cccf_create(gc_sub, gc_sub_len);
w[i] = windowcf_create(gc_sub_len);
#if DEBUG
printf("gc_sub[%u] : \n", i);
for (j=0; j<gc_sub_len; j++)
printf(" g[%3u] = %12.8f + %12.8f\n", j, crealf(gc_sub[j]), cimagf(gc_sub[j]));
#endif
}
// generate DFT object
float complex x[num_channels]; // time-domain buffer
float complex X[num_channels]; // freq-domain buffer
#if 0
fftplan fft = fft_create_plan(num_channels, X, x, FFT_REVERSE, 0);
#else
fftplan fft = fft_create_plan(num_channels, X, x, FFT_FORWARD, 0);
#endif
//
// run analysis filter bank
//
#if 0
unsigned int filter_index = 0;
#else
unsigned int filter_index = num_channels-1;
#endif
float complex y_hat; // input sample
float complex * r; // read pointer
for (i=0; i<num_frames; i++) {
// load buffers
for (j=0; j<num_channels; j++) {
// grab sample
y_hat = y[i*num_channels + j];
// push sample into buffer at filter index
windowcf_push(w[filter_index], y_hat);
// decrement filter index
filter_index = (filter_index + num_channels - 1) % num_channels;
//filter_index = (filter_index + 1) % num_channels;
}
// execute filter outputs, reversing order of output (not
// sure why this is necessary)
for (j=0; j<num_channels; j++) {
windowcf_read(w[j], &r);
dotprod_cccf_execute(dp[j], r, &X[num_channels-j-1]);
}
#if 1
// compensate for carrier frequency offset (before transform)
for (j=0; j<num_channels; j++) {
X[j] *= cexpf(-_Complex_I*(dphi*i*num_channels));
}
#endif
// execute DFT, store result in buffer 'x'
fft_execute(fft);
#if 0
// compensate for carrier frequency offset (after transform)
for (j=0; j<num_channels; j++) {
x[j] *= cexpf(-_Complex_I*(dphi*i*num_channels));
}
#endif
// move to output array
for (j=0; j<num_channels; j++)
Y0[i][j] = x[j];
}
// destroy objects
for (i=0; i<num_channels; i++) {
dotprod_cccf_destroy(dp[i]);
windowcf_destroy(w[i]);
}
fft_destroy_plan(fft);
#if 0
// print filterbank channelizer
printf("\n");
printf("filterbank channelizer:\n");
for (i=0; i<num_symbols; i++) {
printf("%3u: ", i);
for (j=0; j<num_channels; j++) {
printf(" %8.5f+j%8.5f, ", crealf(Y0[i][j]), cimagf(Y0[i][j]));
}
printf("\n");
}
#endif
//
// export data
//
FILE*fid = fopen(OUTPUT_FILENAME,"w");
fprintf(fid,"%% %s: auto-generated file\n\n", OUTPUT_FILENAME);
fprintf(fid,"clear all;\nclose all;\n\n");
fprintf(fid,"num_channels=%u;\n", num_channels);
fprintf(fid,"num_symbols=%u;\n", num_symbols);
fprintf(fid,"num_frames = %u;\n", num_frames);
fprintf(fid,"num_samples = num_frames*num_channels;\n");
fprintf(fid,"y = zeros(1,%u);\n", num_samples);
fprintf(fid,"Y0 = zeros(%u,%u);\n", num_frames, num_channels);
fprintf(fid,"Y1 = zeros(%u,%u);\n", num_frames, num_channels);
for (i=0; i<num_frames; i++) {
for (j=0; j<num_channels; j++) {
fprintf(fid,"Y0(%4u,%4u) = %12.4e + j*%12.4e;\n", i+1, j+1, crealf(Y0[i][j]), cimagf(Y0[i][j]));
fprintf(fid,"Y1(%4u,%4u) = %12.4e + j*%12.4e;\n", i+1, j+1, crealf(Y1[i][j]), cimagf(Y1[i][j]));
}
}
// plot BPSK results
fprintf(fid,"figure;\n");
fprintf(fid,"plot(Y0(:,1:2:end),'x');\n");
fprintf(fid,"axis([-1 1 -1 1]*1.2*sqrt(num_channels));\n");
fprintf(fid,"axis square;\n");
fprintf(fid,"grid on;\n");
fclose(fid);
printf("results written to '%s'\n", OUTPUT_FILENAME);
printf("done.\n");
return 0;
}
void SpectrumVisualProcessor::process() {
if (!isOutputEmpty()) {
return;
}
if (!input || input->empty()) {
return;
}
if (fftSizeChanged.load()) {
setup(newFFTSize);
fftSizeChanged.store(false);
}
DemodulatorThreadIQData *iqData;
input->pop(iqData);
if (!iqData) {
return;
}
//Start by locking concurrent access to iqData
std::lock_guard < std::recursive_mutex > lock(iqData->getMonitor());
//then get the busy_lock
std::lock_guard < std::mutex > busy_lock(busy_run);
bool doPeak = peakHold.load() && (peakReset.load() == 0);
if (fft_result.size() != fftSizeInternal) {
if (fft_result.capacity() < fftSizeInternal) {
fft_result.reserve(fftSizeInternal);
fft_result_ma.reserve(fftSizeInternal);
fft_result_maa.reserve(fftSizeInternal);
fft_result_peak.reserve(fftSizeInternal);
}
fft_result.resize(fftSizeInternal);
fft_result_ma.resize(fftSizeInternal);
fft_result_maa.resize(fftSizeInternal);
fft_result_temp.resize(fftSizeInternal);
fft_result_peak.resize(fftSizeInternal);
}
if (peakReset.load() != 0) {
peakReset--;
if (peakReset.load() == 0) {
for (unsigned int i = 0, iMax = fftSizeInternal; i < iMax; i++) {
fft_result_peak[i] = fft_floor_maa;
}
fft_ceil_peak = fft_floor_maa;
fft_floor_peak = fft_ceil_maa;
}
}
std::vector<liquid_float_complex> *data = &iqData->data;
if (data && data->size()) {
unsigned int num_written;
long resampleBw = iqData->sampleRate;
bool newResampler = false;
int bwDiff;
if (is_view.load()) {
if (!iqData->sampleRate) {
iqData->decRefCount();
return;
}
while (resampleBw / SPECTRUM_VZM >= bandwidth) {
resampleBw /= SPECTRUM_VZM;
}
resamplerRatio = (double) (resampleBw) / (double) iqData->sampleRate;
size_t desired_input_size = fftSizeInternal / resamplerRatio;
this->desiredInputSize.store(desired_input_size);
if (iqData->data.size() < desired_input_size) {
// std::cout << "fft underflow, desired: " << desired_input_size << " actual:" << input->data.size() << std::endl;
desired_input_size = iqData->data.size();
}
if (centerFreq != iqData->frequency) {
if ((centerFreq - iqData->frequency) != shiftFrequency || lastInputBandwidth != iqData->sampleRate) {
if (abs(iqData->frequency - centerFreq) < (wxGetApp().getSampleRate() / 2)) {
long lastShiftFrequency = shiftFrequency;
shiftFrequency = centerFreq - iqData->frequency;
nco_crcf_set_frequency(freqShifter, (2.0 * M_PI) * (((double) abs(shiftFrequency)) / ((double) iqData->sampleRate)));
if (is_view.load()) {
long freqDiff = shiftFrequency - lastShiftFrequency;
if (lastBandwidth!=0) {
double binPerHz = double(lastBandwidth) / double(fftSizeInternal);
unsigned int numShift = floor(double(abs(freqDiff)) / binPerHz);
if (numShift < fftSizeInternal/2 && numShift) {
if (freqDiff > 0) {
memmove(&fft_result_ma[0], &fft_result_ma[numShift], (fftSizeInternal-numShift) * sizeof(double));
memmove(&fft_result_maa[0], &fft_result_maa[numShift], (fftSizeInternal-numShift) * sizeof(double));
// memmove(&fft_result_peak[0], &fft_result_peak[numShift], (fftSizeInternal-numShift) * sizeof(double));
// memset(&fft_result_peak[fftSizeInternal-numShift], 0, numShift * sizeof(double));
} else {
memmove(&fft_result_ma[numShift], &fft_result_ma[0], (fftSizeInternal-numShift) * sizeof(double));
memmove(&fft_result_maa[numShift], &fft_result_maa[0], (fftSizeInternal-numShift) * sizeof(double));
// memmove(&fft_result_peak[numShift], &fft_result_peak[0], (fftSizeInternal-numShift) * sizeof(double));
// memset(&fft_result_peak[0], 0, numShift * sizeof(double));
}
}
}
}
}
peakReset.store(PEAK_RESET_COUNT);
}
if (shiftBuffer.size() != desired_input_size) {
if (shiftBuffer.capacity() < desired_input_size) {
shiftBuffer.reserve(desired_input_size);
}
shiftBuffer.resize(desired_input_size);
}
if (shiftFrequency < 0) {
nco_crcf_mix_block_up(freqShifter, &iqData->data[0], &shiftBuffer[0], desired_input_size);
} else {
nco_crcf_mix_block_down(freqShifter, &iqData->data[0], &shiftBuffer[0], desired_input_size);
}
} else {
shiftBuffer.assign(iqData->data.begin(), iqData->data.begin()+desired_input_size);
}
if (!resampler || resampleBw != lastBandwidth || lastInputBandwidth != iqData->sampleRate) {
float As = 60.0f;
if (resampler) {
msresamp_crcf_destroy(resampler);
}
resampler = msresamp_crcf_create(resamplerRatio, As);
bwDiff = resampleBw-lastBandwidth;
lastBandwidth = resampleBw;
lastInputBandwidth = iqData->sampleRate;
newResampler = true;
peakReset.store(PEAK_RESET_COUNT);
}
unsigned int out_size = ceil((double) (desired_input_size) * resamplerRatio) + 512;
if (resampleBuffer.size() != out_size) {
if (resampleBuffer.capacity() < out_size) {
resampleBuffer.reserve(out_size);
}
resampleBuffer.resize(out_size);
}
msresamp_crcf_execute(resampler, &shiftBuffer[0], desired_input_size, &resampleBuffer[0], &num_written);
if (num_written < fftSizeInternal) {
memcpy(fftInData, resampleBuffer.data(), num_written * sizeof(liquid_float_complex));
memset(&(fftInData[num_written]), 0, (fftSizeInternal-num_written) * sizeof(liquid_float_complex));
} else {
memcpy(fftInData, resampleBuffer.data(), fftSizeInternal * sizeof(liquid_float_complex));
}
} else {
this->desiredInputSize.store(fftSizeInternal);
num_written = data->size();
if (data->size() < fftSizeInternal) {
memcpy(fftInData, data->data(), data->size() * sizeof(liquid_float_complex));
memset(&fftInData[data->size()], 0, (fftSizeInternal - data->size()) * sizeof(liquid_float_complex));
} else {
memcpy(fftInData, data->data(), fftSizeInternal * sizeof(liquid_float_complex));
}
}
bool execute = false;
if (num_written >= fftSizeInternal) {
execute = true;
memcpy(fftInput, fftInData, fftSizeInternal * sizeof(liquid_float_complex));
memcpy(fftLastData, fftInput, fftSizeInternal * sizeof(liquid_float_complex));
} else {
if (lastDataSize + num_written < fftSizeInternal) { // priming
unsigned int num_copy = fftSizeInternal - lastDataSize;
if (num_written > num_copy) {
num_copy = num_written;
}
memcpy(fftLastData, fftInData, num_copy * sizeof(liquid_float_complex));
lastDataSize += num_copy;
} else {
unsigned int num_last = (fftSizeInternal - num_written);
memcpy(fftInput, fftLastData + (lastDataSize - num_last), num_last * sizeof(liquid_float_complex));
memcpy(fftInput + num_last, fftInData, num_written * sizeof(liquid_float_complex));
memcpy(fftLastData, fftInput, fftSizeInternal * sizeof(liquid_float_complex));
execute = true;
}
}
if (execute) {
SpectrumVisualData *output = outputBuffers.getBuffer();
if (output->spectrum_points.size() != fftSize * 2) {
output->spectrum_points.resize(fftSize * 2);
}
if (doPeak) {
if (output->spectrum_hold_points.size() != fftSize * 2) {
output->spectrum_hold_points.resize(fftSize * 2);
}
} else {
output->spectrum_hold_points.resize(0);
}
float fft_ceil = 0, fft_floor = 1;
fft_execute(fftPlan);
for (int i = 0, iMax = fftSizeInternal / 2; i < iMax; i++) {
float a = fftOutput[i].real;
float b = fftOutput[i].imag;
float c = sqrt(a * a + b * b);
float x = fftOutput[fftSizeInternal / 2 + i].real;
float y = fftOutput[fftSizeInternal / 2 + i].imag;
float z = sqrt(x * x + y * y);
fft_result[i] = (z);
fft_result[fftSizeInternal / 2 + i] = (c);
}
if (newResampler && lastView) {
if (bwDiff < 0) {
for (unsigned int i = 0, iMax = fftSizeInternal; i < iMax; i++) {
fft_result_temp[i] = fft_result_ma[(fftSizeInternal/4) + (i/2)];
}
for (unsigned int i = 0, iMax = fftSizeInternal; i < iMax; i++) {
fft_result_ma[i] = fft_result_temp[i];
fft_result_temp[i] = fft_result_maa[(fftSizeInternal/4) + (i/2)];
}
for (unsigned int i = 0, iMax = fftSizeInternal; i < iMax; i++) {
fft_result_maa[i] = fft_result_temp[i];
}
} else {
for (size_t i = 0, iMax = fftSizeInternal; i < iMax; i++) {
if (i < fftSizeInternal/4) {
fft_result_temp[i] = 0; // fft_result_ma[fftSizeInternal/4];
} else if (i >= fftSizeInternal - fftSizeInternal/4) {
fft_result_temp[i] = 0; // fft_result_ma[fftSizeInternal - fftSizeInternal/4-1];
} else {
fft_result_temp[i] = fft_result_ma[(i-fftSizeInternal/4)*2];
}
}
for (unsigned int i = 0, iMax = fftSizeInternal; i < iMax; i++) {
fft_result_ma[i] = fft_result_temp[i];
if (i < fftSizeInternal/4) {
fft_result_temp[i] = 0; //fft_result_maa[fftSizeInternal/4];
} else if (i >= fftSizeInternal - fftSizeInternal/4) {
fft_result_temp[i] = 0; // fft_result_maa[fftSizeInternal - fftSizeInternal/4-1];
} else {
fft_result_temp[i] = fft_result_maa[(i-fftSizeInternal/4)*2];
}
}
for (unsigned int i = 0, iMax = fftSizeInternal; i < iMax; i++) {
fft_result_maa[i] = fft_result_temp[i];
}
}
}
for (int i = 0, iMax = fftSizeInternal; i < iMax; i++) {
if (fft_result_maa[i] != fft_result_maa[i]) fft_result_maa[i] = fft_result[i];
fft_result_maa[i] += (fft_result_ma[i] - fft_result_maa[i]) * fft_average_rate;
if (fft_result_ma[i] != fft_result_ma[i]) fft_result_ma[i] = fft_result[i];
fft_result_ma[i] += (fft_result[i] - fft_result_ma[i]) * fft_average_rate;
if (fft_result_maa[i] > fft_ceil || fft_ceil != fft_ceil) {
fft_ceil = fft_result_maa[i];
}
if (fft_result_maa[i] < fft_floor || fft_floor != fft_floor) {
fft_floor = fft_result_maa[i];
}
if (doPeak) {
if (fft_result_maa[i] > fft_result_peak[i]) {
fft_result_peak[i] = fft_result_maa[i];
}
}
}
if (fft_ceil_ma != fft_ceil_ma) fft_ceil_ma = fft_ceil;
fft_ceil_ma = fft_ceil_ma + (fft_ceil - fft_ceil_ma) * 0.05;
if (fft_ceil_maa != fft_ceil_maa) fft_ceil_maa = fft_ceil;
fft_ceil_maa = fft_ceil_maa + (fft_ceil_ma - fft_ceil_maa) * 0.05;
if (fft_floor_ma != fft_floor_ma) fft_floor_ma = fft_floor;
fft_floor_ma = fft_floor_ma + (fft_floor - fft_floor_ma) * 0.05;
if (fft_floor_maa != fft_floor_maa) fft_floor_maa = fft_floor;
fft_floor_maa = fft_floor_maa + (fft_floor_ma - fft_floor_maa) * 0.05;
if (doPeak) {
if (fft_ceil_maa > fft_ceil_peak) {
fft_ceil_peak = fft_ceil_maa;
}
if (fft_floor_maa < fft_floor_peak) {
fft_floor_peak = fft_floor_maa;
}
}
float sf = scaleFactor.load();
double visualRatio = (double(bandwidth) / double(resampleBw));
double visualStart = (double(fftSizeInternal) / 2.0) - (double(fftSizeInternal) * (visualRatio / 2.0));
double visualAccum = 0;
double peak_acc = 0, acc = 0, accCount = 0, i = 0;
double point_ceil = doPeak?fft_ceil_peak:fft_ceil_maa;
double point_floor = doPeak?fft_floor_peak:fft_floor_maa;
for (int x = 0, xMax = output->spectrum_points.size() / 2; x < xMax; x++) {
visualAccum += visualRatio * double(SPECTRUM_VZM);
while (visualAccum >= 1.0) {
unsigned int idx = round(visualStart+i);
if (idx > 0 && idx < fftSizeInternal) {
acc += fft_result_maa[idx];
if (doPeak) {
peak_acc += fft_result_peak[idx];
}
} else {
acc += fft_floor_maa;
if (doPeak) {
peak_acc += fft_floor_maa;
}
}
accCount += 1.0;
visualAccum -= 1.0;
i++;
}
output->spectrum_points[x * 2] = ((float) x / (float) xMax);
if (doPeak) {
output->spectrum_hold_points[x * 2] = ((float) x / (float) xMax);
}
if (accCount) {
output->spectrum_points[x * 2 + 1] = ((log10((acc/accCount)+0.25 - (point_floor-0.75)) / log10((point_ceil+0.25) - (point_floor-0.75))))*sf;
acc = 0.0;
if (doPeak) {
output->spectrum_hold_points[x * 2 + 1] = ((log10((peak_acc/accCount)+0.25 - (point_floor-0.75)) / log10((point_ceil+0.25) - (point_floor-0.75))))*sf;
peak_acc = 0.0;
}
accCount = 0.0;
}
}
if (hideDC.load()) { // DC-spike removal
long long freqMin = centerFreq-(bandwidth/2);
long long freqMax = centerFreq+(bandwidth/2);
long long zeroPt = (iqData->frequency-freqMin);
if (freqMin < iqData->frequency && freqMax > iqData->frequency) {
int freqRange = int(freqMax-freqMin);
int freqStep = freqRange/fftSize;
int fftStart = (zeroPt/freqStep)-(2000/freqStep);
int fftEnd = (zeroPt/freqStep)+(2000/freqStep);
// std::cout << "range:" << freqRange << ", step: " << freqStep << ", start: " << fftStart << ", end: " << fftEnd << std::endl;
if (fftEnd-fftStart < 2) {
fftEnd++;
fftStart--;
}
int numSteps = (fftEnd-fftStart);
int halfWay = fftStart+(numSteps/2);
if ((fftEnd+numSteps/2+1 < (long long) fftSize) && (fftStart-numSteps/2-1 >= 0) && (fftEnd > fftStart)) {
int n = 1;
for (int i = fftStart; i < halfWay; i++) {
output->spectrum_points[i * 2 + 1] = output->spectrum_points[(fftStart - n) * 2 + 1];
n++;
}
n = 1;
for (int i = halfWay; i < fftEnd; i++) {
output->spectrum_points[i * 2 + 1] = output->spectrum_points[(fftEnd + n) * 2 + 1];
n++;
}
if (doPeak) {
int n = 1;
for (int i = fftStart; i < halfWay; i++) {
output->spectrum_hold_points[i * 2 + 1] = output->spectrum_hold_points[(fftStart - n) * 2 + 1];
n++;
}
n = 1;
for (int i = halfWay; i < fftEnd; i++) {
output->spectrum_hold_points[i * 2 + 1] = output->spectrum_hold_points[(fftEnd + n) * 2 + 1];
n++;
}
}
}
}
}
output->fft_ceiling = point_ceil/sf;
output->fft_floor = point_floor;
output->centerFreq = centerFreq;
output->bandwidth = bandwidth;
distribute(output);
}
}
iqData->decRefCount();
lastView = is_view.load();
}
