From 90683eeefb0386d07c57faa0d9ce5b416acc0d62 Mon Sep 17 00:00:00 2001 From: ymherklotz Date: Thu, 8 Mar 2018 15:08:05 +0000 Subject: add power --- Project/RTDSP/.launches/RTDSP.launch | 2 +- Project/RTDSP/enhance.c | 39 +++++++++++++++++++----------------- 2 files changed, 22 insertions(+), 19 deletions(-) (limited to 'Project') diff --git a/Project/RTDSP/.launches/RTDSP.launch b/Project/RTDSP/.launches/RTDSP.launch index a54495a..fe8b6e4 100644 --- a/Project/RTDSP/.launches/RTDSP.launch +++ b/Project/RTDSP/.launches/RTDSP.launch @@ -13,7 +13,7 @@ - + diff --git a/Project/RTDSP/enhance.c b/Project/RTDSP/enhance.c index 5f2e1ad..728194c 100644 --- a/Project/RTDSP/enhance.c +++ b/Project/RTDSP/enhance.c @@ -103,9 +103,10 @@ volatile int frame_ptr=0; /* Frame pointer */ volatile int frame_ctr = 0; volatile int m_ptr = 0; float lambda = 0.05; -float alpha = 20; +float alpha = 400; double avg = 0; MVal M[NUM_M]; +float mag_N_X; float K; float time_constant = 50E-6; /* Time constant in ms */ int started = 0; @@ -150,12 +151,8 @@ void main() outgain=OUTGAIN; for (k = 0; k < NUM_M; ++k) { - int i; M[k].pow = (float *) calloc(FFTLEN, sizeof(float)); - for(i = 0; i < FFTLEN; ++i) { - M[k].pow[i] = MAX_FLOAT; - } - M[k].sum = MAX_FLOAT; + M[k].sum = 0; } // initializing the value to estimate the low pass filter @@ -205,13 +202,20 @@ void init_HWI(void) // Spectrum calculations for the new values void write_spectrum(void) { unsigned int k; - M[m_ptr].sum = 0; - for(k = 0; k < FFTLEN; ++k) { - if(power_in[k] < M[m_ptr].pow[k] && power_in[k] != 0) { + if(M[m_ptr].sum == 0) { + for(k = 0; k < FFTLEN; ++k) { M[m_ptr].pow[k] = power_in[k]; M[m_ptr].sum += power_in[k]; - } else { - M[m_ptr].sum += M[m_ptr].pow[k]; + } + } else { + M[m_ptr].sum = 0; + for(k = 0; k < FFTLEN; ++k) { + if(power_in[k] < M[m_ptr].pow[k]) { + M[m_ptr].pow[k] = power_in[k]; + M[m_ptr].sum += power_in[k]; + } else { + M[m_ptr].sum += M[m_ptr].pow[k]; + } } } } @@ -221,7 +225,7 @@ void get_noise(void) { int min_index = 0, k; for(k = 1; k < NUM_M; ++k) { - if (M[k].sum < min_sum) { + if (M[k].sum < min_sum && M[k].sum != 0) { min_sum = M[k].sum; min_index = k; } @@ -235,7 +239,6 @@ void process_frame(void) { int k, m; int io_ptr0; - float mag_N_X; /* work out fraction of available CPU time used by algorithm */ cpufrac = ((float) (io_ptr & (FRAMEINC - 1)))/FRAMEINC; @@ -269,8 +272,8 @@ void process_frame(void) fft(FFTLEN, fft_out); // calculate the power spectrum - for (k = 0; i < FFTLEN; ++l) { - power_in[k] = pow(fft_out[k].r, 2) + pow(fft_out[k].i, 2); + for (k = 0; k < FFTLEN; ++k) { + power_in[k] = fft_out[k].r * fft_out[k].r + fft_out[k].i * fft_out[k].i; } // Get average of fft_out and write to Spectrum @@ -285,15 +288,15 @@ void process_frame(void) if(++m_ptr == NUM_M) m_ptr = 0; M[m_ptr].sum = 0; for(i = 0; i < FFTLEN; ++i) { - M[m_ptr].pow[i] = power_in[k]; - M[m_ptr].sum += power_in[k]; + M[m_ptr].pow[i] = power_in[i]; + M[m_ptr].sum += power_in[i]; } } // max(lambda, |N(w)/g(w)| for (k = 0; k < FFTLEN; ++k) { float g; - mag_N_X = 1 - alpha * noise[k]/power_in[k]; + mag_N_X = sqrt(1 - alpha * noise[k]/power_in[k]); g = mag_N_X > lambda ? mag_N_X : lambda; fft_out[k] = rmul(g, fft_out[k]); } -- cgit