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

5 files changed, 136 insertions, 38 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;
+        }
     }
 }