Fix tone detection

5 files changed, 65 insertions, 205 deletions

diff --git a/src/common/includes/Audio/tone.h b/src/common/includes/Audio/tone.h
index 7de1c31..272fcb9 100644
--- a/src/common/includes/Audio/tone.h
+++ b/src/common/includes/Audio/tone.h
@@ -44,6 +44,6 @@ int tone_fax_status(void);
 void tone_do_analysis(void);
 
 /* Push a sample from the ADC ISR */
-void tone_detect_push_sample_from_irq(const int16_t sample);
+void tone_detect_push_sample_from_irq(const uint16_t sample);
 
 #endif
diff --git a/src/common/includes/Core/common.h b/src/common/includes/Core/common.h
index 2dc0680..4918a5b 100644
--- a/src/common/includes/Core/common.h
+++ b/src/common/includes/Core/common.h
@@ -65,6 +65,7 @@ int random_bool(void);
 #define FAULT_SOURCE_ADC1 7
 #define FAULT_SOURCE_TIM6_ISR 8
 #define FAULT_SOURCE_ADC2_QUEUE 9
+#define FAULT_SOURCE_ADC2_IRQ 10
 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 6691479..b8bebf8 100644
--- a/src/common/src/Audio/tone.c
+++ b/src/common/src/Audio/tone.c
@@ -70,13 +70,13 @@ struct tone_detector {
     float coef;
     float Q1;
     float Q2;
-
-    int num_samples_analysed;
 };
 
 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;
 
@@ -116,15 +116,17 @@ static inline void push_dtmf_code(enum dtmf_code code)
     }
     dtmf_sequence[NUM_DTMF_SEQ-1] = code;
 
-    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]));
+    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 = 200;
+const int thresh_1750 = 300;
 
 static void analyse_dtmf()
 {
@@ -198,7 +200,6 @@ 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;
-    detector->num_samples_analysed = 0;
 }
 
 void tone_init() {
@@ -221,10 +222,12 @@ void tone_init() {
 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;
-            detectors[det].num_samples_analysed = 0;
         }
         audio_in_enable(1);
         detectors_enabled = 1;
@@ -235,65 +238,53 @@ void tone_detector_enable(int enable)
     }
 }
 
-/* Analyse a sample. Returns -1 if more samples needed, 0 if no tone detected,
- * 1 if a tone was detected.
- */
-static inline float analyse_sample(int16_t sample, struct tone_detector *detector)
+void tone_detect_push_sample_from_irq(const uint16_t sample)
 {
-    float Q0 = detector->coef * detector->Q1 - detector->Q2 + sample;
-    detector->Q2 = detector->Q1;
-    detector->Q1 = Q0;
-
-    detector->num_samples_analysed++;
+    num_samples_analysed++;
 
-    if (detector->num_samples_analysed == TONE_N) {
-        const float m = sqrtf(
-                detector->Q1 * detector->Q1 +
-                detector->Q2 * detector->Q2 -
-                detector->coef * detector->Q1 * detector->Q2);
+    accum += sample;
 
-        detector->Q1 = 0;
-        detector->Q2 = 0;
-        detector->num_samples_analysed = 0;
+    // Do not do tone detection before we have calculated a mean
+    if (prev_mean > 0) {
+        const float s = sample - prev_mean;
 
-        return m;
-    }
-    else {
-        return 0;
-    }
-}
+        for (int det = 0; det < NUM_DETECTORS; det++) {
+            struct tone_detector *detector = &detectors[det];
 
-void tone_detect_push_sample_from_irq(const int16_t sample)
-{
-    for (int det = 0; det < NUM_DETECTORS; det++) {
-        struct tone_detector *detector = &detectors[det];
-        float Q0 = detector->coef * detector->Q1 - detector->Q2 + sample;
-        detector->Q2 = detector->Q1;
-        detector->Q1 = Q0;
+            float Q0 = detector->coef * detector->Q1 - detector->Q2 + s;
+            detector->Q2 = detector->Q1;
+            detector->Q1 = Q0;
+        }
     }
-    num_samples_analysed++;
 
     if (num_samples_analysed == TONE_N) {
-        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;
+        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 = xQueueSendToBackFromISR(
+                    m_squared_queue,
+                    &m_squared,
+                    &require_context_switch);
+
+            if (success == pdFALSE) {
+                lost_results++;
+            }
         }
 
-        BaseType_t require_context_switch = 0;
-        int success = xQueueSendToBackFromISR(
-                m_squared_queue,
-                &m_squared,
-                &require_context_switch);
-
-        if (success == pdFALSE) {
-            lost_results++;
-        }
+        prev_mean = (float)accum / (float)TONE_N;
+        accum = 0;
     }
 }
 
@@ -332,80 +323,3 @@ void tone_do_analysis()
     }
 }
 
