Rework tone detector

- Send buffer pointers from ISR to userspace
- Use ADC interrupt to fetch data, not TIM6
- Increase size of buffer and do double-buffering
- Implement DTMF detectors
- Compare relative results instead of using absolute thresholds
- Add an IIR averaging filter on the results

8 files changed, 232 insertions, 90 deletions

diff --git a/src/common/includes/Audio/audio_in.h b/src/common/includes/Audio/audio_in.h
index 3bc543c..a543a69 100644
--- a/src/common/includes/Audio/audio_in.h
+++ b/src/common/includes/Audio/audio_in.h
@@ -28,10 +28,11 @@
 #include "FreeRTOS.h"
 
 #define AUDIO_IN_RATE 8000
-#define AUDIO_IN_BUF_LEN 32
+#define AUDIO_IN_BUF_LEN 400
 
-void audio_in_initialize(int rate);
+void audio_in_initialize(void);
 
-// Fill the buffer with AUDIO_IN_BUF_LEN samples.
-void audio_in_get_buffer(int16_t *buffer /*of length AUDIO_IN_BUF_LEN*/ );
+// After calling this function, *buffer will point to new audio data.
+// Returns the number of times filling the internal buffer failed.
+int32_t audio_in_get_buffer(int16_t **buffer /*of length AUDIO_IN_BUF_LEN*/ );
 
diff --git a/src/common/includes/Audio/tone.h b/src/common/includes/Audio/tone.h
index 861a472..b8a7f72 100644
--- a/src/common/includes/Audio/tone.h
+++ b/src/common/includes/Audio/tone.h
@@ -30,18 +30,17 @@
 
 #define TONE_BUFFER_LEN AUDIO_IN_BUF_LEN
 
-#define TONE_N 100
+#define TONE_N 400
 
 struct tone_detector {
     float coef;
     float Q1;
     float Q2;
-    float threshold;
 
     int num_samples_analysed;
 };
 
-void tone_init(int threshold);
+void tone_init(void);
 
 /* Return 1 when 1750 detected, 0 otherwise */
 int tone_1750_status(void);
diff --git a/src/common/includes/Core/common.h b/src/common/includes/Core/common.h
index 3f7dbcc..2dc0680 100644
--- a/src/common/includes/Core/common.h
+++ b/src/common/includes/Core/common.h
@@ -63,7 +63,8 @@ int random_bool(void);
 #define FAULT_SOURCE_TASK_OVERFLOW 5
 #define FAULT_SOURCE_CW_AUDIO_QUEUE 6
 #define FAULT_SOURCE_ADC1 7
-#define FAULT_SOURCE_ADC2 8
+#define FAULT_SOURCE_TIM6_ISR 8
+#define FAULT_SOURCE_ADC2_QUEUE 9
 void trigger_fault(int source);
 
 int find_last_sunday(const struct tm*);
diff --git a/src/common/src/Audio/tone.c b/src/common/src/Audio/tone.c
index 5065b31..e498cb7 100644
--- a/src/common/src/Audio/tone.c
+++ b/src/common/src/Audio/tone.c
@@ -1,7 +1,7 @@
 /*
  * The MIT License (MIT)
  *
- * Copyright (c) 2018 Maximilien Cuony, Matthias P. Braendli
+ * 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
@@ -24,6 +24,7 @@
 
 #include "Audio/tone.h"
 #include "Core/common.h"
+#include "GPIO/usart.h"
 
 #include <stdlib.h>
 
@@ -35,31 +36,56 @@
 #include "arm_math.h"
 #endif
 
-static struct tone_detector detector_1750;
+/* 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
+static struct tone_detector detectors[NUM_DETECTORS];
+
+// 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 tone_1750_detected = 0;
 
+
 int tone_1750_status()
 {
     return tone_1750_detected;
 }
 
-static void init_tone(struct tone_detector* detector, int freq, int threshold) {
+static void init_tone(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;
-    detector->threshold = threshold;
     detector->num_samples_analysed = 0;
 }
 
-void tone_init(int threshold) {
-    init_tone(&detector_1750, 1750, threshold);
+void tone_init() {
+    init_tone(&detectors[DET_COL_1], 1209);
+    init_tone(&detectors[DET_COL_2], 1336);
+    init_tone(&detectors[DET_ROW_7], 852);
+    init_tone(&detectors[DET_ROW_STAR], 941);
+    init_tone(&detectors[DET_1750], 1750);
 }
 
 /* Analyse a sample. Returns -1 if more samples needed, 0 if no tone detected,
  * 1 if a tone was detected.
  */
