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;
}
static void adc_calibration(struct adc* me, struct adc_event* e) {
if (me == NULL || e == NULL) {
return;
}
switch (e->super_.signal) {
case ADC_CALIBRATION_SIG:
PORTD |= (1 << PD0); /* start calibration */
PORTC = 0x00;
RESET_SMA_BUF(&me->sma); /* reset SMA buffer and append new value */
case ADC_NEW_SAMPLE_SIG:
if (TCNT1 >= CLK_TICKS_PER_SMA_SAMPLES(SMA_SAMPLES_PER_SECONDS(0.5))) { /* 0.5s passed */
APPEND_SMA_VAL(&me->sma, MIN(255, e->sample / 4)); /* buffer stores uint8_t */
PORTD ^= (1 << PD1); /* flash PD1 */
TCNT1 = 0; /* reset timer counter */
if (me->sma.lpos >= SMA_SAMPLES_PER_SECONDS(10)) {
me->sma_prev = me->sma.sma;
me->resolution = MIN(me->sma.sma, 255 - me->sma.sma) / 4; /* classification segment length */
PORTD &= ~(1 << PD0); /* turn PD0 off */
FSM_TRANSITION_(&me->super_, &adc_default); /* stop calibration */
}
break;
}
default:
break;
}
}
static void adc_default(struct adc* me, struct adc_event* e) {
uint8_t sma;
uint8_t resolution;
if (me == NULL || e == NULL) {
return;
}
switch (e->super_.signal) {
case ADC_NEW_SAMPLE_SIG:
if (TCNT1 >= CLK_TICKS_PER_SMA_SAMPLES(SMA_SAMPLES_PER_SECONDS(0.5))) { /* 0.5s passed */
APPEND_SMA_VAL(&me->sma, MIN(255, e->sample / 4)); /* buffer stores uint8_t */
TCNT1 = 0; /* reset timer counter */
PORTD ^= (1 << PD3); /* toggle PD3 */
}
adc_flash(me, e);
sma = me->sma.sma;
resolution = me->resolution;
if (me->sma.lpos >= SMA_SAMPLES_PER_SECONDS(10) && ((sma > me->sma_prev + MIN(resolution, 255 - me->sma_prev)) || (sma < me->sma_prev - MIN(resolution, me->sma_prev)))) {
FSM_TRANSITION_(&me->super_, &adc_calibration); /* start calibration, do the state transition */
e->super_.signal = ADC_CALIBRATION_SIG;
FSM_DISPATCH_(&me->super_, &e->super_);
}
break;
default:
break;
}
}
/*
* 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;
}
/*! \brief Process one frame of data with avmd algorithm
* @author Eric des Courtis
* @param session An avmd session
* @param frame A audio frame
*/
static void avmd_process(avmd_session_t *session, switch_frame_t *frame)
{
switch_event_t *event;
switch_status_t status;
switch_event_t *event_copy;
switch_channel_t *channel;
circ_buffer_t *b;
size_t pos;
double f;
double a;
double error = 0.0;
double success = 0.0;
double amp = 0.0;
double s_rate;
double e_rate;
double avg_a;
double sine_len;
uint32_t sine_len_i;
int valid;
b = &session->b;
/*! If beep has already been detected skip the CPU heavy stuff */
if(session->state.beep_state == BEEP_DETECTED){
return;
}
/*! Precompute values used heavily in the inner loop */
sine_len_i = SINE_LEN(session->rate);
sine_len = (double)sine_len_i;
channel = switch_core_session_get_channel(session->session);
/*! Insert frame of 16 bit samples into buffer */
INSERT_INT16_FRAME(b, (int16_t *)(frame->data), frame->samples);
/*! INNER LOOP -- OPTIMIZATION TARGET */
for(pos = GET_BACKLOG_POS(b); pos != (GET_CURRENT_POS(b) - P); pos++){
/*! Get a desa2 frequency estimate in Hertz */
f = TO_HZ(session->rate, desa2(b, pos));
/*! Don't caculate amplitude if frequency is not within range */
if(f < MIN_FREQUENCY || f > MAX_FREQUENCY) {
a = 0.0;
error += 1.0;
} else {
a = amplitude(b, pos, f);
success += 1.0;
if(!ISNAN(a)){
amp += a;
}
}
/*! Every once in a while we evaluate the desa2 and amplitude results */
if(((pos + 1) % sine_len_i) == 0){
s_rate = success / (error + success);
e_rate = error / (error + success);
avg_a = amp / sine_len;
/*! Results out of these ranges are considered invalid */
valid = 0;
if( s_rate > 0.60 && avg_a > 0.50) valid = 1;
else if(s_rate > 0.65 && avg_a > 0.45) valid = 1;
else if(s_rate > 0.70 && avg_a > 0.40) valid = 1;
else if(s_rate > 0.80 && avg_a > 0.30) valid = 1;
else if(s_rate > 0.95 && avg_a > 0.05) valid = 1;
else if(s_rate >= 0.99 && avg_a > 0.04) valid = 1;
else if(s_rate == 1.00 && avg_a > 0.02) valid = 1;
if(valid) {
APPEND_SMA_VAL(&session->sma_b, s_rate * avg_a);
}
else {
APPEND_SMA_VAL(&session->sma_b, 0.0 );
}
/*! If sma is higher then 0 we have some kind of detection (increase this value to eliminate false positives ex: 0.01) */
if(session->sma_b.sma > 0.00){
/*! Throw an event to FreeSWITCH */
status = switch_event_create_subclass(&event, SWITCH_EVENT_CUSTOM, AVMD_EVENT_BEEP);
if(status != SWITCH_STATUS_SUCCESS) {
return;
}
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "Beep-Status", "stop");
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "Unique-ID", switch_core_session_get_uuid(session->session));
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "call-command", "avmd");
if ((switch_event_dup(&event_copy, event)) != SWITCH_STATUS_SUCCESS) {
return;
}
switch_core_session_queue_event(session->session, &event);
switch_event_fire(&event_copy);
switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(session->session), SWITCH_LOG_INFO, "<<< AVMD - Beep Detected >>>\n");
switch_channel_set_variable(channel, "avmd_detect", "TRUE");
RESET_SMA_BUFFER(&session->sma_b);
session->state.beep_state = BEEP_DETECTED;
return;
}
amp = 0.0;
success = 0.0;
error = 0.0;
}
}
}
