Diff: main.cpp

Revision:

0:f0a3e5729b8f

Child:

1:1bec1c13107e

diff -r 000000000000 -r f0a3e5729b8f main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 16 04:09:09 2019 +0000
@@ -0,0 +1,152 @@
+#include "mbed.h"
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+AnalogIn MicIn(p15);
+AnalogOut HeadPhoneOut(p18);
+
+#define DATA_SIZE 15000
+#define PI 3.1415926535897932384626433832795
+signed short data[DATA_SIZE];
+
+/**
+ * fs: Frequency Sampling
+ * offset: AD-C, DA-C
+ */
+Timer t;
+int fs = 8000;
+int dt_us = 125;
+int offset;
+
+int filter_size;
+double b[50];
+double w[50];
+
+void wait100us(void) {
+    while (t.read_us() < dt_us) {};
+    t.reset();
+}
+
+void rec(void) {
+    unsigned short InDat;
+    int i;
+    
+    /// recording
+    myled1 = 1;
+    for (i = 0; i < DATA_SIZE; ++i) {
+        InDat = MicIn.read_u16() >> 4;
+        data[i] = InDat;
+        wait100us();
+    }
+    myled1 = 0;
+}
+
+void get_offset(void) {
+    unsigned short InDat;
+    double a = 0;
+    int i;
+    for (i = 0; i < 1000; ++i) {
+        InDat = MicIn.read_u16() >> 4;
+        a += (double)InDat;
+        wait100us();
+    }
+    offset = (unsigned int)(a / 1000);
+    printf("%d\n\r", offset);
+}
+
+void hanning_window(int window_size) {
+    int i;
+    for (i = 0; i < window_size; ++i) {
+        w[i] = 0.5 - 0.5 * cos(2 * PI * ((i + 1) - 0.5) / window_size);
+    }
+}
+
+int lps(int k, int offsets, int fc) {
+    if (k == offsets) {
+        return 2 * fc;
+    }
+    return 2 * fc * sin(2 * PI * fc * (k - offsets)) / (2 * PI * fc * (k - offsets));
+}
+
+int high_pass_filter(int k, int offsets, int fc) {
+    if (k == offsets) {
+        return 1 - 2 * fc;
+    }
+    return -2 * fc * sin(2 * PI * fc * k) / (2 * PI * fc * k);
+}
+
+int band_pass_filter(int k, int offsets, int fc1, int fc2) {
+    if (k == offsets) {
+        return 2 * (fc2 - fc1);
+    }
+    return 2 * fc2 * sin(2 * PI * fc2 * k) / (2 * PI * fc2 * k) - 2 * fc1 * sin(2 * PI * fc1 * k) / (2 * PI * fc1 * k);
+}
+
+int band_eliminate_filter(int k, int offsets, int fc1, int fc2) {
+    if (k == offsets) {
+        return 1 - 2 * (fc2 - fc1);
+    }
+    return 2 * fc1 * sin(2 * PI * fc1 * k) / (2 * PI * fc1 * k) - 2 * fc2 * sin(2 * PI * fc2 * k) / (2 * PI * fc2 * k);
+}
+
+void fir_lpf(int fs, int fe, int delta) {
+    int offsets;
+    int k;
+    float fc, delta_s;
+    
+    fc = (((float)fe +(float)delta / 2) / fs);
+    delta_s = ((float)delta / fs);
+    filter_size = (int)(3.1 / delta_s);
+    if (filter_size % 2 == 0) filter_size += 1;
+    hanning_window(filter_size);
+    
+    offsets = (filter_size - 1) / 2;
+    for (k = 0; k < filter_size; ++k) {
+        b[k] = lps(k, offsets, fc);
+    }
+    
+    for (k = 0; k < filter_size; ++k) {
+        b[k] *= w[k];
+    }
+}
+
+int main() {
+    unsigned short InDat, OutDat;
+    int fe = 100;
+    int delta = 1000;
+    int fs = 8000;
+    int n, k;
+    double y;
+    
+    /// Timer interrupt
+    t.start();
+    t.reset();
+    get_offset();
+    
+    /// Filter parameter
+    myled1 = 1;
+    fir_lpf(fs, fe, delta);
+    myled1 = 0;
+
+    while(1) {
+        myled2 = 1;
+        rec();
+        wait(2);
+        myled2 = 0;
+        
+        /// filter
+        myled3 = 1;
+        for (n = filter_size; n < DATA_SIZE; ++n) {
+            y = 0;
+            for (k = 0; k < filter_size; ++k) {
+                y += (int)(b[k] * (data[n - k] - offset));
+            }
+            InDat = y + offset;
+            OutDat = InDat >> 2;
+            HeadPhoneOut.write_u16(OutDat << 6);
+            wait100us();
+        }
+        myled3 = 0;
+    }
+}