-static inline int analyse_sample(int16_t sample, struct tone_detector *detector)
+static inline float analyse_sample(int16_t sample, struct tone_detector *detector)
 {
     float Q0 = detector->coef * detector->Q1 - detector->Q2 + sample;
     detector->Q2 = detector->Q1;
@@ -73,35 +99,85 @@ static inline int analyse_sample(int16_t sample, struct tone_detector *detector)
                 detector->Q2 * detector->Q2 -
                 detector->coef * detector->Q1 * detector->Q2);
 
-#ifdef SIMULATOR
-#define FRMT "%- 12.4f"
-        fprintf(stderr, "1750: Q1 " FRMT "  Q2 " FRMT " m " FRMT " thresh " FRMT "\n", detector->Q1, detector->Q2, m, detector->threshold);
-#endif
-
-
         detector->Q1 = 0;
         detector->Q2 = 0;
         detector->num_samples_analysed = 0;
 
-        if (m > detector->threshold) {
-            return 1;
-        }
-        else {
-            return 0;
-        }
+        return m;
     }
     else {
-        return -1;
+        return 0;
     }
 }
 
 void tone_detect_1750(const int16_t *samples, int len)
 {
+    int32_t mean = 0;
+    int32_t min = 0x7fffffff;
+    int32_t max = -0x7fffffff;
     for (int i = 0; i < len; i++) {
-        int r = analyse_sample(samples[i], &detector_1750);
-        if (r == 0 || r == 1) {
-            tone_1750_detected = r;
+        const int16_t s = samples[i];
+        mean += s;
+        if (s < min) {
+            min = s;
         }
+        if (s > max) {
+            max = s;
+        }
+    }
+    mean /= len;
+
+    max -= mean;
+    min -= mean;
+
+    /* min and max should now be more or less opposite, assuming the
+     * signal is symmetric around its mean. Take the larger of the two,
+     * for the scalefactor calculation. */
+    const int32_t dev = (max > -min) ? max : -min;
+
+    /* Scale the signal [min - max] -> [-2^15, 2^15] by doing
+     * dev -> 2^15
+     */
+    const int32_t scalefactor = (1<<15) / dev;
+
+    float results[NUM_DETECTORS];
+    for (int det = 0; det < NUM_DETECTORS; det++) {
+        results[det] = 0;
+    }
+
+    for (int i = 0; i < len; i++) {
+        const int16_t s = scalefactor * (samples[i] - mean);
+
+        for (int det = 0; det < NUM_DETECTORS; det++) {
+            results[det] += analyse_sample(s, &detectors[det]);
+        }
+    }
+
+    float inv_mean = 0;
+    for (int det = 0; det < NUM_DETECTORS; det++) {
+        inv_mean += results[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 * results[det] * inv_mean))
+            >> 4; // divide by 16
+    }
+
+    tone_1750_detected = (normalised_results[DET_1750] > 400);
+
+    static int printcounter = 0;
+    if (++printcounter == 5) {
+        usart_debug("Tones: % 3d % 3d % 3d % 3d % 3d\r\n",
+                normalised_results[0],
+                normalised_results[1],
+                normalised_results[2],
+                normalised_results[3],
+                normalised_results[4]);
+
+        printcounter = 0;
     }
 }
 
diff --git a/src/common/src/Core/main.c b/src/common/src/Core/main.c
index ed3dfef..49ca8e6 100644
--- a/src/common/src/Core/main.c
+++ b/src/common/src/Core/main.c
@@ -211,17 +211,17 @@ static void launcher_task(void __attribute__ ((unused))*pvParameters)
     audio_play_with_callback(audio_callback, NULL);
 
     usart_debug_puts("Tone init\r\n");
-    tone_init(120000);
+    tone_init();
 
     usart_debug_puts("Audio in init\r\n");
