diff --git a/lasp/c/CMakeLists.txt b/lasp/c/CMakeLists.txt index fa3e715..00bb27f 100644 --- a/lasp/c/CMakeLists.txt +++ b/lasp/c/CMakeLists.txt @@ -1,5 +1,6 @@ if(!LASP_DEBUG) SET_SOURCE_FILES_PROPERTIES(lasp_sosfilterbank.c PROPERTIES COMPILE_FLAGS -O3) +SET_SOURCE_FILES_PROPERTIES(lasp_slm.c PROPERTIES COMPILE_FLAGS -O3) endif(!LASP_DEBUG) add_library(lasp_lib diff --git a/lasp/c/lasp_mat.h b/lasp/c/lasp_mat.h index 810cf9a..aa536d0 100644 --- a/lasp/c/lasp_mat.h +++ b/lasp/c/lasp_mat.h @@ -445,10 +445,22 @@ static inline void dmat_copy(dmat* to,const dmat* from) { dbgassert(to->n_cols==from->n_cols,SIZEINEQUAL); for(us col=0;coln_cols;col++) { d_copy(getdmatval(to,0,col), - getdmatval(from,0,col), - to->n_rows,1,1); + getdmatval(from,0,col), + to->n_rows,1,1); } } + +/** + * Allocate a new array, with size based on other. + * + * @param[in] from: Array to copy + */ +static inline dmat dmat_alloc_from_dmat(const dmat* from) { + assertvalidptr(from); + dmat thecopy = dmat_alloc(from->n_rows, from->n_cols); + return thecopy; +} + /** * Copy contents of one matrix to another. Sizes should be equal * @@ -547,17 +559,36 @@ static inline void cmat_conj_inplace(cmat* x) { } } +/** + * Computes the maximum value for each row, returns a vector with maximum + * values for each column in the matrix. + * + * @param x + */ +static inline vd dmat_max(const dmat x) { + vd max_vals = vd_alloc(x.n_cols); + d max_val = -d_inf; + for(us j=0; j< x.n_cols; j++) { + for(us i=0; i< x.n_rows; i++) { + max_val = *getdmatval(&x, i, j) < max_val? max_val : *getdmatval(&x, i,j); + } + *getvdval(&max_vals, j) = max_val; + } + return max_vals; +} + #ifdef LASP_DEBUG void print_cmat(const cmat* m); void print_dmat(const dmat* m); -#define print_vc print_cmat -#define print_vd print_dmat +#define print_vc(x) assert_vx(x) print_cmat(x) +#define print_vd(x) assert_vx(x) print_dmat(x) #else #define print_cmat(m) -#define print_vc(m) #define print_dmat(m) +#define print_vc(m) +#define print_vd(m) #endif #endif // LASP_MAT_H diff --git a/lasp/c/lasp_math_raw.h b/lasp/c/lasp_math_raw.h index 66a2901..376aec6 100644 --- a/lasp/c/lasp_math_raw.h +++ b/lasp/c/lasp_math_raw.h @@ -11,6 +11,7 @@ #include "lasp_assert.h" #include "lasp_tracer.h" #include "lasp_types.h" +#include #include #if LASP_USE_BLAS == 1 @@ -34,6 +35,7 @@ #define d_cos cos #define d_pow pow #define d_log10 log10 +#define d_epsilon (DBL_EPSILON) #else // LASP_DOUBLE_PRECISION not defined #define c_conj conjf @@ -49,8 +51,12 @@ #define d_cos cosf #define d_pow powf #define d_log10 log10f +#define d_epsilon (FLT_EPSILON) #endif // LASP_DOUBLE_PRECISION +/// Positive infinite +#define d_inf (INFINITY) + #ifdef M_PI static const d number_pi = M_PI; #else diff --git a/lasp/c/lasp_slm.c b/lasp/c/lasp_slm.c index b7dbc37..5b3a012 100644 --- a/lasp/c/lasp_slm.c +++ b/lasp/c/lasp_slm.c @@ -1,4 +1,4 @@ -#define TRACERPLUS (10) +#define TRACERPLUS (-5) #include "lasp_slm.h" #include "lasp_assert.h" #include "lasp_tracer.h" @@ -10,7 +10,10 @@ typedef struct Slm { d ref_level; /// Reference value for computing decibels us downsampling_fac; /// Every x'th sample is returned. us cur_offset; /// Storage for offset point in input arrays - vd Leq; /// Storage for the computed equivalent levels so far. + vd Pm; /// Storage for the computing the mean of the square of the signal. + vd Pmax; /// Storage for maximum computed signal power so far. + vd Ppeak; /// Storage for computing peak powers so far. + us N; /// Counter for the number of time samples counted that came in } Slm; @@ -42,9 +45,10 @@ Slm *Slm_create(Sosfilterbank *prefilter, Sosfilterbank *bandpass, const d fs, if (tau > 0) { const d fs_slm = 1 / (2 * number_pi * tau) * (1 - 0.01) / 0.01; dVARTRACE(15, fs_slm); - ds_fac = (us) (fs / fs_slm); + ds_fac = (us)(fs / fs_slm); // If we get 0, it should be 1 - if(ds_fac == 0) ds_fac++; + if (ds_fac == 0) + ds_fac++; } else { ds_fac = 1; } @@ -83,6 +87,14 @@ Slm *Slm_create(Sosfilterbank *prefilter, Sosfilterbank *bandpass, const d fs, slm->splowpass = NULL; } + /// Initialize statistics gatherers + slm->Ppeak = vd_alloc(filterbank_size); + slm->Pmax = vd_alloc(filterbank_size); + slm->Pm = vd_alloc(filterbank_size); + slm->N = 0; + vd_set(&(slm->Ppeak), 0); + vd_set(&(slm->Pmax), 0); + vd_set(&(slm->Pm), 0); feTRACE(15); return slm; } @@ -121,7 +133,7 @@ dmat Slm_run(Slm *slm, vd *input_data) { /// Compute the number of samples output int nsamples_output = samples_bandpassed; - if(downsampling_fac > 1) { + if (downsampling_fac > 1) { nsamples_output = (samples_bandpassed - cur_offset) / downsampling_fac; while (nsamples_output * downsampling_fac + cur_offset < samples_bandpassed) nsamples_output++; @@ -132,14 +144,16 @@ dmat Slm_run(Slm *slm, vd *input_data) { iVARTRACE(15, nsamples_output); iVARTRACE(15, cur_offset); dmat levels = dmat_alloc(nsamples_output, filterbank_size); + us N, ch; - for (us ch = 0; ch < bandpassed.n_cols; ch++) { + for (ch = 0; ch < bandpassed.n_cols; ch++) { iVARTRACE(15, ch); vd chan = dmat_column(&bandpassed, ch); /// Inplace squaring of the signal for (us sample = 0; sample < bandpassed.n_rows; sample++) { tmp = getdmatval(&bandpassed, sample, ch); *tmp = *tmp * *tmp; + *getvdval(&(slm->Ppeak), ch) = d_max(*getvdval(&(slm->Ppeak), ch), *tmp); } // Now that all data for the channel is squared, we can run it through @@ -149,7 +163,7 @@ dmat Slm_run(Slm *slm, vd *input_data) { /// Apply single-pole lowpass filter for current filterbank channel TRACE(15, "Start filtering"); vd power_filtered; - if(slm->splowpass) { + if (slm->splowpass) { power_filtered = Sosfilterbank_filter(slm->splowpass[ch], &chan); } else { power_filtered = dmat_foreign(&chan); @@ -159,12 +173,27 @@ dmat Slm_run(Slm *slm, vd *input_data) { /// Output resulting levels at a lower interval us i = 0; + N = slm->N; + d *Pm = getvdval(&(slm->Pm), ch); while (cur_offset < samples_bandpassed) { iVARTRACE(10, i); iVARTRACE(10, cur_offset); + + /// Filtered power. + const d P = *getvdval(&power_filtered, cur_offset); + dVARTRACE(15, P); + + /// Compute maximum, compare to current maximum + *getvdval(&(slm->Pmax), ch) = d_max(*getvdval(&(slm->Pmax), ch), P); + + /// Update mean power + d Nd = (d) N; + *Pm = (*Pm*Nd + P ) / (Nd+1); + N++; + dVARTRACE(15, *Pm); + /// Compute level - d level = 10 * d_log10(*getvdval(&power_filtered, cur_offset) / - ref_level / ref_level); + d level = 10 * d_log10((P + d_epsilon ) / ref_level / ref_level); *getdmatval(&levels, i++, ch) = level; cur_offset = cur_offset + downsampling_fac; @@ -176,6 +205,9 @@ dmat Slm_run(Slm *slm, vd *input_data) { vd_free(&power_filtered); vd_free(&chan); } + /// Update sample counter + dbgassert(ch >0, "BUG"); + slm->N = N; slm->cur_offset = cur_offset - samples_bandpassed; vd_free(&prefiltered); @@ -184,6 +216,44 @@ dmat Slm_run(Slm *slm, vd *input_data) { return levels; } + +static inline vd levels_from_power(const vd* power,const d ref_level){ + fsTRACE(15); + + vd levels = dmat_alloc_from_dmat(power); + for(us i=0; i< levels.n_rows; i++) { + *getvdval(&levels, i) = 10 * d_log10( + (*getvdval(power, i) + d_epsilon) / ref_level / ref_level); + + } + feTRACE(15); + return levels; +} + +vd Slm_Lpeak(Slm* slm) { + fsTRACE(15); + assertvalidptr(slm); + vd Lpeak = levels_from_power(&(slm->Ppeak), slm->ref_level); + feTRACE(15); + return Lpeak; +} + +vd Slm_Lmax(Slm* slm) { + fsTRACE(15); + assertvalidptr(slm); + vd Lpeak = levels_from_power(&(slm->Pmax), slm->ref_level); + feTRACE(15); + return Lpeak; +} + +vd Slm_Leq(Slm* slm) { + fsTRACE(15); + assertvalidptr(slm); + vd Lpeak = levels_from_power(&(slm->Pm), slm->ref_level); + feTRACE(15); + return Lpeak; +} + void Slm_free(Slm *slm) { fsTRACE(15); assertvalidptr(slm); @@ -204,6 +274,9 @@ void Slm_free(Slm *slm) { } a_free(slm->splowpass); } + vd_free(&(slm->Ppeak)); + vd_free(&(slm->Pmax)); + vd_free(&(slm->Pm)); a_free(slm); feTRACE(15); diff --git a/lasp/c/lasp_slm.h b/lasp/c/lasp_slm.h index 67970ca..479b383 100644 --- a/lasp/c/lasp_slm.h +++ b/lasp/c/lasp_slm.h @@ -67,6 +67,30 @@ dmat Slm_run(Slm* slm, */ void Slm_free(Slm* f); +/** + * Returns the (raw) peak level computed so far. + * + * @param[in] slm: Slm handle + * @return Vector of peak level for each channel in the filterbank. + */ +vd Slm_Lpeak(Slm* slm); + +/** + * Returns the equivalent level computed so far. + * + * @param[in] slm: Slm handle + * @return Vector of equivalent levels for each channel in the filterbank. + */ +vd Slm_Leq(Slm* slm); + +/** + * Returns the maximum level computed so far. + * + * @param[in] slm: Slm handle + * @return Vector of maximum levels for each channel in the filterbank. + */ +vd Slm_Lmax(Slm* slm); + #endif // LASP_SLM_H ////////////////////////////////////////////////////////////////////// diff --git a/lasp/wrappers.pyx b/lasp/wrappers.pyx index 089fbe8..91af69b 100644 --- a/lasp/wrappers.pyx +++ b/lasp/wrappers.pyx @@ -465,6 +465,9 @@ cdef extern from "lasp_slm.h" nogil: us* downsampling_fac) dmat Slm_run(c_Slm* slm,vd* input_data) void Slm_free(c_Slm* slm) + vd Slm_Leq(c_Slm*) + vd Slm_Lmax(c_Slm*) + vd Slm_Lpeak(c_Slm*) tau_fast = TAU_FAST @@ -538,6 +541,23 @@ cdef class Slm: result = dmat_to_ndarray(&res,True) return result + def Leq(self): + cdef vd res + res = Slm_Leq(self.c_slm) + # True below, means transfer ownership of allocated data to Numpy + return dmat_to_ndarray(&res,True) + + def Lmax(self): + cdef vd res + res = Slm_Lmax(self.c_slm) + # True below, means transfer ownership of allocated data to Numpy + return dmat_to_ndarray(&res,True) + + def Lpeak(self): + cdef vd res + res = Slm_Lpeak(self.c_slm) + # True below, means transfer ownership of allocated data to Numpy + return dmat_to_ndarray(&res,True) def __dealloc__(self): if self.c_slm: