Base with a working implementation on the simulator for software 1750 detection

7 files changed, 246 insertions, 2 deletions

diff --git a/src/common/includes/Audio/audio.h b/src/common/includes/Audio/audio.h
index 7aeb6e9..d48cbe5 100644
--- a/src/common/includes/Audio/audio.h
+++ b/src/common/includes/Audio/audio.h
@@ -32,7 +32,7 @@ void audio_stop_dma(void);
 
 // Initialize and power up audio hardware. Use the above defines for the parameters.
 // Can probably only be called once.
-void audio_initialize(int plln,int pllr,int i2sdiv,int i2sodd, int rate);
+void audio_initialize(int plln, int pllr, int i2sdiv, int i2sodd, int rate);
 
 // Power up and down the audio hardware.
 void audio_put_codec_in_reset(void);
diff --git a/src/common/includes/Audio/audio_in.h b/src/common/includes/Audio/audio_in.h
new file mode 100644
index 0000000..86c93b2
--- /dev/null
+++ b/src/common/includes/Audio/audio_in.h
@@ -0,0 +1,40 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2018 Maximilien Cuony
+ *
+ * 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.
+*/
+
+#ifndef __AUDIO_IN_H__
+#define __AUDIO_IN_H__
+
+#include <stdio.h>
+#include "FreeRTOS.h"
+
+#define AUDIO_IN_BUF_LEN 2048
+#define AUDIO_IN_RATE 8000
+
+int16_t * audio_in_buffer;
+
+void audio_in_initialize(int rate);
+void audio_in_initialize_plateform(int rate);
+void audio_in_buffer_ready(void);
+
+#endif
diff --git a/src/common/includes/Audio/tone.h b/src/common/includes/Audio/tone.h
new file mode 100644
index 0000000..1398ca5
--- /dev/null
+++ b/src/common/includes/Audio/tone.h
@@ -0,0 +1,52 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2018 Maximilien Cuony
+ *
+ * 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.
+*/
+
+#ifndef __TONE_H_
+#define __TONE_H_
+
+#include <stdio.h>
+#include "Audio/audio_in.h"
+
+#define TONE_BUFFER_LEN AUDIO_IN_BUF_LEN
+
+#define TONE_N 100
+
+struct tone_detector {
+    float coef;
+    float Q1;
+    float Q2;
+    float threshold;
+};
+
+static struct tone_detector detector_1750;
+
+
+void init_tones(void);
+void init_tone(struct tone_detector* detector, int freq, int threshold);
+void detect_tones(int16_t * buffer);
+
+int TONE_1750_DETECTED = 0;
+
+
+#endif
diff --git a/src/common/sourcelist.txt b/src/common/sourcelist.txt
index eada47f..9d6ec78 100644
--- a/src/common/sourcelist.txt
+++ b/src/common/sourcelist.txt
@@ -8,3 +8,5 @@ src/Core/fsm.c
 src/Core/main.c
 src/Audio/cw.c
 src/Audio/audio.c
