void
desa2_freeswitch_double(double *input, double *mean1, double *mean2, double *var1, double *var2)
{
int i;
circ_buffer_t b;
sma_buffer_t sma_b;
sma_buffer_t sqa_b;
double freq[BLOCK]; // frequency estimates
INIT_CIRC_BUFFER(&b, BLOCK);
INIT_SMA_BUFFER(&sma_b, 10);
INIT_SMA_BUFFER(&sqa_b, 10);
INSERT_DOUBLE_FRAME(&b, input, BLOCK);
// calculate the frequency estimate for each sample as in FS
for (i = 0; i < (BLOCK - P); i++)
{
freq[i] = desa2_fs(&b, i);
APPEND_SMA_VAL(&sma_b, freq[i]);
APPEND_SMA_VAL(&sqa_b, freq[i] * freq[i]);
*var1 = sqa_b.sma - (sma_b.sma * sma_b.sma);
printf("<<< AVMD v[%f] f[%f][%f]Hz sma[%f][%f]Hz sqa[%f]\tsample[%d]\t[%f][%f]>>>\n",
*var1, freq[i], TO_HZ(8000, freq[i]), sma_b.sma, TO_HZ(8000, sma_b.sma), sqa_b.sma, i, input[i], GET_SAMPLE((&b), i));
}
/* set mean */
*mean1 = sma_b.sma;
/* calculate the variance */
*var1 = sqa_b.sma - (sma_b.sma * sma_b.sma);
/* for comparison calculate mean2 frequency & var2 */
double mean = 0.0;
for (i = 0; i < (BLOCK - P); i++)
{
mean += freq[i];
}
mean /= (double) (BLOCK - P);
*mean2 = mean;
*var2 = 0.0;
for (i = 0; i < (BLOCK - P); i++ )
{
*var2 += freq[i] * freq[i];
}
*var2 /= (double)(BLOCK - P);
*var2 -= (mean * mean);
free(b.buf);
free(sma_b.data);
free(sqa_b.data);
return;
}
/*! \brief The avmd session data initialization function
* @author Eric des Courtis
* @param avmd_session A reference to a avmd session
* @param fs_session A reference to a FreeSWITCH session
*/
static void init_avmd_session_data(avmd_session_t *avmd_session, switch_core_session_t *fs_session)
{
/*! This is a worst case sample rate estimate */
avmd_session->rate = 48000;
INIT_CIRC_BUFFER(&avmd_session->b, BEEP_LEN(avmd_session->rate), FRAME_LEN(avmd_session->rate), fs_session);
avmd_session->session = fs_session;
avmd_session->pos = 0;
avmd_session->state.last_beep = 0;
avmd_session->state.beep_state = BEEP_NOTDETECTED;
INIT_SMA_BUFFER(
&avmd_session->sma_b,
BEEP_LEN(avmd_session->rate) / SINE_LEN(avmd_session->rate),
fs_session
);
}
/*
* Purpose: detect a tone using DESA-1 algorithm
*
* Parameters:
* input pointer to input samples
* variance the variance of the frequency estimates
*
* Return value: frequency estimate in Hz
*/
double
desa1(double *input, double *variance)
{
// detector variables
static double diff0 = 0.0; // delayed differences
static double diff1 = 0.0;
static double diff2 = 0.0;
static double diff3 = 0.0;
static double x1 = 0.0; // delayed inputs
static double x2 = 0.0;
static double x3 = 0.0;
sma_buffer_t sma_b;
sma_buffer_t sqa_b;
double num; // numerator
double den; // denominator
double freq[BLOCK]; // frequency estimates
int i;
INIT_SMA_BUFFER(&sma_b, 10);
INIT_SMA_BUFFER(&sqa_b, 10);
// calculate the frequency estimate for each sample
for ( i = 0; i < BLOCK; i++ )
{
diff0 = input[i] - x1;
num = diff2 * diff2 - diff1 * diff3
+ diff1 * diff1 - diff0 * diff2;
den = x2 * x2 - x1 * x3;
freq[i] = SAMPLE_RATE * asin(sqrt(num/(8.0 * den))) / M_PI;
APPEND_SMA_VAL(&sma_b, freq[i]);
APPEND_SMA_VAL(&sqa_b, freq[i] * freq[i]);
// handle errors - division by zero, square root of
// negative number or asin of number > 1 or < -1
if (isnan(freq[i]))
{
freq[i] = 0;
} else if (isinf(freq[i]))
{
freq[i] = 2000.0;
}
diff3 = diff2;
diff2 = diff1;
diff1 = diff0;
x3 = x2;
x2 = x1;
x1 = input[i];
printf("<<< AVMD f[%f]Hz\tsample[%d]\t[%f] >>>\n", freq[i], i, input[i]);
printf("<<< AVMD v[%f] f[%f]Hz sma[%f]Hz sqa[%f]\tsample[%d]\t[%d] >>>\n",
sqa_b.sma - (sma_b.sma * sma_b.sma), freq[i], sma_b.sma, sqa_b.sma, i, input[i]);
}
// calculate mean frequency
double mean = 0.0;
for ( i = 0; i < BLOCK; i++ )
{
mean += freq[i];
}
mean /= (double)BLOCK;
// calculate the variance in the frequency estimates
*variance = 0.0;
for ( i = 0; i < BLOCK; i++ )
{
*variance += freq[i] * freq[i];
}
*variance /= (double)BLOCK;
*variance -= (mean * mean);
return mean;
}
