Gemma Kosaka / Mbed 2 deprecated mbed_fir

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@1:1bec1c13107e, 2019-10-23 (annotated)

Committer:
gemmaro
Date:
Wed Oct 23 02:01:18 2019 +0000
Revision:
1:1bec1c13107e
Parent:
0:f0a3e5729b8f 
test not yet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gemmaro 0:f0a3e5729b8f 1 #include "mbed.h"
gemmaro 0:f0a3e5729b8f 2
gemmaro 0:f0a3e5729b8f 3 DigitalOut myled1(LED1);
gemmaro 0:f0a3e5729b8f 4 DigitalOut myled2(LED2);
gemmaro 0:f0a3e5729b8f 5 DigitalOut myled3(LED3);
gemmaro 0:f0a3e5729b8f 6 AnalogIn MicIn(p15);
gemmaro 0:f0a3e5729b8f 7 AnalogOut HeadPhoneOut(p18);
gemmaro 0:f0a3e5729b8f 8
gemmaro 0:f0a3e5729b8f 9 #define DATA_SIZE 15000
gemmaro 0:f0a3e5729b8f 10 #define PI 3.1415926535897932384626433832795
gemmaro 0:f0a3e5729b8f 11 signed short data[DATA_SIZE];
gemmaro 0:f0a3e5729b8f 12
gemmaro 0:f0a3e5729b8f 13 /**
gemmaro 0:f0a3e5729b8f 14 * fs: Frequency Sampling
gemmaro 0:f0a3e5729b8f 15 * offset: AD-C, DA-C
gemmaro 0:f0a3e5729b8f 16 */
gemmaro 0:f0a3e5729b8f 17 Timer t;
gemmaro 0:f0a3e5729b8f 18 int fs = 8000;
gemmaro 0:f0a3e5729b8f 19 int dt_us = 125;
gemmaro 0:f0a3e5729b8f 20 int offset;
gemmaro 0:f0a3e5729b8f 21
gemmaro 0:f0a3e5729b8f 22 int filter_size;
gemmaro 0:f0a3e5729b8f 23 double b[50];
gemmaro 0:f0a3e5729b8f 24 double w[50];
gemmaro 0:f0a3e5729b8f 25
gemmaro 1:1bec1c13107e 26 namespace lib
gemmaro 1:1bec1c13107e 27 {
gemmaro 1:1bec1c13107e 28
gemmaro 1:1bec1c13107e 29 void wait100us(void)
gemmaro 1:1bec1c13107e 30 {
gemmaro 1:1bec1c13107e 31 while (t.read_us() < dt_us) {
gemmaro 1:1bec1c13107e 32 };
gemmaro 0:f0a3e5729b8f 33 t.reset();
gemmaro 0:f0a3e5729b8f 34 }
gemmaro 0:f0a3e5729b8f 35
gemmaro 1:1bec1c13107e 36 void rec(void)
gemmaro 1:1bec1c13107e 37 {
gemmaro 0:f0a3e5729b8f 38 unsigned short InDat;
gemmaro 0:f0a3e5729b8f 39 int i;
gemmaro 1:1bec1c13107e 40
gemmaro 0:f0a3e5729b8f 41 /// recording
gemmaro 0:f0a3e5729b8f 42 myled1 = 1;
gemmaro 0:f0a3e5729b8f 43 for (i = 0; i < DATA_SIZE; ++i) {
gemmaro 0:f0a3e5729b8f 44 InDat = MicIn.read_u16() >> 4;
gemmaro 0:f0a3e5729b8f 45 data[i] = InDat;
gemmaro 1:1bec1c13107e 46 lib::wait100us();
gemmaro 0:f0a3e5729b8f 47 }
gemmaro 0:f0a3e5729b8f 48 myled1 = 0;
gemmaro 0:f0a3e5729b8f 49 }
gemmaro 0:f0a3e5729b8f 50
gemmaro 1:1bec1c13107e 51 void get_offset(void)
gemmaro 1:1bec1c13107e 52 {
gemmaro 0:f0a3e5729b8f 53 unsigned short InDat;
gemmaro 0:f0a3e5729b8f 54 double a = 0;
gemmaro 0:f0a3e5729b8f 55 int i;
gemmaro 0:f0a3e5729b8f 56 for (i = 0; i < 1000; ++i) {
gemmaro 0:f0a3e5729b8f 57 InDat = MicIn.read_u16() >> 4;
gemmaro 0:f0a3e5729b8f 58 a += (double)InDat;
gemmaro 1:1bec1c13107e 59 lib::wait100us();
gemmaro 0:f0a3e5729b8f 60 }
gemmaro 0:f0a3e5729b8f 61 offset = (unsigned int)(a / 1000);
gemmaro 0:f0a3e5729b8f 62 printf("%d\n\r", offset);
gemmaro 0:f0a3e5729b8f 63 }
gemmaro 0:f0a3e5729b8f 64
gemmaro 1:1bec1c13107e 65 void hanning_window(int window_size)
gemmaro 1:1bec1c13107e 66 {
gemmaro 0:f0a3e5729b8f 67 int i;
gemmaro 0:f0a3e5729b8f 68 for (i = 0; i < window_size; ++i) {
gemmaro 0:f0a3e5729b8f 69 w[i] = 0.5 - 0.5 * cos(2 * PI * ((i + 1) - 0.5) / window_size);
gemmaro 0:f0a3e5729b8f 70 }
gemmaro 0:f0a3e5729b8f 71 }
gemmaro 0:f0a3e5729b8f 72
gemmaro 1:1bec1c13107e 73 namespace filter
gemmaro 1:1bec1c13107e 74 {
gemmaro 1:1bec1c13107e 75
gemmaro 1:1bec1c13107e 76 int low_pass(int k, int offsets, int fc)
gemmaro 1:1bec1c13107e 77 {
gemmaro 0:f0a3e5729b8f 78 if (k == offsets) {
gemmaro 0:f0a3e5729b8f 79 return 2 * fc;
gemmaro 0:f0a3e5729b8f 80 }
gemmaro 1:1bec1c13107e 81 return 2 * fc * sin(2 * PI * fc * (k - offsets)) /
gemmaro 1:1bec1c13107e 82 (2 * PI * fc * (k - offsets));
gemmaro 0:f0a3e5729b8f 83 }
gemmaro 0:f0a3e5729b8f 84
gemmaro 1:1bec1c13107e 85 int high_pass(int k, int offsets, int fc)
gemmaro 1:1bec1c13107e 86 {
gemmaro 0:f0a3e5729b8f 87 if (k == offsets) {
gemmaro 0:f0a3e5729b8f 88 return 1 - 2 * fc;
gemmaro 0:f0a3e5729b8f 89 }
gemmaro 0:f0a3e5729b8f 90 return -2 * fc * sin(2 * PI * fc * k) / (2 * PI * fc * k);
gemmaro 0:f0a3e5729b8f 91 }
gemmaro 0:f0a3e5729b8f 92
gemmaro 1:1bec1c13107e 93 int band_pass(int k, int offsets, int fc1, int fc2)
gemmaro 1:1bec1c13107e 94 {
gemmaro 0:f0a3e5729b8f 95 if (k == offsets) {
gemmaro 0:f0a3e5729b8f 96 return 2 * (fc2 - fc1);
gemmaro 0:f0a3e5729b8f 97 }
gemmaro 1:1bec1c13107e 98 return 2 * fc2 * sin(2 * PI * fc2 * k) / (2 * PI * fc2 * k) -
gemmaro 1:1bec1c13107e 99 2 * fc1 * sin(2 * PI * fc1 * k) / (2 * PI * fc1 * k);
gemmaro 0:f0a3e5729b8f 100 }
gemmaro 0:f0a3e5729b8f 101
gemmaro 1:1bec1c13107e 102 int band_eliminate(int k, int offsets, int fc1, int fc2)
gemmaro 1:1bec1c13107e 103 {
gemmaro 0:f0a3e5729b8f 104 if (k == offsets) {
gemmaro 0:f0a3e5729b8f 105 return 1 - 2 * (fc2 - fc1);
gemmaro 0:f0a3e5729b8f 106 }
gemmaro 1:1bec1c13107e 107 return 2 * fc1 * sin(2 * PI * fc1 * k) / (2 * PI * fc1 * k) -
gemmaro 1:1bec1c13107e 108 2 * fc2 * sin(2 * PI * fc2 * k) / (2 * PI * fc2 * k);
gemmaro 0:f0a3e5729b8f 109 }
gemmaro 0:f0a3e5729b8f 110
gemmaro 1:1bec1c13107e 111 } // namespace filter
gemmaro 1:1bec1c13107e 112
gemmaro 1:1bec1c13107e 113 void finite_impulse_response(int fs, int fe, int delta)
gemmaro 1:1bec1c13107e 114 {
gemmaro 0:f0a3e5729b8f 115 int offsets;
gemmaro 0:f0a3e5729b8f 116 int k;
gemmaro 0:f0a3e5729b8f 117 float fc, delta_s;
gemmaro 1:1bec1c13107e 118
gemmaro 1:1bec1c13107e 119 fc = (((float)fe + (float)delta / 2) / fs);
gemmaro 0:f0a3e5729b8f 120 delta_s = ((float)delta / fs);
gemmaro 0:f0a3e5729b8f 121 filter_size = (int)(3.1 / delta_s);
gemmaro 0:f0a3e5729b8f 122 if (filter_size % 2 == 0) filter_size += 1;
gemmaro 1:1bec1c13107e 123 lib::hanning_window(filter_size);
gemmaro 1:1bec1c13107e 124
gemmaro 0:f0a3e5729b8f 125 offsets = (filter_size - 1) / 2;
gemmaro 0:f0a3e5729b8f 126 for (k = 0; k < filter_size; ++k) {
gemmaro 1:1bec1c13107e 127 b[k] = lib::filter::low_pass(k, offsets, fc);
gemmaro 0:f0a3e5729b8f 128 }
gemmaro 1:1bec1c13107e 129
gemmaro 0:f0a3e5729b8f 130 for (k = 0; k < filter_size; ++k) {
gemmaro 0:f0a3e5729b8f 131 b[k] *= w[k];
gemmaro 0:f0a3e5729b8f 132 }
gemmaro 0:f0a3e5729b8f 133 }
gemmaro 0:f0a3e5729b8f 134
gemmaro 1:1bec1c13107e 135 void finite_impulse_response2(int fs, int fe1, int fe2, int delta)
gemmaro 1:1bec1c13107e 136 {
gemmaro 1:1bec1c13107e 137 int offsets;
gemmaro 1:1bec1c13107e 138 int k;
gemmaro 1:1bec1c13107e 139 float fc1, fc2, delta_s;
gemmaro 1:1bec1c13107e 140
gemmaro 1:1bec1c13107e 141 fc1 = (((float)fe1 + (float)delta / 2) / fs);
gemmaro 1:1bec1c13107e 142 fc2 = (((float)fe2 + (float)delta / 2) / fs);
gemmaro 1:1bec1c13107e 143 delta_s = ((float)delta / fs);
gemmaro 1:1bec1c13107e 144 filter_size = (int)(3.1 / delta_s);
gemmaro 1:1bec1c13107e 145 if (filter_size % 2 == 0) filter_size += 1;
gemmaro 1:1bec1c13107e 146 lib::hanning_window(filter_size);
gemmaro 1:1bec1c13107e 147
gemmaro 1:1bec1c13107e 148 offsets = (filter_size - 1) / 2;
gemmaro 1:1bec1c13107e 149 for (k = 0; k < filter_size; ++k) {
gemmaro 1:1bec1c13107e 150 b[k] = lib::filter::band_pass(k, offsets, fc1, fc2);
gemmaro 1:1bec1c13107e 151 }
gemmaro 1:1bec1c13107e 152
gemmaro 1:1bec1c13107e 153 for (k = 0; k < filter_size; ++k) {
gemmaro 1:1bec1c13107e 154 b[k] *= w[k];
gemmaro 1:1bec1c13107e 155 }
gemmaro 1:1bec1c13107e 156 }
gemmaro 1:1bec1c13107e 157
gemmaro 1:1bec1c13107e 158 } // namespace lib
gemmaro 1:1bec1c13107e 159
gemmaro 1:1bec1c13107e 160 int main()
gemmaro 1:1bec1c13107e 161 {
gemmaro 0:f0a3e5729b8f 162 unsigned short InDat, OutDat;
gemmaro 1:1bec1c13107e 163 int fe1 = 500;
gemmaro 1:1bec1c13107e 164 int fe2 = 600;
gemmaro 0:f0a3e5729b8f 165 int delta = 1000;
gemmaro 0:f0a3e5729b8f 166 int fs = 8000;
gemmaro 0:f0a3e5729b8f 167 int n, k;
gemmaro 0:f0a3e5729b8f 168 double y;
gemmaro 1:1bec1c13107e 169
gemmaro 0:f0a3e5729b8f 170 /// Timer interrupt
gemmaro 0:f0a3e5729b8f 171 t.start();
gemmaro 0:f0a3e5729b8f 172 t.reset();
gemmaro 1:1bec1c13107e 173 lib::get_offset();
gemmaro 1:1bec1c13107e 174
gemmaro 0:f0a3e5729b8f 175 /// Filter parameter
gemmaro 0:f0a3e5729b8f 176 myled1 = 1;
gemmaro 1:1bec1c13107e 177 // lib::finite_impulse_response(fs, fe1, delta);
gemmaro 1:1bec1c13107e 178 lib::finite_impulse_response2(fs, fe1, fe2, delta);
gemmaro 0:f0a3e5729b8f 179 myled1 = 0;
gemmaro 0:f0a3e5729b8f 180
gemmaro 1:1bec1c13107e 181 while (1) {
gemmaro 0:f0a3e5729b8f 182 myled2 = 1;
gemmaro 1:1bec1c13107e 183 lib::rec();
gemmaro 0:f0a3e5729b8f 184 wait(2);
gemmaro 0:f0a3e5729b8f 185 myled2 = 0;
gemmaro 1:1bec1c13107e 186
gemmaro 0:f0a3e5729b8f 187 /// filter
gemmaro 0:f0a3e5729b8f 188 myled3 = 1;
gemmaro 0:f0a3e5729b8f 189 for (n = filter_size; n < DATA_SIZE; ++n) {
gemmaro 0:f0a3e5729b8f 190 y = 0;
gemmaro 0:f0a3e5729b8f 191 for (k = 0; k < filter_size; ++k) {
gemmaro 0:f0a3e5729b8f 192 y += (int)(b[k] * (data[n - k] - offset));
gemmaro 0:f0a3e5729b8f 193 }
gemmaro 0:f0a3e5729b8f 194 InDat = y + offset;
gemmaro 0:f0a3e5729b8f 195 OutDat = InDat >> 2;
gemmaro 0:f0a3e5729b8f 196 HeadPhoneOut.write_u16(OutDat << 6);
gemmaro 1:1bec1c13107e 197 lib::wait100us();
gemmaro 0:f0a3e5729b8f 198 }
gemmaro 0:f0a3e5729b8f 199 myled3 = 0;
gemmaro 0:f0a3e5729b8f 200 }
gemmaro 1:1bec1c13107e 201 }