/*
* The MIT License (MIT)
*
* Copyright (c) 2019 Maximilien Cuony, Matthias P. Braendli
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include "Audio/tone.h"
#include "Core/common.h"
#include "GPIO/usart.h"
#include <stdlib.h>
#include "queue.h"
#ifdef SIMULATOR
#include <math.h>
#define arm_cos_f32 cosf
#define arm_sin_f32 sinf
#else
#include "arm_math.h"
#endif
#define TONE_N 800
/* DTMF frequencies
* COL_1 COL_2 COL_3 COL_A
* 1209 1336 1477 1633
* 697 [1] [2] [3] [A] ROW_1
* 770 [4] [5] [6] [B] ROW_4
* 852 [7] [8] [9] [C] ROW_7
* 941 [*] [0] [#] [D] ROW_STAR
*
* We need 0, 7, and 1750Hz for now, having detectors for
* 1209, 1336, 852, 941 and 1750 is sufficient. */
#define DET_COL_1 0
#define DET_COL_2 1
#define DET_ROW_7 2
#define DET_ROW_STAR 3
#define DET_1750 4
#define NUM_DETECTORS 5
// Incomplete because not all frequencies decoded
enum dtmf_code {
DTMF_NONE = 0,
DTMF_0,
DTMF_7,
DTMF_8,
DTMF_STAR,
};
struct tone_detector {
float coef;
float Q1;
float Q2;
};
static struct tone_detector detectors[NUM_DETECTORS];
static int num_samples_analysed = 0;
static float prev_mean = 0;
static uint32_t accum = 0;
static int lost_results = 0;
static QueueHandle_t m_squared_queue;
// Apply an IIR filter with alpha = 5/16 to smooth out variations.
// Values are normalised s.t. the mean corresponds to 100
static int32_t normalised_results[NUM_DETECTORS];
static int num_tone_1750_detected = 0;
static uint64_t tone_1750_detected_since = 0;
static int detectors_enabled = 0;
static enum dtmf_code dtmf_last_seen = 0;
static uint64_t dtmf_last_seen_at = 0;
// Store the sequence of dtmf codes in this FIFO. If no DTMF code gets
// decoded in the interval, a NONE gets inserted into the sequence
#define NUM_DTMF_SEQ 3
#define DTMF_MAX_TONE_INTERVAL 2500
static enum dtmf_code dtmf_sequence[NUM_DTMF_SEQ];
static char* dtmf_to_str(enum dtmf_code code)
{
char *codestr = "?";
switch (code) {
case DTMF_0: codestr = "0"; break;
case DTMF_7: codestr = "7"; break;
case DTMF_8: codestr = "8"; break;
case DTMF_STAR: codestr = "*"; break;
case DTMF_NONE: codestr = "x"; break;
}
return codestr;
}
static inline void push_dtmf_code(enum dtmf_code code)
{
for (int i = 0; i < NUM_DTMF_SEQ-1; i++) {
dtmf_sequence[i] = dtmf_sequence[i+1];
}
dtmf_sequence[NUM_DTMF_SEQ-1] = code;
if (code != DTMF_NONE) {
usart_debug("DTMF: [%s, %s, %s]\r\n",
dtmf_to_str(dtmf_sequence[0]),
dtmf_to_str(dtmf_sequence[1]),
dtmf_to_str(dtmf_sequence[2]));
}
}
// TODO: Does that depend on TONE_N?
const int thresh_dtmf = 200;
const int thresh_1750 = 300;
const int num_1750_required = 5;
static void analyse_dtmf()
{
// Bits 0 to 9 are numbers, bit 10 to 13 letters, bit 14 is star, 15 is hash
const uint16_t pattern =
((normalised_results[DET_COL_1] > thresh_dtmf &&
normalised_results[DET_ROW_7] > thresh_dtmf) ? (1 << 7) : 0) +
((normalised_results[DET_COL_2] > thresh_dtmf &&
normalised_results[DET_ROW_7] > thresh_dtmf) ? (1 << 8) : 0) +
((normalised_results[DET_COL_1] > thresh_dtmf &&
normalised_results[DET_ROW_STAR] > thresh_dtmf) ? (1 << 14) : 0) +
((normalised_results[DET_COL_2] > thresh_dtmf &&
normalised_results[DET_ROW_STAR] > thresh_dtmf) ? (1 << 0) : 0);
// Match patterns exactly to exclude multiple simultaneous DTMF codes.
if (pattern == (1 << 0)) {
if (dtmf_last_seen != DTMF_0) {
push_dtmf_code(DTMF_0);
}
dtmf_last_seen = DTMF_0;
dtmf_last_seen_at = timestamp_now();
}
else if (pattern == (1 << 7)) {
if (dtmf_last_seen != DTMF_7) {
push_dtmf_code(DTMF_7);
}
dtmf_last_seen = DTMF_7;
dtmf_last_seen_at = timestamp_now();
}
else if (pattern == (1 << 8)) {
if (dtmf_last_seen != DTMF_8) {
push_dtmf_code(DTMF_8);
}
dtmf_last_seen = DTMF_8;
dtmf_last_seen_at = timestamp_now();
}
else if (pattern == (1 << 14)) {
if (dtmf_last_seen != DTMF_STAR) {
push_dtmf_code(DTMF_STAR);
}
dtmf_last_seen = DTMF_STAR;
dtmf_last_seen_at = timestamp_now();
}
else if (dtmf_last_seen_at + DTMF_MAX_TONE_INTERVAL < timestamp_now()) {
// Flush out all codes
push_dtmf_code(DTMF_NONE);
dtmf_last_seen = DTMF_NONE;
dtmf_last_seen_at = timestamp_now();
}
}
int tone_1750_status()
{
return num_tone_1750_detected >= num_1750_required;
}
int tone_1750_for_5_seconds()
{
return (num_tone_1750_detected >= num_1750_required) &&
tone_1750_detected_since + 5000 < timestamp_now();
}
int tone_fax_status()
{
return dtmf_sequence[0] == DTMF_0 &&
dtmf_sequence[1] == DTMF_7 &&
dtmf_sequence[2] == DTMF_STAR;
}
static inline void init_detector(struct tone_detector* detector, int freq) {
detector->coef = 2.0f * arm_cos_f32(2.0f * FLOAT_PI * freq / AUDIO_IN_RATE);
detector->Q1 = 0;
detector->Q2 = 0;
}
void tone_init() {
m_squared_queue = xQueueCreate(2, NUM_DETECTORS * sizeof(float));
if (m_squared_queue == 0) {
trigger_fault(FAULT_SOURCE_ADC2_QUEUE);
}
for (int i = 0; i < NUM_DTMF_SEQ; i++) {
dtmf_sequence[i] = DTMF_NONE;
}
init_detector(&detectors[DET_COL_1], 1209);
init_detector(&detectors[DET_COL_2], 1336);
init_detector(&detectors[DET_ROW_7], 852);
init_detector(&detectors[DET_ROW_STAR], 941);
init_detector(&detectors[DET_1750], 1750);
}
void tone_detector_enable(int enable)
{
if (enable && !detectors_enabled) {
num_samples_analysed = 0;
prev_mean = 0;
accum = 0;
for (int det = 0; det < NUM_DETECTORS; det++) {
detectors[det].Q1 = 0;
detectors[det].Q2 = 0;
}
audio_in_enable(1);
detectors_enabled = 1;
}
else if (!enable && detectors_enabled) {
audio_in_enable(0);
detectors_enabled = 0;
}
}
void tone_detect_push_sample(const uint16_t sample, int is_irq)
{
num_samples_analysed++;
accum += sample;
// Do not do tone detection before we have calculated a mean
if (prev_mean > 0) {
const float s = sample - prev_mean;
for (int det = 0; det < NUM_DETECTORS; det++) {
struct tone_detector *detector = &detectors[det];
float Q0 = detector->coef * detector->Q1 - detector->Q2 + s;
detector->Q2 = detector->Q1;
detector->Q1 = Q0;
}
}
if (num_samples_analysed == TONE_N) {
num_samples_analysed = 0;
if (prev_mean > 0) {
float m_squared[NUM_DETECTORS];
for (int det = 0; det < NUM_DETECTORS; det++) {
struct tone_detector *detector = &detectors[det];
m_squared[det] =
detector->Q1 * detector->Q1 +
detector->Q2 * detector->Q2 -
detector->coef * detector->Q1 * detector->Q2;
detector->Q1 = 0;
detector->Q2 = 0;
}
BaseType_t require_context_switch = 0;
int success;
if (is_irq) {
success = xQueueSendToBackFromISR(
m_squared_queue,
&m_squared,
&require_context_switch);
}
else {
success = xQueueSendToBack(
m_squared_queue,
&m_squared,
0);
}
if (success == pdFALSE) {
lost_results++;
}
}
prev_mean = (float)accum / (float)TONE_N;
accum = 0;
}
}
void tone_do_analysis()
{
float m[NUM_DETECTORS];
while (!xQueueReceive(m_squared_queue, &m, portMAX_DELAY)) {}
float inv_mean = 0;
for (int det = 0; det < NUM_DETECTORS; det++) {
m[det] = sqrtf(m[det]);
inv_mean += m[det];
}
inv_mean = NUM_DETECTORS / inv_mean;
for (int det = 0; det < NUM_DETECTORS; det++) {
normalised_results[det] =
(11 * normalised_results[det] +
(int)(5 * 100 * m[det] * inv_mean))
>> 4; // divide by 16
}
if (num_tone_1750_detected < num_1750_required &&
normalised_results[DET_1750] > thresh_1750) {
num_tone_1750_detected++;
if (num_tone_1750_detected == num_1750_required) {
tone_1750_detected_since = timestamp_now();
}
}
else if (num_tone_1750_detected > 0 &&
normalised_results[DET_1750] <= thresh_1750) {
num_tone_1750_detected--;
}
analyse_dtmf();
#if PRINT_TONES_STATS
static int printcounter = 0;
if (++printcounter == 5) {
usart_debug("Tones: % 3d % 3d % 3d % 3d % 3d since %d\r\n",
normalised_results[0],
normalised_results[1],
normalised_results[2],
normalised_results[3],
normalised_results[4],
(int)(timestamp_now() - tone_1750_detected_since)
);
printcounter = 0;
}
#endif // PRINT_TONES_STATS
}