294 lines
8.9 KiB
C
294 lines
8.9 KiB
C
#define TRACERPLUS (-5)
|
|
#include "lasp_slm.h"
|
|
#include "lasp_assert.h"
|
|
#include "lasp_tracer.h"
|
|
|
|
typedef struct Slm {
|
|
Sosfilterbank *prefilter; /// Pre-filter, A, or C. If NULL, not used.
|
|
Sosfilterbank *bandpass; /// Filterbank. If NULL, not used
|
|
Sosfilterbank **splowpass; /// Used for time-weighting of the squared signal
|
|
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 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;
|
|
|
|
Slm *Slm_create(Sosfilterbank *prefilter, Sosfilterbank *bandpass, const d fs,
|
|
const d tau, const d ref_level, us *downsampling_fac) {
|
|
fsTRACE(15);
|
|
assertvalidptr(downsampling_fac);
|
|
|
|
Slm *slm = NULL;
|
|
if (ref_level <= 0) {
|
|
WARN("Invalid reference level");
|
|
return NULL;
|
|
} else if (fs <= 0) {
|
|
WARN("Invalid sampling frequency");
|
|
return NULL;
|
|
}
|
|
|
|
slm = (Slm *)a_malloc(sizeof(Slm));
|
|
slm->ref_level = ref_level;
|
|
slm->prefilter = prefilter;
|
|
slm->bandpass = bandpass;
|
|
|
|
/// Compute the downsampling factor. This one is chosen based on the
|
|
/// lowpass filter. Which has a -3 dB point of f = 1/(tau*2*pi). See LASP
|
|
/// documentation for the computation of its minus 20 dB point. We set the
|
|
/// reduction in its 'sampling frequency' such that its noise is at a level
|
|
/// of 20 dB less than its 'signal'.
|
|
us ds_fac;
|
|
if (tau > 0) {
|
|
// A reasonable 'framerate' for the sound level meter, based on the
|
|
// filtering time constant.
|
|
d fs_slm = 10 / tau;
|
|
dVARTRACE(15, fs_slm);
|
|
if(fs_slm < 30) {
|
|
fs_slm = 30;
|
|
}
|
|
ds_fac = (us)(fs / fs_slm);
|
|
if (ds_fac == 0) {
|
|
// If we get 0, it should be 1
|
|
ds_fac++;
|
|
}
|
|
} else {
|
|
ds_fac = 1;
|
|
}
|
|
slm->downsampling_fac = ds_fac;
|
|
*downsampling_fac = ds_fac;
|
|
slm->cur_offset = 0;
|
|
|
|
/// Create the single pole lowpass
|
|
us filterbank_size;
|
|
if (bandpass) {
|
|
filterbank_size = Sosfilterbank_getFilterbankSize(bandpass);
|
|
} else {
|
|
filterbank_size = 1;
|
|
}
|
|
|
|
if (tau > 0) {
|
|
|
|
vd lowpass_sos = vd_alloc(6);
|
|
d b0 = 1.0 / (1 + 2 * tau * fs);
|
|
*getvdval(&lowpass_sos, 0) = b0;
|
|
*getvdval(&lowpass_sos, 1) = b0;
|
|
*getvdval(&lowpass_sos, 2) = 0;
|
|
*getvdval(&lowpass_sos, 3) = 1;
|
|
*getvdval(&lowpass_sos, 4) = (1 - 2 * tau * fs) * b0;
|
|
*getvdval(&lowpass_sos, 5) = 0;
|
|
|
|
slm->splowpass = a_malloc(filterbank_size * sizeof(Sosfilterbank *));
|
|
for (us ch = 0; ch < filterbank_size; ch++) {
|
|
/// Allocate a filterbank with one channel and one section.
|
|
slm->splowpass[ch] = Sosfilterbank_create(0, 1, 1);
|
|
Sosfilterbank_setFilter(slm->splowpass[ch], 0, lowpass_sos);
|
|
}
|
|
vd_free(&lowpass_sos);
|
|
} else {
|
|
/// No low-pass filtering. Tau set to zero
|
|
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;
|
|
}
|
|
|
|
dmat Slm_run(Slm *slm, vd *input_data) {
|
|
fsTRACE(15);
|
|
assertvalidptr(slm);
|
|
assert_vx(input_data);
|
|
|
|
/// First step: run the input data through the pre-filter
|
|
vd prefiltered;
|
|
if (slm->prefilter)
|
|
prefiltered = Sosfilterbank_filter(slm->prefilter, input_data);
|
|
else {
|
|
prefiltered = dmat_foreign(input_data);
|
|
}
|
|
dmat bandpassed;
|
|
if (slm->bandpass) {
|
|
bandpassed = Sosfilterbank_filter(slm->bandpass, &prefiltered);
|
|
} else {
|
|
bandpassed = dmat_foreign(&prefiltered);
|
|
}
|
|
us filterbank_size = bandpassed.n_cols;
|
|
|
|
/// Next step: square all values. We do this in-place. Then we filter for
|
|
/// each channel.
|
|
d ref_level = slm->ref_level;
|
|
d *tmp;
|
|
|
|
/// Pre-calculate the size of the output data
|
|
us downsampling_fac = slm->downsampling_fac;
|
|
us samples_bandpassed = bandpassed.n_rows;
|
|
iVARTRACE(15, samples_bandpassed);
|
|
iVARTRACE(15, downsampling_fac);
|
|
us cur_offset = slm->cur_offset;
|
|
|
|
/// Compute the number of samples output
|
|
us nsamples_output = samples_bandpassed;
|
|
if (downsampling_fac > 1) {
|
|
nsamples_output = (samples_bandpassed - cur_offset) / downsampling_fac;
|
|
if(nsamples_output > samples_bandpassed) {
|
|
// This means overflow of unsigned number calculations
|
|
nsamples_output = 0;
|
|
}
|
|
while(nsamples_output * downsampling_fac + cur_offset < samples_bandpassed) {
|
|
nsamples_output++;
|
|
}
|
|
}
|
|
|
|
iVARTRACE(15, nsamples_output);
|
|
iVARTRACE(15, cur_offset);
|
|
dmat levels = dmat_alloc(nsamples_output, filterbank_size);
|
|
us N, 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
|
|
// the low-pass filter
|
|
cur_offset = slm->cur_offset;
|
|
|
|
/// Apply single-pole lowpass filter for current filterbank channel
|
|
TRACE(15, "Start filtering");
|
|
vd power_filtered;
|
|
if (slm->splowpass) {
|
|
power_filtered = Sosfilterbank_filter(slm->splowpass[ch], &chan);
|
|
} else {
|
|
power_filtered = dmat_foreign(&chan);
|
|
}
|
|
TRACE(15, "Filtering done");
|
|
dbgassert(chan.n_rows == power_filtered.n_rows, "BUG");
|
|
|
|
/// 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((P + d_epsilon ) / ref_level / ref_level);
|
|
|
|
*getdmatval(&levels, i++, ch) = level;
|
|
cur_offset = cur_offset + downsampling_fac;
|
|
}
|
|
iVARTRACE(15, cur_offset);
|
|
iVARTRACE(15, i);
|
|
dbgassert(i == nsamples_output, "BUG");
|
|
|
|
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);
|
|
dmat_free(&bandpassed);
|
|
feTRACE(15);
|
|
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 Lmax = levels_from_power(&(slm->Pmax), slm->ref_level);
|
|
feTRACE(15);
|
|
return Lmax;
|
|
}
|
|
|
|
vd Slm_Leq(Slm* slm) {
|
|
fsTRACE(15);
|
|
assertvalidptr(slm);
|
|
print_vd(&(slm->Pm));
|
|
vd Leq = levels_from_power(&(slm->Pm), slm->ref_level);
|
|
feTRACE(15);
|
|
return Leq;
|
|
}
|
|
|
|
void Slm_free(Slm *slm) {
|
|
fsTRACE(15);
|
|
assertvalidptr(slm);
|
|
if (slm->prefilter) {
|
|
Sosfilterbank_free(slm->prefilter);
|
|
}
|
|
|
|
us filterbank_size;
|
|
if (slm->bandpass) {
|
|
filterbank_size = Sosfilterbank_getFilterbankSize(slm->bandpass);
|
|
Sosfilterbank_free(slm->bandpass);
|
|
} else {
|
|
filterbank_size = 1;
|
|
}
|
|
if (slm->splowpass) {
|
|
for (us ch = 0; ch < filterbank_size; ch++) {
|
|
Sosfilterbank_free(slm->splowpass[ch]);
|
|
}
|
|
a_free(slm->splowpass);
|
|
}
|
|
vd_free(&(slm->Ppeak));
|
|
vd_free(&(slm->Pmax));
|
|
vd_free(&(slm->Pm));
|
|
a_free(slm);
|
|
|
|
feTRACE(15);
|
|
}
|