-    audio_in_initialize(AUDIO_IN_RATE);
+    audio_in_initialize();
 
     usart_debug_puts("TaskNF init\r\n");
 
     xTaskCreate(
             nf_analyse,
             "TaskNF",
-            1*configMINIMAL_STACK_SIZE,
+            3*configMINIMAL_STACK_SIZE,
             (void*) NULL,
             tskIDLE_PRIORITY + 2UL,
             &task_handle);
@@ -609,25 +609,46 @@ static void exercise_fsm(void __attribute__ ((unused))*pvParameters)
     }
 }
 
-static int16_t audio_in_buffer[AUDIO_IN_BUF_LEN];
 static int led_phase = 0;
 static void nf_analyse(void __attribute__ ((unused))*pvParameters)
 {
+    int num_fails = 0;
+    int total_samples_analysed = 0;
+    int timestamp = 0;
+
+    uint64_t t0 = timestamp_now();
+
     while (1) {
         if (led_phase == 0) {
             leds_turn_on(LED_BLUE);
         }
-        else if (led_phase == 400) {
+        else if (led_phase == 40) {
             leds_turn_off(LED_BLUE);
         }
 
         led_phase++;
 
-        if (led_phase >= 800) {
+        if (led_phase >= 80) {
             led_phase = 0;
         }
 
-        audio_in_get_buffer(audio_in_buffer);
+        int16_t *audio_in_buffer;
+        const int new_num_fails = num_fails + audio_in_get_buffer(&audio_in_buffer);
+
+        if (new_num_fails != num_fails) {
+            usart_debug("Number of input samples lost: %d\r\n", new_num_fails);
+        }
+        num_fails = new_num_fails;
         tone_detect_1750(audio_in_buffer, AUDIO_IN_BUF_LEN);
+
+        total_samples_analysed += AUDIO_IN_BUF_LEN;
+        if (++timestamp == 80) {
+            uint64_t t1 = timestamp_now();
+            usart_debug("Total samples analysed: %d in %ldms = %d\r\n",
+                    total_samples_analysed,
+                    t1 - t0,
+                    1000 * total_samples_analysed / (int)(t1 - t0));
+            timestamp = 0;
+        }
     }
 }
diff --git a/src/glutt-o-logique/audio_in.c b/src/glutt-o-logique/audio_in.c
index 743c79b..a820b5f 100644
--- a/src/glutt-o-logique/audio_in.c
+++ b/src/glutt-o-logique/audio_in.c
@@ -26,61 +26,74 @@
 #include "stm32f4xx_gpio.h"
 #include "stm32f4xx_adc.h"
 
+#include "GPIO/usart.h"
 #include "Core/common.h"
 #include "Audio/audio_in.h"
 #include "queue.h"
 
 // APB1 prescaler = 4, see bsp/system_stm32f4xx.c
 #define APB1_FREQ (168000000ul / 4)
-#define ADC2_SAMPLE_FREQ 16000
-#define ADC2_CHANNEL_AUDIO ADC_Channel_14
+#define ADC2_CHANNEL_AUDIO ADC_Channel_9
 
 // see doc/pio.txt for allocation
 #define PINS_ADC2 /* PB1 on ADC2 IN9 */ (GPIO_Pin_1)
 
-#warning "TODO: initialise ADC2 and use it for NF input"
-
-// The TIM6 ISR reads from ADC2 and writes into this buffer
-static int16_t adc2_values[AUDIO_IN_BUF_LEN];
+// The TIM6 ISR reads from ADC2 and writes into these double buffers
+static int16_t adc2_values[2][AUDIO_IN_BUF_LEN];
+static int     adc2_current_buffer = 0; // which buffer is being written into
 static int     adc2_values_end = 0;
+static int32_t adc2_lost_samples = 0;
 
-// ADC2 data from interrupt to userspace goes through the queue
+// ADC2 data from interrupt to userspace. The queue contains the buffer index
 static QueueHandle_t adc2_values_queue;
 
 /* ISR for Timer6 and DAC1&2 underrun */
-void TIM6_DAC_IRQHandler(void)
+void TIM6_DAC_IRQHandler(void);
+
+void TIM6_DAC_IRQHandler()
 {
     if (TIM_GetITStatus(TIM6, TIM_IT_Update)) {
-        if (ADC_GetFlagStatus(ADC2, ADC_FLAG_EOC) == SET) {
-            uint16_t value = ADC_GetConversionValue(ADC2);
-            /* input range: 0 to 65535
-             * output range: -32768 to 32767 */
-            adc2_values[adc2_values_end++] = (int32_t)value - 32768;
-            if (adc2_values_end == AUDIO_IN_BUF_LEN) {
-                int success = xQueueSendToBackFromISR(
-                        adc2_values_queue,
-                        adc2_values,
-                        NULL);
-
-                adc2_values_end = 0;
-
-                if (success == pdFALSE) {
-                    trigger_fault(FAULT_SOURCE_ADC2);
-                }
-            }
-        }
-        else {
-#warning "handle fault"
-        }
-
         ADC_SoftwareStartConv(ADC2);
-
         TIM_ClearITPendingBit(TIM6, TIM_IT_Update);
     }
+    else {
+        usart_debug("%x\r\n", TIM6->SR);
+        trigger_fault(FAULT_SOURCE_TIM6_ISR);
+    }
+}
+
+void ADC_IRQHandler(void);
+void ADC_IRQHandler()
+{
+    if (ADC_GetFlagStatus(ADC2, ADC_FLAG_EOC) == SET) {
+        uint16_t value = ADC_GetConversionValue(ADC2);
+
+        ADC_ClearITPendingBit(ADC2, ADC_IT_EOC);
+
+        /* input range: 0 to 2^12
+         * output range: -32768 to 32767 */
+        adc2_values[adc2_current_buffer][adc2_values_end++] = (int32_t)value - (1 << 11);
+        if (adc2_values_end == AUDIO_IN_BUF_LEN) {
+            int success = xQueueSendToBackFromISR(
+                    adc2_values_queue,
+                    &adc2_current_buffer,
+                    NULL);
+
+            adc2_values_end = 0;
+            adc2_current_buffer = (adc2_current_buffer + 1) % 2;
+
+            if (success == pdFALSE) {
+                adc2_lost_samples += AUDIO_IN_BUF_LEN;
+            }
+        }
+    }
+    else {
+        adc2_lost_samples++;
+    }
 }
 
 // Timer6 is used for ADC2 sampling
-static void enable_timer6(void)
+static void enable_timer6()
 {
     /* TIM6 Periph clock enable */
     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE);
@@ -88,26 +101,27 @@ static void enable_timer6(void)
     /* Time base configuration */
     TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
     TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
-    TIM_TimeBaseStructure.TIM_Period = (int)(APB1_FREQ/ADC2_SAMPLE_FREQ);
-    TIM_TimeBaseStructure.TIM_Prescaler = 0;
-    TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+    TIM_TimeBaseStructure.TIM_Period = (int)(APB1_FREQ/AUDIO_IN_RATE);
+    TIM_TimeBaseStructure.TIM_Prescaler = 1;
+    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
     TIM_TimeBaseInit(TIM6, &TIM_TimeBaseStructure);
 
     NVIC_InitTypeDef NVIC_InitStructure;
     NVIC_InitStructure.NVIC_IRQChannel = TIM6_DAC_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 5;
     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
     NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
     NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(TIM6_DAC_IRQn, 5);
 
-    TIM_Cmd(TIM6, ENABLE);
-    ADC_SoftwareStartConv(ADC2);
     TIM_ITConfig(TIM6, TIM_IT_Update, ENABLE);
+    ADC_SoftwareStartConv(ADC2);
+    TIM_Cmd(TIM6, ENABLE);
 }
 
 
-void audio_in_initialize(int rate)
+void audio_in_initialize()
 {
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC2, ENABLE);
     RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
@@ -131,18 +145,36 @@ void audio_in_initialize(int rate)
     ADC_InitStruct.ADC_NbrOfConversion = 1;
     ADC_Init(ADC2, &ADC_InitStruct);
 
+    ADC_RegularChannelConfig(ADC2, ADC2_CHANNEL_AUDIO, /*rank*/ 1, ADC_SampleTime_15Cycles);
+    ADC_ClearITPendingBit(ADC2, ADC_IT_EOC);
+    ADC_ITConfig(ADC2, ADC_IT_EOC, ENABLE);
+
+    NVIC_InitTypeDef NVIC_InitStructure;
+    NVIC_InitStructure.NVIC_IRQChannel = ADC_IRQn;
+    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 7;
+    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(ADC_IRQn, 7);
+
     ADC_Cmd(ADC2, ENABLE);
 
-    adc2_values_queue = xQueueCreate(4, AUDIO_IN_BUF_LEN);
+
+    adc2_values_queue = xQueueCreate(4, sizeof(adc2_current_buffer));
     if (adc2_values_queue == 0) {
-        while(1); /* fatal error */
+        trigger_fault(FAULT_SOURCE_ADC2_QUEUE);
     }
 
     enable_timer6();
 }
 
