SPAN_DECLARE(int) ademco_contactid_sender_fillin(ademco_contactid_sender_state_t *s, int samples)
{
/* Restart any Goertzel and energy gathering operation we might be in the middle of. */
goertzel_reset(&s->tone_1400);
goertzel_reset(&s->tone_2300);
#if defined(SPANDSP_USE_FIXED_POINT)
s->energy = 0;
#else
s->energy = 0.0f;
#endif
s->current_sample = 0;
/* Don't update the hit detection. Pretend it never happened. */
/* TODO: Surely we can be cleverer than this. */
return 0;
}
SPAN_DECLARE(int) dtmf_rx_fillin(dtmf_rx_state_t *s, int samples)
{
int i;
/* Restart any Goertzel and energy gathering operation we might be in the middle of. */
for (i = 0; i < 4; i++)
{
goertzel_reset(&s->row_out[i]);
goertzel_reset(&s->col_out[i]);
}
#if defined(SPANDSP_USE_FIXED_POINT)
s->energy = 0;
#else
s->energy = 0.0f;
#endif
s->current_sample = 0;
/* Don't update the hit detection. Pretend it never happened. */
/* TODO: Surely we can be cleverer than this. */
return 0;
}
int r2_mf_rx(r2_mf_rx_state_t *s, const int16_t amp[], int samples)
{
float energy[6];
float famp;
float v1;
int i;
int j;
int sample;
int best;
int second_best;
int hit;
int hit_char;
int limit;
hit = 0;
hit_char = 0;
for (sample = 0; sample < samples; sample = limit)
{
if ((samples - sample) >= (s->samples - s->current_sample))
limit = sample + (s->samples - s->current_sample);
else
limit = samples;
for (j = sample; j < limit; j++)
{
famp = amp[j];
/* With GCC 2.95, the following unrolled code seems to take about 35%
(rough estimate) as long as a neat little 0-5 loop */
v1 = s->out[0].v2;
s->out[0].v2 = s->out[0].v3;
s->out[0].v3 = s->out[0].fac*s->out[0].v2 - v1 + famp;
v1 = s->out[1].v2;
s->out[1].v2 = s->out[1].v3;
s->out[1].v3 = s->out[1].fac*s->out[1].v2 - v1 + famp;
v1 = s->out[2].v2;
s->out[2].v2 = s->out[2].v3;
s->out[2].v3 = s->out[2].fac*s->out[2].v2 - v1 + famp;
v1 = s->out[3].v2;
s->out[3].v2 = s->out[3].v3;
s->out[3].v3 = s->out[3].fac*s->out[3].v2 - v1 + famp;
v1 = s->out[4].v2;
s->out[4].v2 = s->out[4].v3;
s->out[4].v3 = s->out[4].fac*s->out[4].v2 - v1 + famp;
v1 = s->out[5].v2;
s->out[5].v2 = s->out[5].v3;
s->out[5].v3 = s->out[5].fac*s->out[5].v2 - v1 + famp;
}
s->current_sample += (limit - sample);
if (s->current_sample < s->samples)
continue;
/* We are at the end of an MF detection block */
/* Find the two highest energies */
energy[0] = goertzel_result(&s->out[0]);
energy[1] = goertzel_result(&s->out[1]);
if (energy[0] > energy[1])
{
best = 0;
second_best = 1;
}
else
{
best = 1;
second_best = 0;
}
for (i = 2; i < 6; i++)
{
energy[i] = goertzel_result(&s->out[i]);
if (energy[i] >= energy[best])
{
second_best = best;
best = i;
}
else if (energy[i] >= energy[second_best])
{
second_best = i;
}
}
/* Basic signal level and twist tests */
hit = FALSE;
if (energy[best] >= R2_MF_THRESHOLD
&&
energy[second_best] >= R2_MF_THRESHOLD
&&
energy[best] < energy[second_best]*R2_MF_TWIST
&&
energy[best]*R2_MF_TWIST > energy[second_best])
{
/* Relative peak test */
hit = TRUE;
for (i = 0; i < 6; i++)
{
if (i != best && i != second_best)
{
if (energy[i]*R2_MF_RELATIVE_PEAK >= energy[second_best])
{
/* The best two are not clearly the best */
hit = FALSE;
break;
}
}
}
}
if (hit)
{
/* Get the values into ascending order */
if (second_best < best)
{
i = best;
best = second_best;
second_best = i;
}
best = best*5 + second_best - 1;
hit_char = r2_mf_positions[best];
}
else
{
hit_char = 0;
}
/* Reinitialise the detector for the next block */
if (s->fwd)
{
for (i = 0; i < 6; i++)
goertzel_reset(&s->out[i]);
}
else
{
for (i = 0; i < 6; i++)
goertzel_reset(&s->out[i]);
}
s->current_sample = 0;
}
return hit_char;
}
int bell_mf_rx(bell_mf_rx_state_t *s, const int16_t amp[], int samples)
{
float energy[6];
float famp;
float v1;
int i;
int j;
int sample;
int best;
int second_best;
int limit;
uint8_t hit;
hit = 0;
for (sample = 0; sample < samples; sample = limit)
{
if ((samples - sample) >= (120 - s->current_sample))
limit = sample + (120 - s->current_sample);
else
limit = samples;
for (j = sample; j < limit; j++)
{
famp = amp[j];
/* With GCC 2.95, the following unrolled code seems to take about 35%
(rough estimate) as long as a neat little 0-5 loop */
v1 = s->out[0].v2;
s->out[0].v2 = s->out[0].v3;
s->out[0].v3 = s->out[0].fac*s->out[0].v2 - v1 + famp;
v1 = s->out[1].v2;
s->out[1].v2 = s->out[1].v3;
s->out[1].v3 = s->out[1].fac*s->out[1].v2 - v1 + famp;
v1 = s->out[2].v2;
s->out[2].v2 = s->out[2].v3;
s->out[2].v3 = s->out[2].fac*s->out[2].v2 - v1 + famp;
v1 = s->out[3].v2;
s->out[3].v2 = s->out[3].v3;
s->out[3].v3 = s->out[3].fac*s->out[3].v2 - v1 + famp;
v1 = s->out[4].v2;
s->out[4].v2 = s->out[4].v3;
s->out[4].v3 = s->out[4].fac*s->out[4].v2 - v1 + famp;
v1 = s->out[5].v2;
s->out[5].v2 = s->out[5].v3;
s->out[5].v3 = s->out[5].fac*s->out[5].v2 - v1 + famp;
}
s->current_sample += (limit - sample);
if (s->current_sample < 120)
continue;
/* We are at the end of an MF detection block */
/* Find the two highest energies. The spec says to look for
two tones and two tones only. Taking this literally -ie
only two tones pass the minimum threshold - doesn't work
well. The sinc function mess, due to rectangular windowing
ensure that! Find the two highest energies and ensure they
are considerably stronger than any of the others. */
energy[0] = goertzel_result(&s->out[0]);
energy[1] = goertzel_result(&s->out[1]);
if (energy[0] > energy[1])
{
best = 0;
second_best = 1;
}
else
{
best = 1;
second_best = 0;
}
for (i = 2; i < 6; i++)
{
energy[i] = goertzel_result(&s->out[i]);
if (energy[i] >= energy[best])
{
second_best = best;
best = i;
}
else if (energy[i] >= energy[second_best])
{
second_best = i;
}
}
/* Basic signal level and twist tests */
hit = 0;
if (energy[best] >= BELL_MF_THRESHOLD
&&
energy[second_best] >= BELL_MF_THRESHOLD
&&
energy[best] < energy[second_best]*BELL_MF_TWIST
&&
energy[best]*BELL_MF_TWIST > energy[second_best])
{
/* Relative peak test */
hit = 'X';
for (i = 0; i < 6; i++)
{
if (i != best && i != second_best)
{
if (energy[i]*BELL_MF_RELATIVE_PEAK >= energy[second_best])
{
/* The best two are not clearly the best */
hit = 0;
break;
}
}
}
}
if (hit)
{
/* Get the values into ascending order */
if (second_best < best)
{
i = best;
best = second_best;
second_best = i;
}
best = best*5 + second_best - 1;
hit = bell_mf_positions[best];
/* Look for two successive similar results */
/* The logic in the next test is:
For KP we need 4 successive identical clean detects, with
two blocks of something different preceeding it. For anything
else we need two successive identical clean detects, with
two blocks of something different preceeding it. */
if (hit == s->hits[4]
&&
hit == s->hits[3]
&&
((hit != '*' && hit != s->hits[2] && hit != s->hits[1])
||
(hit == '*' && hit == s->hits[2] && hit != s->hits[1] && hit != s->hits[0])))
{
if (s->current_digits < MAX_BELL_MF_DIGITS)
{
s->digits[s->current_digits++] = (char) hit;
s->digits[s->current_digits] = '\0';
if (s->callback)
{
s->callback(s->callback_data, s->digits, s->current_digits);
s->current_digits = 0;
}
}
else
{
s->lost_digits++;
}
}
}
s->hits[0] = s->hits[1];
s->hits[1] = s->hits[2];
s->hits[2] = s->hits[3];
s->hits[3] = s->hits[4];
s->hits[4] = hit;
/* Reinitialise the detector for the next block */
for (i = 0; i < 6; i++)
goertzel_reset(&s->out[i]);
s->current_sample = 0;
}
if (s->current_digits && s->callback)
{
s->callback(s->callback_data, s->digits, s->current_digits);
s->digits[0] = '\0';
s->current_digits = 0;
}
return 0;
}
static int dtmf_detect (dtmf_detect_state_t *s, int16_t amp[], int samples,
int digitmode, int *writeback, int faxdetect)
{
float row_energy[4];
float col_energy[4];
#ifdef FAX_DETECT
float fax_energy;
#ifdef OLD_DSP_ROUTINES
float fax_energy_2nd;
#endif
#endif /* FAX_DETECT */
float famp;
float v1;
int i;
int j;
int sample;
int best_row;
int best_col;
int hit;
int limit;
hit = 0;
for (sample = 0; sample < samples; sample = limit) {
/* 102 is optimised to meet the DTMF specs. */
if ((samples - sample) >= (102 - s->current_sample))
limit = sample + (102 - s->current_sample);
else
limit = samples;
#if defined(USE_3DNOW)
_dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
_dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
#ifdef OLD_DSP_ROUTINES
_dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
_dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
#endif
/* XXX Need to fax detect for 3dnow too XXX */
#warning "Fax Support Broken"
#else
/* The following unrolled loop takes only 35% (rough estimate) of the
time of a rolled loop on the machine on which it was developed */
for (j=sample;j<limit;j++) {
famp = amp[j];
s->energy += famp*famp;
/* With GCC 2.95, the following unrolled code seems to take about 35%
(rough estimate) as long as a neat little 0-3 loop */
v1 = s->row_out[0].v2;
s->row_out[0].v2 = s->row_out[0].v3;
s->row_out[0].v3 = s->row_out[0].fac*s->row_out[0].v2 - v1 + famp;
v1 = s->col_out[0].v2;
s->col_out[0].v2 = s->col_out[0].v3;
s->col_out[0].v3 = s->col_out[0].fac*s->col_out[0].v2 - v1 + famp;
v1 = s->row_out[1].v2;
s->row_out[1].v2 = s->row_out[1].v3;
s->row_out[1].v3 = s->row_out[1].fac*s->row_out[1].v2 - v1 + famp;
v1 = s->col_out[1].v2;
s->col_out[1].v2 = s->col_out[1].v3;
s->col_out[1].v3 = s->col_out[1].fac*s->col_out[1].v2 - v1 + famp;
v1 = s->row_out[2].v2;
s->row_out[2].v2 = s->row_out[2].v3;
s->row_out[2].v3 = s->row_out[2].fac*s->row_out[2].v2 - v1 + famp;
v1 = s->col_out[2].v2;
s->col_out[2].v2 = s->col_out[2].v3;
s->col_out[2].v3 = s->col_out[2].fac*s->col_out[2].v2 - v1 + famp;
v1 = s->row_out[3].v2;
s->row_out[3].v2 = s->row_out[3].v3;
s->row_out[3].v3 = s->row_out[3].fac*s->row_out[3].v2 - v1 + famp;
v1 = s->col_out[3].v2;
s->col_out[3].v2 = s->col_out[3].v3;
s->col_out[3].v3 = s->col_out[3].fac*s->col_out[3].v2 - v1 + famp;
#ifdef FAX_DETECT
/* Update fax tone */
v1 = s->fax_tone.v2;
s->fax_tone.v2 = s->fax_tone.v3;
s->fax_tone.v3 = s->fax_tone.fac*s->fax_tone.v2 - v1 + famp;
#endif /* FAX_DETECT */
#ifdef OLD_DSP_ROUTINES
v1 = s->col_out2nd[0].v2;
s->col_out2nd[0].v2 = s->col_out2nd[0].v3;
s->col_out2nd[0].v3 = s->col_out2nd[0].fac*s->col_out2nd[0].v2 - v1 + famp;
v1 = s->row_out2nd[0].v2;
s->row_out2nd[0].v2 = s->row_out2nd[0].v3;
s->row_out2nd[0].v3 = s->row_out2nd[0].fac*s->row_out2nd[0].v2 - v1 + famp;
v1 = s->col_out2nd[1].v2;
s->col_out2nd[1].v2 = s->col_out2nd[1].v3;
s->col_out2nd[1].v3 = s->col_out2nd[1].fac*s->col_out2nd[1].v2 - v1 + famp;
v1 = s->row_out2nd[1].v2;
s->row_out2nd[1].v2 = s->row_out2nd[1].v3;
s->row_out2nd[1].v3 = s->row_out2nd[1].fac*s->row_out2nd[1].v2 - v1 + famp;
v1 = s->col_out2nd[2].v2;
s->col_out2nd[2].v2 = s->col_out2nd[2].v3;
s->col_out2nd[2].v3 = s->col_out2nd[2].fac*s->col_out2nd[2].v2 - v1 + famp;
v1 = s->row_out2nd[2].v2;
s->row_out2nd[2].v2 = s->row_out2nd[2].v3;
s->row_out2nd[2].v3 = s->row_out2nd[2].fac*s->row_out2nd[2].v2 - v1 + famp;
v1 = s->col_out2nd[3].v2;
s->col_out2nd[3].v2 = s->col_out2nd[3].v3;
s->col_out2nd[3].v3 = s->col_out2nd[3].fac*s->col_out2nd[3].v2 - v1 + famp;
v1 = s->row_out2nd[3].v2;
s->row_out2nd[3].v2 = s->row_out2nd[3].v3;
s->row_out2nd[3].v3 = s->row_out2nd[3].fac*s->row_out2nd[3].v2 - v1 + famp;
#ifdef FAX_DETECT
/* Update fax tone */
v1 = s->fax_tone.v2;
s->fax_tone2nd.v2 = s->fax_tone2nd.v3;
s->fax_tone2nd.v3 = s->fax_tone2nd.fac*s->fax_tone2nd.v2 - v1 + famp;
#endif /* FAX_DETECT */
#endif
}
#endif
s->current_sample += (limit - sample);
if (s->current_sample < 102) {
if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) {
/* If we had a hit last time, go ahead and clear this out since likely it
will be another hit */
for (i=sample;i<limit;i++)
amp[i] = 0;
*writeback = 1;
}
continue;
}
#ifdef FAX_DETECT
/* Detect the fax energy, too */
fax_energy = goertzel_result(&s->fax_tone);
#endif
/* We are at the end of a DTMF detection block */
/* Find the peak row and the peak column */
row_energy[0] = goertzel_result (&s->row_out[0]);
col_energy[0] = goertzel_result (&s->col_out[0]);
for (best_row = best_col = 0, i = 1; i < 4; i++) {
row_energy[i] = goertzel_result (&s->row_out[i]);
if (row_energy[i] > row_energy[best_row])
best_row = i;
col_energy[i] = goertzel_result (&s->col_out[i]);
if (col_energy[i] > col_energy[best_col])
best_col = i;
}
hit = 0;
/* Basic signal level test and the twist test */
if (row_energy[best_row] >= DTMF_THRESHOLD &&
col_energy[best_col] >= DTMF_THRESHOLD &&
col_energy[best_col] < row_energy[best_row]*DTMF_REVERSE_TWIST &&
col_energy[best_col]*DTMF_NORMAL_TWIST > row_energy[best_row]) {
/* Relative peak test */
for (i = 0; i < 4; i++) {
if ((i != best_col &&
col_energy[i]*DTMF_RELATIVE_PEAK_COL > col_energy[best_col]) ||
(i != best_row
&& row_energy[i]*DTMF_RELATIVE_PEAK_ROW > row_energy[best_row])) {
break;
}
}
#ifdef OLD_DSP_ROUTINES
/* ... and second harmonic test */
if (i >= 4 &&
(row_energy[best_row] + col_energy[best_col]) > 42.0*s->energy &&
goertzel_result(&s->col_out2nd[best_col])*DTMF_2ND_HARMONIC_COL < col_energy[best_col]
&& goertzel_result(&s->row_out2nd[best_row])*DTMF_2ND_HARMONIC_ROW < row_energy[best_row]) {
#else
/* ... and fraction of total energy test */
if (i >= 4 &&
(row_energy[best_row] + col_energy[best_col]) > DTMF_TO_TOTAL_ENERGY*s->energy) {
#endif
/* Got a hit */
hit = dtmf_positions[(best_row << 2) + best_col];
if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
/* Zero out frame data if this is part DTMF */
for (i=sample;i<limit;i++)
amp[i] = 0;
*writeback = 1;
}
/* Look for two successive similar results */
/* The logic in the next test is:
We need two successive identical clean detects, with
something different preceeding it. This can work with
back to back differing digits. More importantly, it
can work with nasty phones that give a very wobbly start
to a digit */
#ifdef OLD_DSP_ROUTINES
if (hit == s->hit3 && s->hit3 != s->hit2) {
s->mhit = hit;
s->digit_hits[(best_row << 2) + best_col]++;
s->detected_digits++;
if (s->current_digits < MAX_DTMF_DIGITS) {
s->digits[s->current_digits++] = hit;
s->digits[s->current_digits] = '\0';
} else {
s->lost_digits++;
}
}
#else
if (hit == s->hits[2] && hit != s->hits[1] && hit != s->hits[0]) {
s->mhit = hit;
s->digit_hits[(best_row << 2) + best_col]++;
s->detected_digits++;
if (s->current_digits < MAX_DTMF_DIGITS) {
s->digits[s->current_digits++] = hit;
s->digits[s->current_digits] = '\0';
} else {
s->lost_digits++;
}
}
#endif
}
}
#ifdef FAX_DETECT
if (!hit && (fax_energy >= FAX_THRESHOLD) &&
(fax_energy >= DTMF_TO_TOTAL_ENERGY*s->energy) &&
(faxdetect)) {
#if 0
printf("Fax energy/Second Harmonic: %f\n", fax_energy);
#endif
/* XXX Probably need better checking than just this the energy XXX */
hit = 'f';
s->fax_hits++;
} else {
if (s->fax_hits > 5) {
hit = 'f';
s->mhit = 'f';
s->detected_digits++;
if (s->current_digits < MAX_DTMF_DIGITS) {
s->digits[s->current_digits++] = hit;
s->digits[s->current_digits] = '\0';
} else {
s->lost_digits++;
}
}
s->fax_hits = 0;
}
#endif /* FAX_DETECT */
#ifdef OLD_DSP_ROUTINES
s->hit1 = s->hit2;
s->hit2 = s->hit3;
s->hit3 = hit;
#else
s->hits[0] = s->hits[1];
s->hits[1] = s->hits[2];
s->hits[2] = hit;
#endif
/* Reinitialise the detector for the next block */
for (i = 0; i < 4; i++) {
goertzel_reset(&s->row_out[i]);
goertzel_reset(&s->col_out[i]);
#ifdef OLD_DSP_ROUTINES
goertzel_reset(&s->row_out2nd[i]);
goertzel_reset(&s->col_out2nd[i]);
#endif
}
#ifdef FAX_DETECT
goertzel_reset (&s->fax_tone);
#ifdef OLD_DSP_ROUTINES
goertzel_reset (&s->fax_tone2nd);
#endif
#endif
s->energy = 0.0;
s->current_sample = 0;
}
if ((!s->mhit) || (s->mhit != hit)) {
s->mhit = 0;
return(0);
}
return (hit);
}
/* MF goertzel size */
#ifdef OLD_DSP_ROUTINES
#define MF_GSIZE 160
#else
#define MF_GSIZE 120
#endif
static int mf_detect (mf_detect_state_t *s, int16_t amp[],
int samples, int digitmode, int *writeback)
{
#ifdef OLD_DSP_ROUTINES
float tone_energy[6];
int best1;
int best2;
float max;
int sofarsogood;
#else
float energy[6];
int best;
int second_best;
#endif
float famp;
float v1;
int i;
int j;
int sample;
int hit;
int limit;
hit = 0;
for (sample = 0; sample < samples; sample = limit) {
/* 80 is optimised to meet the MF specs. */
if ((samples - sample) >= (MF_GSIZE - s->current_sample))
limit = sample + (MF_GSIZE - s->current_sample);
else
limit = samples;
#if defined(USE_3DNOW)
_dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
_dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
#ifdef OLD_DSP_ROUTINES
_dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
_dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
#endif
/* XXX Need to fax detect for 3dnow too XXX */
#warning "Fax Support Broken"
#else
/* The following unrolled loop takes only 35% (rough estimate) of the
time of a rolled loop on the machine on which it was developed */
for (j = sample; j < limit; j++) {
famp = amp[j];
#ifdef OLD_DSP_ROUTINES
s->energy += famp*famp;
#endif
/* With GCC 2.95, the following unrolled code seems to take about 35%
(rough estimate) as long as a neat little 0-3 loop */
v1 = s->tone_out[0].v2;
s->tone_out[0].v2 = s->tone_out[0].v3;
s->tone_out[0].v3 = s->tone_out[0].fac*s->tone_out[0].v2 - v1 + famp;
v1 = s->tone_out[1].v2;
s->tone_out[1].v2 = s->tone_out[1].v3;
s->tone_out[1].v3 = s->tone_out[1].fac*s->tone_out[1].v2 - v1 + famp;
v1 = s->tone_out[2].v2;
s->tone_out[2].v2 = s->tone_out[2].v3;
s->tone_out[2].v3 = s->tone_out[2].fac*s->tone_out[2].v2 - v1 + famp;
v1 = s->tone_out[3].v2;
s->tone_out[3].v2 = s->tone_out[3].v3;
s->tone_out[3].v3 = s->tone_out[3].fac*s->tone_out[3].v2 - v1 + famp;
v1 = s->tone_out[4].v2;
s->tone_out[4].v2 = s->tone_out[4].v3;
s->tone_out[4].v3 = s->tone_out[4].fac*s->tone_out[4].v2 - v1 + famp;
v1 = s->tone_out[5].v2;
s->tone_out[5].v2 = s->tone_out[5].v3;
s->tone_out[5].v3 = s->tone_out[5].fac*s->tone_out[5].v2 - v1 + famp;
#ifdef OLD_DSP_ROUTINES
v1 = s->tone_out2nd[0].v2;
s->tone_out2nd[0].v2 = s->tone_out2nd[0].v3;
s->tone_out2nd[0].v3 = s->tone_out2nd[0].fac*s->tone_out2nd[0].v2 - v1 + famp;
v1 = s->tone_out2nd[1].v2;
s->tone_out2nd[1].v2 = s->tone_out2nd[1].v3;
s->tone_out2nd[1].v3 = s->tone_out2nd[1].fac*s->tone_out2nd[1].v2 - v1 + famp;
v1 = s->tone_out2nd[2].v2;
s->tone_out2nd[2].v2 = s->tone_out2nd[2].v3;
s->tone_out2nd[2].v3 = s->tone_out2nd[2].fac*s->tone_out2nd[2].v2 - v1 + famp;
v1 = s->tone_out2nd[3].v2;
s->tone_out2nd[3].v2 = s->tone_out2nd[3].v3;
s->tone_out2nd[3].v3 = s->tone_out2nd[3].fac*s->tone_out2nd[3].v2 - v1 + famp;
v1 = s->tone_out2nd[4].v2;
s->tone_out2nd[4].v2 = s->tone_out2nd[4].v3;
s->tone_out2nd[4].v3 = s->tone_out2nd[4].fac*s->tone_out2nd[2].v2 - v1 + famp;
v1 = s->tone_out2nd[3].v2;
s->tone_out2nd[5].v2 = s->tone_out2nd[6].v3;
s->tone_out2nd[5].v3 = s->tone_out2nd[6].fac*s->tone_out2nd[3].v2 - v1 + famp;
#endif
}
#endif
s->current_sample += (limit - sample);
if (s->current_sample < MF_GSIZE) {
if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) {
/* If we had a hit last time, go ahead and clear this out since likely it
will be another hit */
for (i=sample;i<limit;i++)
amp[i] = 0;
*writeback = 1;
}
continue;
}
#ifdef OLD_DSP_ROUTINES
/* We're at the end of an MF detection block. Go ahead and calculate
all the energies. */
for (i=0;i<6;i++) {
tone_energy[i] = goertzel_result(&s->tone_out[i]);
}
/* Find highest */
best1 = 0;
max = tone_energy[0];
for (i=1;i<6;i++) {
if (tone_energy[i] > max) {
max = tone_energy[i];
best1 = i;
}
}
/* Find 2nd highest */
if (best1) {
max = tone_energy[0];
best2 = 0;
} else {
max = tone_energy[1];
best2 = 1;
}
for (i=0;i<6;i++) {
if (i == best1) continue;
if (tone_energy[i] > max) {
max = tone_energy[i];
best2 = i;
}
}
hit = 0;
if (best1 != best2)
sofarsogood=1;
else
sofarsogood=0;
/* Check for relative energies */
for (i=0;i<6;i++) {
if (i == best1)
continue;
if (i == best2)
continue;
if (tone_energy[best1] < tone_energy[i] * MF_RELATIVE_PEAK) {
sofarsogood = 0;
break;
}
if (tone_energy[best2] < tone_energy[i] * MF_RELATIVE_PEAK) {
sofarsogood = 0;
break;
}
}
if (sofarsogood) {
/* Check for 2nd harmonic */
if (goertzel_result(&s->tone_out2nd[best1]) * MF_2ND_HARMONIC > tone_energy[best1])
sofarsogood = 0;
else if (goertzel_result(&s->tone_out2nd[best2]) * MF_2ND_HARMONIC > tone_energy[best2])
sofarsogood = 0;
}
if (sofarsogood) {
hit = mf_hit[best1][best2];
if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
/* Zero out frame data if this is part DTMF */
for (i=sample;i<limit;i++)
amp[i] = 0;
*writeback = 1;
}
/* Look for two consecutive clean hits */
if ((hit == s->hit3) && (s->hit3 != s->hit2)) {
s->mhit = hit;
s->detected_digits++;
if (s->current_digits < MAX_DTMF_DIGITS - 2) {
s->digits[s->current_digits++] = hit;
s->digits[s->current_digits] = '\0';
} else {
s->lost_digits++;
}
}
}
s->hit1 = s->hit2;
s->hit2 = s->hit3;
s->hit3 = hit;
/* Reinitialise the detector for the next block */
for (i = 0; i < 6; i++) {
goertzel_reset(&s->tone_out[i]);
goertzel_reset(&s->tone_out2nd[i]);
}
s->energy = 0.0;
s->current_sample = 0;
}
#else
/* We're at the end of an MF detection block. */
/* Find the two highest energies. The spec says to look for
two tones and two tones only. Taking this literally -ie
only two tones pass the minimum threshold - doesn't work
well. The sinc function mess, due to rectangular windowing
ensure that! Find the two highest energies and ensure they
are considerably stronger than any of the others. */
energy[0] = goertzel_result(&s->tone_out[0]);
energy[1] = goertzel_result(&s->tone_out[1]);
if (energy[0] > energy[1]) {
best = 0;
second_best = 1;
} else {
best = 1;
second_best = 0;
}
/*endif*/
for (i=2;i<6;i++) {
energy[i] = goertzel_result(&s->tone_out[i]);
if (energy[i] >= energy[best]) {
second_best = best;
best = i;
} else if (energy[i] >= energy[second_best]) {
second_best = i;
}
}
/* Basic signal level and twist tests */
hit = 0;
if (energy[best] >= BELL_MF_THRESHOLD && energy[second_best] >= BELL_MF_THRESHOLD
&& energy[best] < energy[second_best]*BELL_MF_TWIST
&& energy[best]*BELL_MF_TWIST > energy[second_best]) {
/* Relative peak test */
hit = -1;
for (i=0;i<6;i++) {
if (i != best && i != second_best) {
if (energy[i]*BELL_MF_RELATIVE_PEAK >= energy[second_best]) {
/* The best two are not clearly the best */
hit = 0;
break;
}
}
}
}
if (hit) {
/* Get the values into ascending order */
if (second_best < best) {
i = best;
best = second_best;
second_best = i;
}
best = best*5 + second_best - 1;
hit = bell_mf_positions[best];
/* Look for two successive similar results */
/* The logic in the next test is:
For KP we need 4 successive identical clean detects, with
two blocks of something different preceeding it. For anything
else we need two successive identical clean detects, with
two blocks of something different preceeding it. */
if (hit == s->hits[4] && hit == s->hits[3] &&
((hit != '*' && hit != s->hits[2] && hit != s->hits[1])||
(hit == '*' && hit == s->hits[2] && hit != s->hits[1] &&
hit != s->hits[0]))) {
s->detected_digits++;
if (s->current_digits < MAX_DTMF_DIGITS) {
s->digits[s->current_digits++] = hit;
s->digits[s->current_digits] = '\0';
} else {
s->lost_digits++;
}
}
} else {
hit = 0;
}
s->hits[0] = s->hits[1];
s->hits[1] = s->hits[2];
s->hits[2] = s->hits[3];
s->hits[3] = s->hits[4];
s->hits[4] = hit;
/* Reinitialise the detector for the next block */
for (i = 0; i < 6; i++)
goertzel_reset(&s->tone_out[i]);
s->current_sample = 0;
}
int CGoertzelDTMFFax::fax_detect
(
double amp[],
int samples
)
{
double fax_energy;
double fax_energy_2nd;
double famp;
float v1;
int i;
int j;
int sample;
int hit;
int limit;
hit = 0;
for (sample = 0; sample < samples; sample = limit)
{
/* 102 is optimised to meet the DTMF specs. */
if ((samples - sample) >= (102 - state->current_sample))
limit = sample + (102 - state->current_sample);
else
limit = samples;
for (j=sample;j<limit;j++)
{
famp = amp[j];
state->energy += famp*famp;
//goertzel_sample(&(state->fax_tone), famp);
v1 = state->fax_tone.v2;
state->fax_tone.v2 = state->fax_tone.v3;
state->fax_tone.v3 = state->fax_tone.fac*state->fax_tone.v2 - v1 + famp;
//goertzel_sample(&(state->fax_tone2nd), famp);
v1 = state->fax_tone.v2;
state->fax_tone2nd.v2 = state->fax_tone2nd.v3;
state->fax_tone2nd.v3 = state->fax_tone2nd.fac*state->fax_tone2nd.v2 - v1 + famp;
}
state->current_sample += (limit - sample);
/* Detect the fax energy */
fax_energy = goertzel_result(&state->fax_tone);
hit = 0;
if ((fax_energy >= FAX_THRESHOLD) &&
(fax_energy >= DTMF_TO_TOTAL_ENERGY*state->energy))
{
//Bug here...
//Test the 2nd harmonic too
/*fax_energy_2nd = goertzel_result(&state->fax_tone2nd);
if (fax_energy_2nd * FAX_2ND_HARMONIC < fax_energy)
{
Probably need better checking than just this the energy
}*/
hit = 'f';
state->fax_hits++;
//Enough of it...
if (state->fax_hits > 5)
{
break;
}
}
goertzel_reset (&state->fax_tone);
goertzel_reset (&state->fax_tone2nd);
state->energy = 0.0;
state->current_sample = 0;
}
return (hit);
}
