R&J
lab 1 code
Dependencies: CMSIS-DSP_for_STM32F746G BSP_DISCO_F746NG
Diff: signal_processing.cpp
- Revision:
- 28:fe7747b89fb3
- Parent:
- 27:0d65cfcc9001
- Child:
- 29:7d3fff4ac41b
--- a/signal_processing.cpp Wed Jan 01 17:31:05 2020 +0000
+++ b/signal_processing.cpp Wed Jan 01 20:47:48 2020 +0000
@@ -24,20 +24,76 @@
** ------------------------------------------------------------------- */
#define AUDIO_BLOCK_SAMPLES ((uint32_t)128) // Number of samples (L and R) in audio block (each samples is 16 bits)
-#define NUM_TAPS 29
+#define NUM_TAPS 64
+
+#define mic_distance 0.0211 // [m] Distance between microphones on ST32F746G
+#define c_sound 344 // [m/s] Speed of sound at 71 deg Fahrenheit
+#define Omega_S 16000 // [Hz] Sample rate
+#define Omega_C 1000 // [Hz] Carrier frequency
+#define omega_c 2*PI*Omega_C/Omega_S // Hilbert Transform Frequency
/* ----------------------------------------------------------------------
** FIR Coefficients buffer generated using fdatool MATLAB function.
-** lowpass filter to passband 1 kHz, stopband 2 kHz
+** Fpass = 100
+** Fstop = 400
+** Order = 255
+** Type = equiripple
+** Precision = Single-precision floating point
** ------------------------------------------------------------------- */
-const float32_t firCoeffs32[29] = {
- 0.0106548937, 0.00951616466, 0.008600869216, 0.002842310816,-0.007469337899,
- -0.01967334561, -0.02897886001, -0.0296635218, -0.01692939363, 0.01112444047,
- 0.05218170211, 0.09966956824, 0.1441507041, 0.1758090258, 0.1872722059,
- 0.1758090258, 0.1441507041, 0.09966956824, 0.05218170211, 0.01112444047,
- -0.01692939363, -0.0296635218, -0.02897886001, -0.01967334561,-0.007469337899,
- 0.002842310816, 0.008600869216, 0.00951616466, 0.0106548937
+const float32_t LPF_coeffs[256] = {
+ -3.166635361e-05,-1.870986307e-05,-2.394712101e-05,-2.990036774e-05,-3.658870264e-05,
+ -4.399371028e-05,-5.209316805e-05,-6.081685933e-05,-7.011051639e-05,-7.988819561e-05,
+ -9.004087042e-05,-0.000100387304,-0.000110787405,-0.0001210676273,-0.0001309535583,
+ -0.0001402696507,-0.0001487127156,-0.000156007547,-0.000161856733,-0.0001659371774,
+ -0.0001679307898,-0.0001674937957,-0.0001643116557,-0.0001580463286,-0.0001483879751,
+ -0.0001350368111,-0.0001177328377,-9.621375648e-05,-7.031142741e-05,-3.984871728e-05,
+ -4.752255791e-06,3.502548134e-05,7.943151286e-05, 0.00012836518,0.0001816200675,
+ 0.0002389207075,0.0002998831333,0.0003640493087,0.0004308363423,0.0004995908821,
+ 0.000569541764,0.0006398341502,0.0007095160545,0.0007775573758,0.0008428392466,
+ 0.0009041857556,0.0009603485814, 0.001010049134, 0.001051956089, 0.001084744697,
+ 0.001107070362, 0.001117624925, 0.001115125953, 0.001098367968, 0.001066206489,
+ 0.001017625327,0.0009517096914,0.0008677172009,0.0007650525076,0.0006433351082,
+ 0.0005023755366,0.0003422336886,0.0001632031344,-3.414340608e-05,-0.0002489731414,
+ -0.0004801462928,-0.0007262464496,-0.0009855407989,-0.001256009215,-0.001535319607,
+ -0.001820859499, -0.00210972107,-0.002398737939,-0.002684480743, -0.00296330289,
+ -0.003231336828,-0.003484555287,-0.003718773369,-0.003929710016,-0.004113005474,
+ -0.004264282528,-0.004379170015,-0.004453371279,-0.004482686054,-0.004463084508,
+ -0.00439072866,-0.004262049217,-0.004073764663, -0.00382295344, -0.0035070749,
+ -0.003124034265,-0.002672198461,-0.002150455024,-0.001558220945,-0.0008954905206,
+ -0.0001628377504,0.0006385485176, 0.001506871195, 0.002439704956, 0.00343400985,
+ 0.004486127291, 0.005591807421, 0.006746214349, 0.007943967357, 0.00917916093,
+ 0.01044541784, 0.01173591614, 0.01304345857, 0.01436051074, 0.01567927189,
+ 0.01699172705, 0.01828972809, 0.01956504397, 0.02080944739, 0.02201477997,
+ 0.02317302115, 0.02427636459, 0.02531728894, 0.02628861926, 0.0271836035,
+ 0.0279959701, 0.02871998027, 0.02935049124, 0.02988300286, 0.03031369485,
+ 0.03063946404, 0.03085796162, 0.03096760809, 0.03096760809, 0.03085796162,
+ 0.03063946404, 0.03031369485, 0.02988300286, 0.02935049124, 0.02871998027,
+ 0.0279959701, 0.0271836035, 0.02628861926, 0.02531728894, 0.02427636459,
+ 0.02317302115, 0.02201477997, 0.02080944739, 0.01956504397, 0.01828972809,
+ 0.01699172705, 0.01567927189, 0.01436051074, 0.01304345857, 0.01173591614,
+ 0.01044541784, 0.00917916093, 0.007943967357, 0.006746214349, 0.005591807421,
+ 0.004486127291, 0.00343400985, 0.002439704956, 0.001506871195,0.0006385485176,
+ -0.0001628377504,-0.0008954905206,-0.001558220945,-0.002150455024,-0.002672198461,
+ -0.003124034265, -0.0035070749, -0.00382295344,-0.004073764663,-0.004262049217,
+ -0.00439072866,-0.004463084508,-0.004482686054,-0.004453371279,-0.004379170015,
+ -0.004264282528,-0.004113005474,-0.003929710016,-0.003718773369,-0.003484555287,
+ -0.003231336828, -0.00296330289,-0.002684480743,-0.002398737939, -0.00210972107,
+ -0.001820859499,-0.001535319607,-0.001256009215,-0.0009855407989,-0.0007262464496,
+ -0.0004801462928,-0.0002489731414,-3.414340608e-05,0.0001632031344,0.0003422336886,
+ 0.0005023755366,0.0006433351082,0.0007650525076,0.0008677172009,0.0009517096914,
+ 0.001017625327, 0.001066206489, 0.001098367968, 0.001115125953, 0.001117624925,
+ 0.001107070362, 0.001084744697, 0.001051956089, 0.001010049134,0.0009603485814,
+ 0.0009041857556,0.0008428392466,0.0007775573758,0.0007095160545,0.0006398341502,
+ 0.000569541764,0.0004995908821,0.0004308363423,0.0003640493087,0.0002998831333,
+ 0.0002389207075,0.0001816200675, 0.00012836518,7.943151286e-05,3.502548134e-05,
+ -4.752255791e-06,-3.984871728e-05,-7.031142741e-05,-9.621375648e-05,-0.0001177328377,
+ -0.0001350368111,-0.0001483879751,-0.0001580463286,-0.0001643116557,-0.0001674937957,
+ -0.0001679307898,-0.0001659371774,-0.000161856733,-0.000156007547,-0.0001487127156,
+ -0.0001402696507,-0.0001309535583,-0.0001210676273,-0.000110787405,-0.000100387304,
+ -9.004087042e-05,-7.988819561e-05,-7.011051639e-05,-6.081685933e-05,-5.209316805e-05,
+ -4.399371028e-05,-3.658870264e-05,-2.990036774e-05,-2.394712101e-05,-1.870986307e-05,
+ -3.166635361e-05
};
@@ -45,13 +101,34 @@
* Declare State buffer of size (numTaps + blockSize - 1)
* ------------------------------------------------------------------- */
-static float32_t firStateF32[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
+static float32_t S_0i_FIRstate[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
+static float32_t S_0q_FIRstate[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
+static float32_t S_1i_FIRstate[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
+static float32_t S_1q_FIRstate[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
+
+/* Need to have multiplies pre-computed */
+static float32_t i_phase_mult_array[AUDIO_BLOCK_SAMPLES];
+static float32_t q_phase_mult_array[AUDIO_BLOCK_SAMPLES];
-static float32_t output_array_F32[AUDIO_BLOCK_SAMPLES];
+static float32_t x_0i[AUDIO_BLOCK_SAMPLES];
+static float32_t x_0q[AUDIO_BLOCK_SAMPLES];
+static float32_t x_1i[AUDIO_BLOCK_SAMPLES];
+static float32_t x_1q[AUDIO_BLOCK_SAMPLES];
+
+static float32_t s_0i[AUDIO_BLOCK_SAMPLES];
+static float32_t s_0q[AUDIO_BLOCK_SAMPLES];
+static float32_t s_1i[AUDIO_BLOCK_SAMPLES];
+static float32_t s_1q[AUDIO_BLOCK_SAMPLES];
+
+static float32_t s0_s1star_real[AUDIO_BLOCK_SAMPLES];
+static float32_t s0_s1star_imag[AUDIO_BLOCK_SAMPLES];
/* Important to have the structure outside of the execution so it can be initialized */
-arm_fir_instance_f32 S;
+arm_fir_instance_f32 S_0i;
+arm_fir_instance_f32 S_0q;
+arm_fir_instance_f32 S_1i;
+arm_fir_instance_f32 S_1q;
/**
* @brief Initialize filter structures to be used in loops later
@@ -60,8 +137,18 @@
void initalize_signal_processing(void) {
/* Call FIR init function to initialize the instance structure. */
- arm_fir_init_f32(&S, NUM_TAPS, (float32_t *)&firCoeffs32[0], &firStateF32[0], AUDIO_BLOCK_SAMPLES);
+ //arm_fir_init_f32(&S, NUM_TAPS, (float32_t *)&firCoeffs32[0], &firStateF32[0], AUDIO_BLOCK_SAMPLES);
+ arm_fir_init_f32(&S_0i, NUM_TAPS, (float32_t *) &LPF_coeffs[0], &S_0i_FIRstate[0], AUDIO_BLOCK_SAMPLES);
+ arm_fir_init_f32(&S_0q, NUM_TAPS, (float32_t *) &LPF_coeffs[0], &S_0q_FIRstate[0], AUDIO_BLOCK_SAMPLES);
+ arm_fir_init_f32(&S_1i, NUM_TAPS, (float32_t *) &LPF_coeffs[0], &S_1i_FIRstate[0], AUDIO_BLOCK_SAMPLES);
+ arm_fir_init_f32(&S_1q, NUM_TAPS, (float32_t *) &LPF_coeffs[0], &S_1q_FIRstate[0], AUDIO_BLOCK_SAMPLES);
+ uint32_t i;
+ for (i=0; i<AUDIO_BLOCK_SAMPLES; i++)
+ {
+ i_phase_mult_array[i] = cos(omega_c * i);
+ q_phase_mult_array[i] = -sin(omega_c * i);
+ }
}
/**
@@ -73,39 +160,72 @@
*/
void process_audio_channel_signals(float* L_channel, float* R_channel, uint16_t Signal_Length)
{
- char buf[40];
-
+ char buf[40];
BSP_LCD_SetTextColor(LCD_COLOR_CYAN);
sprintf(buf, "Processing Signals" );
BSP_LCD_DisplayStringAt(0, 150, (uint8_t *) buf, LEFT_MODE);
-
-
- uint32_t i;
- arm_status status;
- float32_t *inputF32, *outputF32;
-
- inputF32 = L_channel;
- outputF32 = &output_array_F32[0];
+
+ uint32_t i;
- /* ----------------------------------------------------------------------
- ** Call the FIR process function for every blockSize samples
- ** ------------------------------------------------------------------- */
-
- arm_fir_f32(&S, inputF32, outputF32, AUDIO_BLOCK_SAMPLES);
-
- /* The following just passes things from the input to the output */
- float32_t* L_chan_mem_address;
- float32_t* R_chan_mem_address;
+ /* Need these addresses */
+ float32_t* L_chan_mem_address = L_channel;
+ float32_t* R_chan_mem_address = R_channel;
float32_t L_audio_value;
float32_t R_audio_value;
+
+ for (i=0; i<Signal_Length; i++)
+ {
+ L_audio_value = *L_chan_mem_address;
+ x_0i[i] = L_audio_value * i_phase_mult_array[i];
+ x_0q[i] = L_audio_value * q_phase_mult_array[i];
+ R_audio_value = *R_chan_mem_address;
+ x_1i[i] = R_audio_value * i_phase_mult_array[i];
+ x_1q[i] = R_audio_value * q_phase_mult_array[i];
+
+ L_chan_mem_address++;
+ R_chan_mem_address++;
+ }
+
+ /* Calls the FIR filters and proceeses the audio block */
+ arm_fir_f32(&S_0i, &x_0i[0], &s_0i[0], AUDIO_BLOCK_SAMPLES);
+ arm_fir_f32(&S_0q, &x_0q[0], &s_0q[0], AUDIO_BLOCK_SAMPLES);
+ arm_fir_f32(&S_1i, &x_1i[0], &s_1i[0], AUDIO_BLOCK_SAMPLES);
+ arm_fir_f32(&S_1q, &x_1q[0], &s_1q[0], AUDIO_BLOCK_SAMPLES);
+
+
+ float32_t s0_s1star_sum_real = 0;
+ float32_t s0_s1star_sum_imag = 0;
+
+ /* Complex Multiply and Summation*/
+ for (i=0; i<Signal_Length; i++)
+ {
+ s0_s1star_real[i] = s_0i[i] * s_1i[i] + s_0q[i] * s_1q[i];
+ s0_s1star_imag[i] = s_0q[i] * s_1i[i] - s_0i[i] * s_1q[i];
+
+ s0_s1star_sum_real += s0_s1star_real[i];
+ s0_s1star_sum_imag += s0_s1star_imag[i];
+
+ }
+
+ /* Compute angle */
+ float32_t phase_angle = atan2(s0_s1star_sum_imag, s0_s1star_sum_real);
+ float32_t direction_angle_radians = asin((c_sound / ((float32_t) mic_distance * (float32_t) Omega_C)) * phase_angle);
+ float32_t direction_angle_degrees = direction_angle_radians * (180 / PI);
+
+ BSP_LCD_SetTextColor(LCD_COLOR_CYAN);
+ sprintf(buf, "Angle:%6.2f degrees", direction_angle_degrees);
+ BSP_LCD_DisplayStringAt(0, 175, (uint8_t *) buf, LEFT_MODE);
+
+ sprintf(buf, "PAngle:%6.2f rad", phase_angle);
+ BSP_LCD_DisplayStringAt(0, 200, (uint8_t *) buf, LEFT_MODE);
+
L_chan_mem_address = L_channel;
R_chan_mem_address = R_channel;
-
for (i=0; i<Signal_Length; i++)
{
//L_audio_value = *L_chan_mem_address;
- L_audio_value = output_array_F32[i];
+ L_audio_value = *L_chan_mem_address;
*L_chan_mem_address = L_audio_value;
L_chan_mem_address++;