-void audio_in_get_buffer(int16_t *buffer /*of length AUDIO_IN_BUF_LEN*/ )
+int32_t audio_in_get_buffer(int16_t **buffer /*of length AUDIO_IN_BUF_LEN*/ )
 {
-    while (!xQueueReceive(adc2_values_queue, buffer, portMAX_DELAY)) {}
+    int buf_ix = 0;
+    while (!xQueueReceive(adc2_values_queue, &buf_ix, portMAX_DELAY)) {}
+
+    *buffer = adc2_values[buf_ix];
+
+    return adc2_lost_samples;
 }
 
diff --git a/src/glutt-o-logique/main.c b/src/glutt-o-logique/main.c
index 5ff89bf..7c4c93f 100644
--- a/src/glutt-o-logique/main.c
+++ b/src/glutt-o-logique/main.c
@@ -46,7 +46,8 @@
 #  error  "SOFTFP"
 #endif
 
-void init(void)
+void init(void);
+void init()
 {
     /* Setup Watchdog
      * The IWDG runs at 32kHz. With a prescaler of 32 -> 1kHz.
diff --git a/src/simulator/src/Audio/audio_in.c b/src/simulator/src/Audio/audio_in.c
index d17e8bc..097eaf0 100644
--- a/src/simulator/src/Audio/audio_in.c
+++ b/src/simulator/src/Audio/audio_in.c
@@ -33,24 +33,30 @@
 
 pa_simple *s_in = NULL;
 
+static int16_t buffers[2][AUDIO_IN_BUF_LEN];
+static int current_buffer = 0;
+
 static QueueHandle_t adc2_values_queue;
 
 static void audio_buffer_reader(void __attribute__((unused))*args)
 {
     while (1) {
-        int16_t buffer[AUDIO_IN_BUF_LEN];
-        pa_simple_read(s_in, buffer, AUDIO_IN_BUF_LEN * sizeof(int16_t), NULL);
+        pa_simple_read(s_in, buffers[current_buffer], AUDIO_IN_BUF_LEN * sizeof(int16_t), NULL);
+
         int success = xQueueSendToBack(
                 adc2_values_queue,
-                buffer,
+                &current_buffer,
                 portMAX_DELAY);
         assert(success);
+
+        current_buffer = (current_buffer + 1) % 2;
+
         taskYIELD();
     }
 }
 
 
-void audio_in_initialize(int rate) {
+void audio_in_initialize() {
     int error;
 
     static pa_sample_spec ss = {
@@ -59,12 +65,12 @@ void audio_in_initialize(int rate) {
         .channels = 1
     };
 
-    ss.rate = rate;
+    ss.rate = AUDIO_IN_RATE;
 
     s_in = pa_simple_new(NULL, "GlutteR", PA_STREAM_RECORD, NULL, "record", &ss, NULL, NULL, &error);
     assert(s_in);
 
-    adc2_values_queue = xQueueCreate(4, AUDIO_IN_BUF_LEN);
+    adc2_values_queue = xQueueCreate(4, sizeof(current_buffer));
     assert(adc2_values_queue);
 
     TaskHandle_t task_handle;
@@ -77,8 +83,13 @@ void audio_in_initialize(int rate) {
             &task_handle);
 }
 
-void audio_in_get_buffer(int16_t *buffer /*of length AUDIO_IN_BUF_LEN*/ )
+int32_t audio_in_get_buffer(int16_t **buffer /*of length AUDIO_IN_BUF_LEN*/ )
 {
-    while (!xQueueReceive(adc2_values_queue, buffer, portMAX_DELAY)) {}
+    int last_written_buffer = 0;
+    while (!xQueueReceive(adc2_values_queue, &last_written_buffer, portMAX_DELAY)) {}
+
+    *buffer = buffers[last_written_buffer];
+
+    return 0;
 }