-#if 0
-void tone_detect_push_samples(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++) {
-        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]);
-        }
-    }
-
-    /* Normalise the results so that a relative comparison of
-     * the different detectors can be done. This is more reliable
-     * than looking at the detector outputs independently, especially
-     * in the presence of noise whose amplitude varies. */
-    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] > thresh_1750);
-    analyse_dtmf();
-
-    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;
-    }
-}
-#endif
diff --git a/src/common/src/Core/main.c b/src/common/src/Core/main.c
index 341c560..ec38df5 100644
--- a/src/common/src/Core/main.c
+++ b/src/common/src/Core/main.c
@@ -617,40 +617,25 @@ static void exercise_fsm(void __attribute__ ((unused))*pvParameters)
     }
 }
 
-static int led_phase = 0;
+const int BLUE_LED_INTVL = 4;
+static int blue_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) {
+        if (blue_led_phase == 0) {
             leds_turn_on(LED_BLUE);
         }
-        else if (led_phase == 40) {
+        else if (blue_led_phase == BLUE_LED_INTVL) {
             leds_turn_off(LED_BLUE);
         }
 
-        led_phase++;
+        blue_led_phase++;
 
-        if (led_phase >= 80) {
-            led_phase = 0;
+        if (blue_led_phase >= BLUE_LED_INTVL * 2) {
+            blue_led_phase = 0;
         }
 
         tone_do_analysis();
-
-        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,
-                    (int)(1000.0f * total_samples_analysed / (float)(t1 - t0)));
-            timestamp = 0;
-        }
     }
 }
 
diff --git a/src/glutt-o-logique/audio_in.c b/src/glutt-o-logique/audio_in.c
index 3eee2b3..19c54d3 100644
--- a/src/glutt-o-logique/audio_in.c
+++ b/src/glutt-o-logique/audio_in.c
@@ -29,7 +29,7 @@
 #include "GPIO/usart.h"
 #include "Core/common.h"
 #include "Audio/audio_in.h"
-#include "queue.h"
+#include "Audio/tone.h"
 
 // APB1 prescaler = 4, see bsp/system_stm32f4xx.c
 #define APB1_FREQ (168000000ul / 4)
@@ -38,15 +38,6 @@
 // see doc/pio.txt for allocation
 #define PINS_ADC2 /* PB1 on ADC2 IN9 */ (GPIO_Pin_1)
 
-// 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. The queue contains the buffer index
-static QueueHandle_t adc2_values_queue;
-
 /* ISR for Timer6 and DAC1&2 underrun */
 void TIM6_DAC_IRQHandler(void);
 
@@ -57,7 +48,7 @@ void TIM6_DAC_IRQHandler()
         TIM_ClearITPendingBit(TIM6, TIM_IT_Update);
     }
     else {
-        usart_debug("%x\r\n", TIM6->SR);
+        usart_debug("Spurious TIM6 IRQ: SR=%x\r\n", TIM6->SR);
         trigger_fault(FAULT_SOURCE_TIM6_ISR);
     }
 }
@@ -73,28 +64,13 @@ void ADC_IRQHandler()
         ADC_ClearITPendingBit(ADC2, ADC_IT_EOC);
 
         tone_detect_push_sample_from_irq(value);
-
-#if 0
-        /* 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,
-                    &require_context_switch);
-
-            adc2_values_end = 0;
-            adc2_current_buffer = (adc2_current_buffer + 1) % 2;
-
-            if (success == pdFALSE) {
-                adc2_lost_samples += AUDIO_IN_BUF_LEN;
-            }
-        }
-#endif
+    }
+    else if (ADC_GetFlagStatus(ADC2, ADC_FLAG_STRT) == SET) {
+        // This sometimes happens...
     }
     else {
-        adc2_lost_samples++;
+        usart_debug("Spurious ADC2 IRQ: SR=%x\r\n", ADC2->SR);
+        trigger_fault(FAULT_SOURCE_ADC2_IRQ);
     }
 
     portYIELD_FROM_ISR(require_context_switch);
@@ -165,11 +141,6 @@ void audio_in_initialize()
 
     ADC_Cmd(ADC2, ENABLE);
 
-    adc2_values_queue = xQueueCreate(2, sizeof(adc2_current_buffer));
-    if (adc2_values_queue == 0) {
-        trigger_fault(FAULT_SOURCE_ADC2_QUEUE);
-    }
-
     setup_timer();
 }
 
@@ -178,17 +149,6 @@ void audio_in_enable(int enable)
     TIM_Cmd(TIM6, enable ? ENABLE : DISABLE);
 }
 
-int32_t audio_in_get_buffer(int16_t **buffer /*of length AUDIO_IN_BUF_LEN*/ )
-{
-    int buf_ix = 0;
-    while (!xQueueReceive(adc2_values_queue, &buf_ix, portMAX_DELAY)) {}
-
-    *buffer = adc2_values[buf_ix];
-
-    return adc2_lost_samples;
-}
-
-
 #warning "Test tone detector activation from FSM"
 #warning "Test 1750 not triggered by start bip my yaesu makes"
 #warning "Test DTMF detector"