kenmirrors
/
freeswitch
mirror of https://github.com/signalwire/freeswitch.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

FS-9009 [mod_avmd] Amplitude estimation 

Add DESA-2 estimation of signal amplitude
This commit is contained in:
Piotr Gregor 2016-07-04 16:26:08 +01:00
parent 7f623fc0c8
commit a77387456d
8 changed files with 114 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/mod/applications/mod_avmd/avmd_amplitude.c
View File

@ -5,15 +5,8 @@
#include "avmd_amplitude.h"
#include "avmd_psi.h"
/*! \brief
* @author Eric des Courtis
* @param b A circular audio sample buffer
* @param i Position in the buffer
* @param f Frequency estimate
* @return The amplitude at position i
*/
extern double avmd_amplitude(circ_buffer_t *b, size_t i, double f)
{
double avmd_amplitude(circ_buffer_t *b, size_t i, double f) {
double result;
result = sqrt(PSI(b, i) / sin(f * f));
return result;

9
src/mod/applications/mod_avmd/avmd_amplitude.h
View File

@ -1,7 +1,10 @@
/*
* @brief Estimation of amplitude using DESA-2 algorithm.
* @author Eric des Courtis
* @par Modifications: Piotr Gregor < piotrek.gregor gmail.com >
*
* Contributor(s):
*
* Eric des Courtis <eric.des.courtis@benbria.com>
* Piotr Gregor <piotrek.gregor gmail.com>:
*/
@ -12,7 +15,7 @@
#include "avmd_buffer.h"
extern double avmd_amplitude(circ_buffer_t *, size_t i, double f);
double avmd_amplitude(circ_buffer_t *, size_t i, double f) __attribute__ ((nonnull(1)));
#endif /* __AVMD_AMPLITUDE_H__ */

20
src/mod/applications/mod_avmd/avmd_desa2.c
View File

@ -19,8 +19,8 @@ int __isnan(double);
#include "avmd_fast_acosf.h"
#endif
extern double avmd_desa2(circ_buffer_t *b, size_t i)
{
double avmd_desa2(circ_buffer_t *b, size_t i, double *amplitude) {
double d;
double n;
double x0;
@ -30,6 +30,7 @@ extern double avmd_desa2(circ_buffer_t *b, size_t i)
double x4;
double x2sq;
double result;
double PSI_Xn, PSI_Yn, NEEDED;
x0 = GET_SAMPLE((b), (i));
x1 = GET_SAMPLE((b), ((i) + 1));
@ -39,8 +40,14 @@ extern double avmd_desa2(circ_buffer_t *b, size_t i)
x2sq = x2 * x2;
d = 2.0 * ((x2sq) - (x1 * x3));
if (d == 0.0) return 0.0;
n = ((x2sq) - (x0 * x4)) - ((x1 * x1) - (x0 * x2)) - ((x3 * x3) - (x2 * x4));
if (d == 0.0) {
*amplitude = 0.0;
return 0.0;
}
PSI_Xn = ((x2sq) - (x0 * x4));
NEEDED = ((x1 * x1) - (x0 * x2)) + ((x3 * x3) - (x2 * x4));
n = ((x2sq) - (x0 * x4)) - NEEDED;
PSI_Yn = NEEDED + PSI_Xn;
#ifdef AVMD_FAST_MATH
result = 0.5 * (double)fast_acosf((float)n/d);
@ -48,7 +55,10 @@ extern double avmd_desa2(circ_buffer_t *b, size_t i)
result = 0.5 * acos(n/d);
#endif
if (ISNAN(result)) result = 0.0;
if (ISNAN(result)) {
result = 0.0;
}
*amplitude = 2.0 * PSI_Xn / sqrt(PSI_Yn);
return result;

11
src/mod/applications/mod_avmd/avmd_desa2.h
View File

@ -1,7 +1,10 @@
/*
* @brief DESA-2 algorithm implementation.
* @author Eric des Courtis
* @par Modifications: Piotr Gregor < piotrek.gregor gmail.com >
*
* Contributor(s):
*
* Eric des Courtis <eric.des.courtis@benbria.com>
* Piotr Gregor <piotrek.gregor gmail.com>:
*/
@ -12,8 +15,8 @@
#include <math.h>
#include "avmd_buffer.h"
/* Returns digital frequency estimation. */
extern double avmd_desa2(circ_buffer_t *b, size_t i);
/* Returns digital frequency estimation and amplitude estimation. */
extern double avmd_desa2(circ_buffer_t *b, size_t i, double *amplitude) __attribute__ ((nonnull(1,3)));
#endif /* __AVMD_DESA2_H__ */

21
src/mod/applications/mod_avmd/avmd_desa2_tweaked.c
View File

@ -19,11 +19,10 @@ int __isnan(double);
#include "avmd_fast_acosf.h"
#endif
double
avmd_desa2_tweaked(circ_buffer_t *b, size_t i)
{
double d;
double n;
avmd_desa2_tweaked(circ_buffer_t *b, size_t i, double *amplitude) {
double n, d;
double x0;
double x1;
double x2;
@ -31,6 +30,7 @@ avmd_desa2_tweaked(circ_buffer_t *b, size_t i)
double x4;
double x2sq;
double result;
double PSI_Xn, PSI_Yn, NEEDED;
x0 = GET_SAMPLE((b), (i));
x1 = GET_SAMPLE((b), ((i) + 1));
@ -39,8 +39,10 @@ avmd_desa2_tweaked(circ_buffer_t *b, size_t i)
x4 = GET_SAMPLE((b), ((i) + 4));
x2sq = x2 * x2;
d = 2.0 * ((x2sq) - (x1 * x3));
n = ((x2sq) - (x0 * x4)) - ((x1 * x1)
- (x0 * x2)) - ((x3 * x3) - (x2 * x4));
PSI_Xn = ((x2sq) - (x0 * x4));
NEEDED = ((x1 * x1) - (x0 * x2)) + ((x3 * x3) - (x2 * x4));
n = ((x2sq) - (x0 * x4)) - NEEDED;
PSI_Yn = NEEDED + PSI_Xn;
/* instead of
#ifdef FASTMATH
@ -52,11 +54,14 @@ avmd_desa2_tweaked(circ_buffer_t *b, size_t i)
result = n/d;
if (ISINF(result)) {
if (n < 0.0)
*amplitude = 0.0;
if (n < 0.0) {
return -10.0;
else
} else {
return 10.0;
}
}
*amplitude = 2.0 * PSI_Xn / sqrt(PSI_Yn);
return result;
}

5
src/mod/applications/mod_avmd/avmd_desa2_tweaked.h
View File

@ -33,9 +33,10 @@
* to corresponding frequencies is nonlinear.
* The actual frequency estimation can be retrieved later
* from this partial result using
* 0.5 * acos(n/d)
* 0.5 * acos(returned_value)
* Amplitude estimation is returned by address.
*/
double avmd_desa2_tweaked(circ_buffer_t *b, size_t i);
double avmd_desa2_tweaked(circ_buffer_t *b, size_t i, double *amplitude) __attribute__ ((nonnull(1,3)));
#endif /* __AVMD_DESA2_TWEAKED_H__ */

4
src/mod/applications/mod_avmd/avmd_psi.h
View File

@ -4,6 +4,8 @@
#include "avmd_buffer.h"
#define PSI(b, i) (GET_SAMPLE((b), ((i) + 1))*GET_SAMPLE((b), ((i) + 1))-GET_SAMPLE((b), ((i) + 2))*GET_SAMPLE((b), ((i) + 0)))
#define PSI(b, i) (GET_SAMPLE((b), ((i) + 1)) * GET_SAMPLE((b), ((i) + 1)) - GET_SAMPLE((b), ((i) + 2)) * GET_SAMPLE((b), ((i) + 0)))
#endif /* __AVMD_PSI_H__ */

90
src/mod/applications/mod_avmd/mod_avmd.c
View File

@ -175,6 +175,8 @@ typedef struct {
circ_buffer_t b;
sma_buffer_t sma_b;
sma_buffer_t sqa_b;
sma_buffer_t sma_amp_b;
sma_buffer_t sqa_amp_b;
size_t pos;
double f;
/* freq_table_t ft; */
@ -199,7 +201,7 @@ static switch_status_t avmd_register_all_events(void);
static void avmd_unregister_all_events(void);
static void
avmd_fire_event(enum avmd_event type, switch_core_session_t *fs_s, double freq, double v, avmd_beep_state_t beep_status, uint8_t info);
avmd_fire_event(enum avmd_event type, switch_core_session_t *fs_s, double freq, double v_freq, double amp, double v_amp, avmd_beep_state_t beep_status, uint8_t info);
/* API [set default], reset to factory settings */
static void avmd_set_xml_default_configuration(switch_mutex_t *mutex);
@ -276,6 +278,20 @@ init_avmd_session_data(avmd_session_t *avmd_session, switch_core_session_t *fs_s
goto end;
}
memset(avmd_session->sqa_b.data, 0, sizeof(BUFF_TYPE) * buf_sz);
INIT_SMA_BUFFER(&avmd_session->sma_amp_b, buf_sz, fs_session);
if (avmd_session->sma_amp_b.data == NULL) {
status = SWITCH_STATUS_FALSE;
goto end;
}
memset(avmd_session->sma_amp_b.data, 0, sizeof(BUFF_TYPE) * buf_sz);
INIT_SMA_BUFFER(&avmd_session->sqa_amp_b, buf_sz, fs_session);
if (avmd_session->sqa_amp_b.data == NULL) {
status = SWITCH_STATUS_FALSE;
goto end;
}
memset(avmd_session->sqa_amp_b.data, 0, sizeof(BUFF_TYPE) * buf_sz);
end:
if (mutex != NULL)
{
@ -407,8 +423,7 @@ avmd_unregister_all_events(void)
}
static void
avmd_fire_event(enum avmd_event type, switch_core_session_t *fs_s, double freq, double v, avmd_beep_state_t beep_status, uint8_t info)
{
avmd_fire_event(enum avmd_event type, switch_core_session_t *fs_s, double freq, double v_freq, double amp, double v_amp, avmd_beep_state_t beep_status, uint8_t info) {
int res;
switch_event_t *event;
switch_status_t status;
@ -421,23 +436,36 @@ avmd_fire_event(enum avmd_event type, switch_core_session_t *fs_s, double freq,
}
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "Unique-ID", switch_core_session_get_uuid(fs_s));
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "call-command", "avmd");
switch (type)
{
switch (type) {
case AVMD_EVENT_BEEP:
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "Beep-Status", "DETECTED");
res = snprintf(buf, AVMD_CHAR_BUF_LEN, "%f", freq);
if (res < 0 || res > AVMD_CHAR_BUF_LEN - 1) {
switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(fs_s), SWITCH_LOG_ERROR, "Frequency truncated [%s], [%d] attempted!\n", buf, res);
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "frequency", "ERROR (TRUNCATED)");
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "Frequency", "ERROR (TRUNCATED)");
}
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "frequency", buf);
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "Frequency", buf);
res = snprintf(buf, AVMD_CHAR_BUF_LEN, "%f", v);
res = snprintf(buf, AVMD_CHAR_BUF_LEN, "%f", v_freq);
if (res < 0 || res > AVMD_CHAR_BUF_LEN - 1) {
switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(fs_s), SWITCH_LOG_ERROR, "Error, truncated [%s], [%d] attempeted!\n", buf, res);
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "variance", "ERROR (TRUNCATED)");
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "Frequency-variance", "ERROR (TRUNCATED)");
}
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "variance", buf);
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "Frequency-variance", buf);
res = snprintf(buf, AVMD_CHAR_BUF_LEN, "%f", amp);
if (res < 0 || res > AVMD_CHAR_BUF_LEN - 1) {
switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(fs_s), SWITCH_LOG_ERROR, "Amplitude truncated [%s], [%d] attempted!\n", buf, res);
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "Amplitude", "ERROR (TRUNCATED)");
}
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "Amplitude", buf);
res = snprintf(buf, AVMD_CHAR_BUF_LEN, "%f", v_amp);
if (res < 0 || res > AVMD_CHAR_BUF_LEN - 1) {
switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(fs_s), SWITCH_LOG_ERROR, "Error, truncated [%s], [%d] attempeted!\n", buf, res);
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "Amplitude-variance", "ERROR (TRUNCATED)");
}
switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "Amplitude-variance", buf);
break;
case AVMD_EVENT_SESSION_START:
@ -1055,7 +1083,7 @@ SWITCH_STANDARD_APP(avmd_start_app)
goto end;
}
switch_channel_set_private(channel, "_avmd_", bug); /* Set the avmd tag to detect an existing avmd media bug */
avmd_fire_event(AVMD_EVENT_SESSION_START, session, 0, 0, 0, 0);
avmd_fire_event(AVMD_EVENT_SESSION_START, session, 0, 0, 0, 0, 0, 0);
if (avmd_session->settings.report_status == 1) {
switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(session), SWITCH_LOG_INFO, "Avmd on channel [%s] started!\n", switch_channel_get_name(channel));
}
@ -1113,9 +1141,9 @@ SWITCH_STANDARD_APP(avmd_stop_app)
switch_channel_set_private(channel, "_avmd_", NULL);
switch_core_media_bug_remove(session, &bug);
if (avmd_found == 1) {
avmd_fire_event(AVMD_EVENT_SESSION_STOP, session, 0, 0, beep_status, 1);
avmd_fire_event(AVMD_EVENT_SESSION_STOP, session, 0, 0, 0, 0, beep_status, 1);
} else {
avmd_fire_event(AVMD_EVENT_SESSION_STOP, session, 0, 0, beep_status, 0);
avmd_fire_event(AVMD_EVENT_SESSION_STOP, session, 0, 0, 0, 0, beep_status, 0);
}
if (report_status == 1) {
switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(session), SWITCH_LOG_INFO, "Avmd on channel [%s] stopped, beep status: [%s]\n",
@ -1379,7 +1407,7 @@ SWITCH_STANDARD_API(avmd_api_main)
uuid_dup = switch_core_strdup(switch_core_session_get_pool(fs_session), uuid);
switch_channel_set_private(channel, "_avmd_", NULL);
switch_core_media_bug_remove(fs_session, &bug);
avmd_fire_event(AVMD_EVENT_SESSION_STOP, fs_session, 0, 0, 0, 0);
avmd_fire_event(AVMD_EVENT_SESSION_STOP, fs_session, 0, 0, 0, 0, 0, 0);
if (avmd_globals.settings.report_status == 1) {
stream->write_function(stream, "+OK\n [%s] [%s] stopped\n\n",
uuid_dup, switch_channel_get_name(channel));
@ -1500,7 +1528,7 @@ SWITCH_STANDARD_API(avmd_api_main)
switch_channel_set_private(channel, "_avmd_", bug); /* Set the vmd tag to detect an existing vmd media bug */
avmd_fire_event(AVMD_EVENT_SESSION_START, fs_session, 0, 0, 0, 0);
avmd_fire_event(AVMD_EVENT_SESSION_START, fs_session, 0, 0, 0, 0, 0, 0);
if (avmd_globals.settings.report_status == 1) {
stream->write_function(stream, "+OK\n [%s] [%s] started!\n\n",
uuid, switch_channel_get_name(channel));
@ -1529,9 +1557,9 @@ static void avmd_process(avmd_session_t *s, switch_frame_t *frame) {
switch_channel_t *channel;
circ_buffer_t *b;
size_t pos;
double omega;
double omega, amplitude;
double f;
double v;
double v, v_amp;
double sma_digital_freq;
uint32_t sine_len_i;
int sample_to_skip_n = s->settings.sample_n_to_skip;
@ -1559,7 +1587,7 @@ static void avmd_process(avmd_session_t *s, switch_frame_t *frame) {
/*for (pos = session->pos; pos < (GET_CURRENT_POS(b) - P); pos++) { */
if ((sample_n % sine_len_i) == 0) {
/* Get a desa2 frequency estimate every sine len */
omega = avmd_desa2_tweaked(b, pos + sample_n);
omega = avmd_desa2_tweaked(b, pos + sample_n, &amplitude);
if (omega < -0.999999 || omega > 0.999999) {
if (s->settings.debug == 1) {
@ -1569,6 +1597,8 @@ static void avmd_process(avmd_session_t *s, switch_frame_t *frame) {
if (s->settings.require_continuous_streak == 1) {
RESET_SMA_BUFFER(&s->sma_b);
RESET_SMA_BUFFER(&s->sqa_b);
RESET_SMA_BUFFER(&s->sma_amp_b);
RESET_SMA_BUFFER(&s->sqa_amp_b);
s->samples_streak = s->settings.sample_n_continuous_streak;
sample_to_skip_n = s->settings.sample_n_to_skip;
}
@ -1603,12 +1633,15 @@ static void avmd_process(avmd_session_t *s, switch_frame_t *frame) {
APPEND_SMA_VAL(&s->sma_b, omega); /* append */
APPEND_SMA_VAL(&s->sqa_b, omega * omega);
APPEND_SMA_VAL(&s->sma_amp_b, amplitude); /* append */
APPEND_SMA_VAL(&s->sqa_amp_b, amplitude * amplitude);
if (s->settings.require_continuous_streak == 1) {
if (s->samples_streak > 0) {
--s->samples_streak;
}
}
v = s->sqa_b.sma - (s->sma_b.sma * s->sma_b.sma); /* calculate variance (biased estimator) */
v = s->sqa_b.sma - (s->sma_b.sma * s->sma_b.sma); /* calculate variance of omega(biased estimator) */
v_amp = s->sqa_amp_b.sma - (s->sma_amp_b.sma * s->sma_amp_b.sma); /* calculate variance of amplitude (biased estimator) */
if (s->settings.debug == 1) {
#if !defined(WIN32) && defined(AVMD_FAST_MATH)
f = 0.5 * (double) fast_acosf((float)omega);
@ -1619,11 +1652,16 @@ static void avmd_process(avmd_session_t *s, switch_frame_t *frame) {
#endif /* !WIN32 && AVMD_FAST_MATH */
if (s->settings.require_continuous_streak == 1) {
switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(s->session), SWITCH_LOG_DEBUG, "<<< AVMD v[%.10f]\tomega[%f]\tf[%f] [%f]Hz\t\tsma[%f][%f]Hz\t\tsqa[%f]\t"
"streak[%zu] pos[%zu] sample_n[%zu] lpos[%zu] s[%zu]>>>\n", v, omega, f, TO_HZ(s->rate, f), s->sma_b.sma, TO_HZ(s->rate, sma_digital_freq), s->sqa_b.sma, s->samples_streak,
s->sma_b.pos, sample_n, s->sma_b.lpos, pos);
"amplitude[%f]\tv_amp[%f]\t"
"streak[%zu] pos[%zu] sample_n[%zu] lpos[%zu] s[%zu]>>>\n", v, omega, f, TO_HZ(s->rate, f), s->sma_b.sma, TO_HZ(s->rate, sma_digital_freq), s->sqa_b.sma,
amplitude, v_amp,
s->samples_streak, s->sma_b.pos, sample_n, s->sma_b.lpos, pos);
} else {
switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(s->session), SWITCH_LOG_DEBUG, "<<< AVMD v[%.10f]\tomega[%f]\tf[%f] [%f]Hz\t\tsma[%f][%f]Hz\t\tsqa[%f]\tpos[%zu]"
" sample_n[%zu] lpos[%zu] s[%zu]>>>\n", v, omega, f, TO_HZ(s->rate, f), s->sma_b.sma, TO_HZ(s->rate, sma_digital_freq), s->sqa_b.sma, s->sma_b.pos, sample_n, s->sma_b.lpos, pos);
switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(s->session), SWITCH_LOG_DEBUG, "<<< AVMD v[%.10f]\tomega[%f]\tf[%f] [%f]Hz\t\tsma[%f][%f]Hz\t\tsqa[%f]\t"
"amplitude[%f]\tv_amp[%f]\t"
"pos[%zu] sample_n[%zu] lpos[%zu] s[%zu]>>>\n", v, omega, f, TO_HZ(s->rate, f), s->sma_b.sma, TO_HZ(s->rate, sma_digital_freq), s->sqa_b.sma,
amplitude, v_amp,
s->sma_b.pos, sample_n, s->sma_b.lpos, pos);
}
}
}
@ -1641,13 +1679,15 @@ static void avmd_process(avmd_session_t *s, switch_frame_t *frame) {
switch_channel_set_variable_printf(channel, "avmd_total_time", "[%d]", (int)(switch_micro_time_now() - s->start_time) / 1000);
switch_channel_execute_on(channel, "execute_on_avmd_beep");
avmd_fire_event(AVMD_EVENT_BEEP, s->session, TO_HZ(s->rate, sma_digital_freq), v, 0, 0);
avmd_fire_event(AVMD_EVENT_BEEP, s->session, TO_HZ(s->rate, sma_digital_freq), v, s->sma_amp_b.sma, v_amp, 0, 0);
if (s->settings.report_status == 1) {
switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(s->session), SWITCH_LOG_INFO, "<<< AVMD - Beep Detected: f = [%f], variance = [%f] >>>\n", TO_HZ(s->rate, sma_digital_freq), v);
switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(s->session), SWITCH_LOG_INFO, "<<< AVMD - Beep Detected: f = [%f] variance = [%f], amplitude = [%f] variance = [%f] >>>\n", TO_HZ(s->rate, sma_digital_freq), v, s->sma_amp_b.sma, v_amp);
}
switch_channel_set_variable(channel, "avmd_detect", "TRUE");
RESET_SMA_BUFFER(&s->sma_b);
RESET_SMA_BUFFER(&s->sqa_b);
RESET_SMA_BUFFER(&s->sma_amp_b);
RESET_SMA_BUFFER(&s->sqa_amp_b);
s->state.beep_state = BEEP_DETECTED;
goto done;
}