+src/Audio/audio_in.c
+src/Audio/tone.c
diff --git a/src/common/src/Audio/audio_in.c b/src/common/src/Audio/audio_in.c
new file mode 100644
index 0000000..01f4794
--- /dev/null
+++ b/src/common/src/Audio/audio_in.c
@@ -0,0 +1,46 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2018 Maximilien Cuony
+ *
+ * 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/audio_in.h"
+
+int16_t audio_in_buffer_0[AUDIO_IN_BUF_LEN];
+int16_t audio_in_buffer_1[AUDIO_IN_BUF_LEN];
+
+void audio_in_initialize(int rate) {
+    audio_in_buffer = &audio_in_buffer_0;
+    audio_in_initialize_plateform(rate);
+}
+
+void audio_in_buffer_ready() {
+
+    detect_tones(audio_in_buffer);
+
+    if (audio_in_buffer == audio_in_buffer_0) {
+        audio_in_buffer = audio_in_buffer_1;
+    } else {
+        audio_in_buffer = audio_in_buffer_0;
+    }
+
+}
diff --git a/src/common/src/Audio/tone.c b/src/common/src/Audio/tone.c
new file mode 100644
index 0000000..dda9432
--- /dev/null
+++ b/src/common/src/Audio/tone.c
@@ -0,0 +1,97 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2018 Maximilien Cuony
+ *
+ * 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"
+
+#ifdef SIMULATOR
+#include <math.h>
+#define arm_cos_f32 cosf
+#define arm_sin_f32 sinf
+#else
+#include "arm_math.h"
+#endif
+
+static int current_pos = 0;
+
+
+void init_tones() {
+    init_tone(&detector_1750, 1750, 1);
+}
+
+void init_tone(struct tone_detector* detector, int freq, int threshold) {
+
+    detector->coef = 2.0 * cos(2.0 * FLOAT_PI * freq / AUDIO_IN_RATE);
+    detector->Q1 = 0;
+    detector->Q2 = 0;
+    detector->threshold = 200000;  // TODO
+
+}
+
+void detect_tones(int16_t * buffer) {
+
+    // Normalize buffer
+    int max_v = 0;
+    for (int i = 0; i < TONE_BUFFER_LEN; i++) {
+        max_v = fmax(abs(buffer[i]), max_v);
+    }
+
+    float coef = 32767.0 / max_v;
+
+    for (int i = 0; i < TONE_BUFFER_LEN; i++) {
+        buffer[i] *= coef;
+    }
+
+    TONE_1750_DETECTED = detect_tone(buffer, &detector_1750);
+}
+
+int detect_tone(int16_t * buffer, struct tone_detector* detector) {
+
+    float Q0;
+    int tt_samples = 0;
+    int tt = 0;
+
+    for (int i = 0; i < TONE_BUFFER_LEN; i++) {
+        Q0 = detector->coef * detector->Q1 - detector->Q2 + buffer[i];
+        detector->Q2 = detector->Q1;
+        detector->Q1 = Q0;
+
+        current_pos++;
+
+        if (current_pos == TONE_N) {
+            float m = sqrt(detector->Q1 * detector->Q1 + detector->Q2 * detector->Q2 - detector->coef * detector->Q1 * detector->Q2);
+
+            if (m > detector->threshold) {
+                tt += 1;
+            }
+
+            tt_samples++;
+            detector->Q1 = 0;
+            detector->Q2 = 0;
+            current_pos = 0;
+        }
+    }
+
+    return (float)tt / tt_samples > 0.5;
+}
diff --git a/src/common/src/Core/main.c b/src/common/src/Core/main.c
index 3fe441f..3a7d186 100644
--- a/src/common/src/Core/main.c
+++ b/src/common/src/Core/main.c
@@ -35,6 +35,8 @@
 
 /* Includes */
 #include "Audio/audio.h"
+#include "Audio/audio_in.h"
+#include "Audio/tone.h"
 #include "Audio/cw.h"
 #include "GPIO/pio.h"
 #include "GPIO/i2c.h"
@@ -194,8 +196,10 @@ static void launcher_task(void __attribute__ ((unused))*pvParameters)
         trigger_fault(FAULT_SOURCE_MAIN);
     }
 
-    usart_debug_puts("Audio init\r\n");
+    usart_debug_puts("Tone init\r\n");
+    init_tones();
 
+    usart_debug_puts("Audio init\r\n");
     audio_initialize(Audio16000HzSettings);
 
     usart_debug_puts("Audio set volume\r\n");
@@ -204,6 +208,9 @@ static void launcher_task(void __attribute__ ((unused))*pvParameters)
     usart_debug_puts("Audio set callback\r\n");
     audio_play_with_callback(audio_callback, NULL);
 
+    usart_debug_puts("Audio in init\r\n");
+    audio_in_initialize(AUDIO_IN_RATE);
+
     usart_debug_puts("Init done.\r\n");
 
     int last_qrp_from_supply = 0;