R&J / Mbed OS 487_Laboratory_1

lab 1 code

Dependencies:   CMSIS-DSP_for_STM32F746G BSP_DISCO_F746NG

signal_processing.cpp@29:7d3fff4ac41b, 2020-01-01 (annotated)

Committer:
bmazzeo
Date:
Wed Jan 01 23:20:43 2020 +0000
Revision:
29:7d3fff4ac41b
Parent:
28:fe7747b89fb3
Child:
30:debea332cdfe
It works - perfectly - for carrier frequency of 500 Hz.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bmazzeo 23:d938f87dd1ee 1 /**
bmazzeo 23:d938f87dd1ee 2 ******************************************************************************
bmazzeo 23:d938f87dd1ee 3 * @file signal_processing.c
bmazzeo 23:d938f87dd1ee 4 * @author Brian Mazzeo
bmazzeo 23:d938f87dd1ee 5 * @date 2020
bmazzeo 23:d938f87dd1ee 6 * @brief This file provides a set of code for signal processing in 487.
bmazzeo 23:d938f87dd1ee 7 * Parts are taken from example code from STMIcroelectronics
bmazzeo 23:d938f87dd1ee 8 ******************************************************************************
bmazzeo 23:d938f87dd1ee 9 * @attention
bmazzeo 23:d938f87dd1ee 10 * This code was specifically developed for BYU ECEn 487 course
bmazzeo 23:d938f87dd1ee 11 * Introduction to Digital Signal Processing.
bmazzeo 23:d938f87dd1ee 12 *
bmazzeo 23:d938f87dd1ee 13 *
bmazzeo 23:d938f87dd1ee 14 ******************************************************************************
bmazzeo 23:d938f87dd1ee 15 */
bmazzeo 23:d938f87dd1ee 16
bmazzeo 23:d938f87dd1ee 17 #include "mbed.h"
bmazzeo 23:d938f87dd1ee 18 #include "stm32746g_discovery_lcd.h"
bmazzeo 24:9ac1187f9012 19 #include "arm_math.h"
bmazzeo 24:9ac1187f9012 20
bmazzeo 24:9ac1187f9012 21
bmazzeo 24:9ac1187f9012 22 /* ----------------------------------------------------------------------
bmazzeo 27:0d65cfcc9001 23 ** Defines for signal processing
bmazzeo 24:9ac1187f9012 24 ** ------------------------------------------------------------------- */
bmazzeo 24:9ac1187f9012 25
bmazzeo 24:9ac1187f9012 26 #define AUDIO_BLOCK_SAMPLES ((uint32_t)128) // Number of samples (L and R) in audio block (each samples is 16 bits)
bmazzeo 28:fe7747b89fb3 27 #define NUM_TAPS 64
bmazzeo 29:7d3fff4ac41b 28 #define CYCLE_AVERAGE_NUM 50
bmazzeo 29:7d3fff4ac41b 29
bmazzeo 28:fe7747b89fb3 30
bmazzeo 28:fe7747b89fb3 31 #define mic_distance 0.0211 // [m] Distance between microphones on ST32F746G
bmazzeo 28:fe7747b89fb3 32 #define c_sound 344 // [m/s] Speed of sound at 71 deg Fahrenheit
bmazzeo 29:7d3fff4ac41b 33 #define Omega_S (2*PI*16000) // [rad Hz] Sample rate
bmazzeo 29:7d3fff4ac41b 34 #define Omega_C (2*PI*500) // [rad Hz] Carrier frequency
bmazzeo 28:fe7747b89fb3 35 #define omega_c 2*PI*Omega_C/Omega_S // Hilbert Transform Frequency
bmazzeo 24:9ac1187f9012 36
bmazzeo 24:9ac1187f9012 37 /* ----------------------------------------------------------------------
bmazzeo 25:5412779376a7 38 ** FIR Coefficients buffer generated using fdatool MATLAB function.
bmazzeo 28:fe7747b89fb3 39 ** Fpass = 100
bmazzeo 28:fe7747b89fb3 40 ** Fstop = 400
bmazzeo 28:fe7747b89fb3 41 ** Order = 255
bmazzeo 28:fe7747b89fb3 42 ** Type = equiripple
bmazzeo 28:fe7747b89fb3 43 ** Precision = Single-precision floating point
bmazzeo 25:5412779376a7 44 ** ------------------------------------------------------------------- */
bmazzeo 25:5412779376a7 45
bmazzeo 28:fe7747b89fb3 46 const float32_t LPF_coeffs[256] = {
bmazzeo 29:7d3fff4ac41b 47 -0.0007800915628,-0.0001674496889,-0.0001849002729,-0.0002029328898,-0.0002218717564,
bmazzeo 29:7d3fff4ac41b 48 -0.0002413307957,-0.0002615691628,-0.0002821780508,-0.0003034212859,-0.000324951252,
bmazzeo 29:7d3fff4ac41b 49 -0.0003471312812,-0.000369623187,-0.000392670423,-0.0004156443756,-0.0004385936772,
bmazzeo 29:7d3fff4ac41b 50 -0.0004611753393,-0.0004842103226,-0.0005074346554,-0.0005290624686,-0.0005512404023,
bmazzeo 29:7d3fff4ac41b 51 -0.0005724770599,-0.0005929443287,-0.0006127390661,-0.0006314100465,-0.0006491405657,
bmazzeo 29:7d3fff4ac41b 52 -0.0006653870223,-0.000680299243,-0.0006933949771,-0.0007049014093,-0.0007143636467,
bmazzeo 29:7d3fff4ac41b 53 -0.0007218476967,-0.0007267798646,-0.0007293550298,-0.0007292577648,-0.0007265052409,
bmazzeo 29:7d3fff4ac41b 54 -0.0007203418063,-0.0007115086773,-0.0006989029353,-0.0006829997874,-0.0006631698925,
bmazzeo 29:7d3fff4ac41b 55 -0.000639601436,-0.0006118300371,-0.0005799498758,-0.0005434799823,-0.0005025339779,
bmazzeo 29:7d3fff4ac41b 56 -0.0004567068536,-0.0004061603686,-0.0003504161141,-0.0002896062215,-0.0002233447594,
bmazzeo 29:7d3fff4ac41b 57 -0.0001518396166,-7.459068001e-05,8.225716556e-06,9.678629431e-05,0.0001911890868,
bmazzeo 29:7d3fff4ac41b 58 0.0002915640071,0.0003978571913,0.0005103156436,0.0006287195138,0.0007533723838,
bmazzeo 29:7d3fff4ac41b 59 0.00088403275, 0.00102098356, 0.001163926558, 0.001313094515, 0.001468220609,
bmazzeo 29:7d3fff4ac41b 60 0.00162948342, 0.001796556171, 0.00196959055, 0.002148323692, 0.002332823817,
bmazzeo 29:7d3fff4ac41b 61 0.002522682771, 0.002718108008, 0.002918580314, 0.003124272451, 0.003334705485,
bmazzeo 29:7d3fff4ac41b 62 0.003549913643, 0.003769469913, 0.003993340768, 0.004221097566, 0.00445264997,
bmazzeo 29:7d3fff4ac41b 63 0.004687562119, 0.004925743211, 0.005166693125, 0.005410279613, 0.00565600628,
bmazzeo 29:7d3fff4ac41b 64 0.005903759971, 0.006152981427, 0.006403496955, 0.006654826459, 0.006906681694,
bmazzeo 29:7d3fff4ac41b 65 0.007158623077, 0.00741035305, 0.007661380339, 0.007911451161, 0.00816002395,
bmazzeo 29:7d3fff4ac41b 66 0.008406852372, 0.008651374839, 0.008893367834, 0.009132288396, 0.009367863648,
bmazzeo 29:7d3fff4ac41b 67 0.00959957391, 0.00982714165, 0.01005008724, 0.01026809961, 0.01048072334,
bmazzeo 29:7d3fff4ac41b 68 0.01068769302, 0.01088850573, 0.01108295284, 0.01127053425, 0.01145104971,
bmazzeo 29:7d3fff4ac41b 69 0.01162407082, 0.01178936474, 0.01194655988, 0.01209541038, 0.01223561913,
bmazzeo 29:7d3fff4ac41b 70 0.01236695051, 0.01248912979, 0.01260196976, 0.01270521339, 0.01279872097,
bmazzeo 29:7d3fff4ac41b 71 0.01288224943, 0.01295570657, 0.01301891916, 0.01307178196, 0.01311419997,
bmazzeo 29:7d3fff4ac41b 72 0.01314606518, 0.01316735335, 0.01317800116, 0.01317800116, 0.01316735335,
bmazzeo 29:7d3fff4ac41b 73 0.01314606518, 0.01311419997, 0.01307178196, 0.01301891916, 0.01295570657,
bmazzeo 29:7d3fff4ac41b 74 0.01288224943, 0.01279872097, 0.01270521339, 0.01260196976, 0.01248912979,
bmazzeo 29:7d3fff4ac41b 75 0.01236695051, 0.01223561913, 0.01209541038, 0.01194655988, 0.01178936474,
bmazzeo 29:7d3fff4ac41b 76 0.01162407082, 0.01145104971, 0.01127053425, 0.01108295284, 0.01088850573,
bmazzeo 29:7d3fff4ac41b 77 0.01068769302, 0.01048072334, 0.01026809961, 0.01005008724, 0.00982714165,
bmazzeo 29:7d3fff4ac41b 78 0.00959957391, 0.009367863648, 0.009132288396, 0.008893367834, 0.008651374839,
bmazzeo 29:7d3fff4ac41b 79 0.008406852372, 0.00816002395, 0.007911451161, 0.007661380339, 0.00741035305,
bmazzeo 29:7d3fff4ac41b 80 0.007158623077, 0.006906681694, 0.006654826459, 0.006403496955, 0.006152981427,
bmazzeo 29:7d3fff4ac41b 81 0.005903759971, 0.00565600628, 0.005410279613, 0.005166693125, 0.004925743211,
bmazzeo 29:7d3fff4ac41b 82 0.004687562119, 0.00445264997, 0.004221097566, 0.003993340768, 0.003769469913,
bmazzeo 29:7d3fff4ac41b 83 0.003549913643, 0.003334705485, 0.003124272451, 0.002918580314, 0.002718108008,
bmazzeo 29:7d3fff4ac41b 84 0.002522682771, 0.002332823817, 0.002148323692, 0.00196959055, 0.001796556171,
bmazzeo 29:7d3fff4ac41b 85 0.00162948342, 0.001468220609, 0.001313094515, 0.001163926558, 0.00102098356,
bmazzeo 29:7d3fff4ac41b 86 0.00088403275,0.0007533723838,0.0006287195138,0.0005103156436,0.0003978571913,
bmazzeo 29:7d3fff4ac41b 87 0.0002915640071,0.0001911890868,9.678629431e-05,8.225716556e-06,-7.459068001e-05,
bmazzeo 29:7d3fff4ac41b 88 -0.0001518396166,-0.0002233447594,-0.0002896062215,-0.0003504161141,-0.0004061603686,
bmazzeo 29:7d3fff4ac41b 89 -0.0004567068536,-0.0005025339779,-0.0005434799823,-0.0005799498758,-0.0006118300371,
bmazzeo 29:7d3fff4ac41b 90 -0.000639601436,-0.0006631698925,-0.0006829997874,-0.0006989029353,-0.0007115086773,
bmazzeo 29:7d3fff4ac41b 91 -0.0007203418063,-0.0007265052409,-0.0007292577648,-0.0007293550298,-0.0007267798646,
bmazzeo 29:7d3fff4ac41b 92 -0.0007218476967,-0.0007143636467,-0.0007049014093,-0.0006933949771,-0.000680299243,
bmazzeo 29:7d3fff4ac41b 93 -0.0006653870223,-0.0006491405657,-0.0006314100465,-0.0006127390661,-0.0005929443287,
bmazzeo 29:7d3fff4ac41b 94 -0.0005724770599,-0.0005512404023,-0.0005290624686,-0.0005074346554,-0.0004842103226,
bmazzeo 29:7d3fff4ac41b 95 -0.0004611753393,-0.0004385936772,-0.0004156443756,-0.000392670423,-0.000369623187,
bmazzeo 29:7d3fff4ac41b 96 -0.0003471312812,-0.000324951252,-0.0003034212859,-0.0002821780508,-0.0002615691628,
bmazzeo 29:7d3fff4ac41b 97 -0.0002413307957,-0.0002218717564,-0.0002029328898,-0.0001849002729,-0.0001674496889,
bmazzeo 29:7d3fff4ac41b 98 -0.0007800915628
bmazzeo 25:5412779376a7 99 };
bmazzeo 25:5412779376a7 100
bmazzeo 24:9ac1187f9012 101
bmazzeo 24:9ac1187f9012 102 /* -------------------------------------------------------------------
bmazzeo 24:9ac1187f9012 103 * Declare State buffer of size (numTaps + blockSize - 1)
bmazzeo 24:9ac1187f9012 104 * ------------------------------------------------------------------- */
bmazzeo 24:9ac1187f9012 105
bmazzeo 28:fe7747b89fb3 106 static float32_t S_0i_FIRstate[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
bmazzeo 28:fe7747b89fb3 107 static float32_t S_0q_FIRstate[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
bmazzeo 28:fe7747b89fb3 108 static float32_t S_1i_FIRstate[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
bmazzeo 28:fe7747b89fb3 109 static float32_t S_1q_FIRstate[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
bmazzeo 28:fe7747b89fb3 110
bmazzeo 28:fe7747b89fb3 111 /* Need to have multiplies pre-computed */
bmazzeo 28:fe7747b89fb3 112 static float32_t i_phase_mult_array[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 113 static float32_t q_phase_mult_array[AUDIO_BLOCK_SAMPLES];
bmazzeo 24:9ac1187f9012 114
bmazzeo 28:fe7747b89fb3 115 static float32_t x_0i[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 116 static float32_t x_0q[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 117 static float32_t x_1i[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 118 static float32_t x_1q[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 119
bmazzeo 28:fe7747b89fb3 120 static float32_t s_0i[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 121 static float32_t s_0q[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 122 static float32_t s_1i[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 123 static float32_t s_1q[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 124
bmazzeo 28:fe7747b89fb3 125 static float32_t s0_s1star_real[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 126 static float32_t s0_s1star_imag[AUDIO_BLOCK_SAMPLES];
bmazzeo 24:9ac1187f9012 127
bmazzeo 29:7d3fff4ac41b 128 static float32_t cycle_results[CYCLE_AVERAGE_NUM];
bmazzeo 29:7d3fff4ac41b 129 uint32_t cycle_count;
bmazzeo 24:9ac1187f9012 130
bmazzeo 25:5412779376a7 131 /* Important to have the structure outside of the execution so it can be initialized */
bmazzeo 28:fe7747b89fb3 132 arm_fir_instance_f32 S_0i;
bmazzeo 28:fe7747b89fb3 133 arm_fir_instance_f32 S_0q;
bmazzeo 28:fe7747b89fb3 134 arm_fir_instance_f32 S_1i;
bmazzeo 28:fe7747b89fb3 135 arm_fir_instance_f32 S_1q;
bmazzeo 23:d938f87dd1ee 136
bmazzeo 29:7d3fff4ac41b 137
bmazzeo 29:7d3fff4ac41b 138 /* FUNCTION DEFINITIONS BELOW */
bmazzeo 29:7d3fff4ac41b 139
bmazzeo 25:5412779376a7 140 /**
bmazzeo 25:5412779376a7 141 * @brief Initialize filter structures to be used in loops later
bmazzeo 25:5412779376a7 142 * @retval None
bmazzeo 25:5412779376a7 143 */
bmazzeo 25:5412779376a7 144 void initalize_signal_processing(void) {
bmazzeo 25:5412779376a7 145
bmazzeo 25:5412779376a7 146 /* Call FIR init function to initialize the instance structure. */
bmazzeo 28:fe7747b89fb3 147 //arm_fir_init_f32(&S, NUM_TAPS, (float32_t *)&firCoeffs32[0], &firStateF32[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 28:fe7747b89fb3 148 arm_fir_init_f32(&S_0i, NUM_TAPS, (float32_t *) &LPF_coeffs[0], &S_0i_FIRstate[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 28:fe7747b89fb3 149 arm_fir_init_f32(&S_0q, NUM_TAPS, (float32_t *) &LPF_coeffs[0], &S_0q_FIRstate[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 28:fe7747b89fb3 150 arm_fir_init_f32(&S_1i, NUM_TAPS, (float32_t *) &LPF_coeffs[0], &S_1i_FIRstate[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 28:fe7747b89fb3 151 arm_fir_init_f32(&S_1q, NUM_TAPS, (float32_t *) &LPF_coeffs[0], &S_1q_FIRstate[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 25:5412779376a7 152
bmazzeo 28:fe7747b89fb3 153 uint32_t i;
bmazzeo 28:fe7747b89fb3 154 for (i=0; i<AUDIO_BLOCK_SAMPLES; i++)
bmazzeo 28:fe7747b89fb3 155 {
bmazzeo 28:fe7747b89fb3 156 i_phase_mult_array[i] = cos(omega_c * i);
bmazzeo 28:fe7747b89fb3 157 q_phase_mult_array[i] = -sin(omega_c * i);
bmazzeo 28:fe7747b89fb3 158 }
bmazzeo 29:7d3fff4ac41b 159
bmazzeo 29:7d3fff4ac41b 160 for (i=0; i<CYCLE_AVERAGE_NUM; i++)
bmazzeo 29:7d3fff4ac41b 161 {
bmazzeo 29:7d3fff4ac41b 162 cycle_results[i] = 0;
bmazzeo 29:7d3fff4ac41b 163 }
bmazzeo 29:7d3fff4ac41b 164 cycle_count = 0;
bmazzeo 29:7d3fff4ac41b 165
bmazzeo 25:5412779376a7 166 }
bmazzeo 23:d938f87dd1ee 167
bmazzeo 23:d938f87dd1ee 168 /**
bmazzeo 23:d938f87dd1ee 169 * @brief Process audio channel signals
bmazzeo 23:d938f87dd1ee 170 * @param L_channel: Pointer to Left channel data (float)
bmazzeo 23:d938f87dd1ee 171 * @param R_channel: Pointer to Right channel data (float)
bmazzeo 23:d938f87dd1ee 172 * @param Signal_Length: length of data to process
bmazzeo 23:d938f87dd1ee 173 * @retval None
bmazzeo 23:d938f87dd1ee 174 */
bmazzeo 23:d938f87dd1ee 175 void process_audio_channel_signals(float* L_channel, float* R_channel, uint16_t Signal_Length)
bmazzeo 23:d938f87dd1ee 176 {
bmazzeo 28:fe7747b89fb3 177 char buf[40];
bmazzeo 29:7d3fff4ac41b 178 /*
bmazzeo 23:d938f87dd1ee 179 BSP_LCD_SetTextColor(LCD_COLOR_CYAN);
bmazzeo 23:d938f87dd1ee 180 sprintf(buf, "Processing Signals" );
bmazzeo 23:d938f87dd1ee 181 BSP_LCD_DisplayStringAt(0, 150, (uint8_t *) buf, LEFT_MODE);
bmazzeo 29:7d3fff4ac41b 182 */
bmazzeo 28:fe7747b89fb3 183
bmazzeo 28:fe7747b89fb3 184 uint32_t i;
bmazzeo 24:9ac1187f9012 185
bmazzeo 28:fe7747b89fb3 186 /* Need these addresses */
bmazzeo 28:fe7747b89fb3 187 float32_t* L_chan_mem_address = L_channel;
bmazzeo 28:fe7747b89fb3 188 float32_t* R_chan_mem_address = R_channel;
bmazzeo 24:9ac1187f9012 189 float32_t L_audio_value;
bmazzeo 24:9ac1187f9012 190 float32_t R_audio_value;
bmazzeo 28:fe7747b89fb3 191
bmazzeo 28:fe7747b89fb3 192 for (i=0; i<Signal_Length; i++)
bmazzeo 28:fe7747b89fb3 193 {
bmazzeo 28:fe7747b89fb3 194 L_audio_value = *L_chan_mem_address;
bmazzeo 28:fe7747b89fb3 195 x_0i[i] = L_audio_value * i_phase_mult_array[i];
bmazzeo 28:fe7747b89fb3 196 x_0q[i] = L_audio_value * q_phase_mult_array[i];
bmazzeo 23:d938f87dd1ee 197
bmazzeo 28:fe7747b89fb3 198 R_audio_value = *R_chan_mem_address;
bmazzeo 28:fe7747b89fb3 199 x_1i[i] = R_audio_value * i_phase_mult_array[i];
bmazzeo 28:fe7747b89fb3 200 x_1q[i] = R_audio_value * q_phase_mult_array[i];
bmazzeo 28:fe7747b89fb3 201
bmazzeo 28:fe7747b89fb3 202 L_chan_mem_address++;
bmazzeo 28:fe7747b89fb3 203 R_chan_mem_address++;
bmazzeo 28:fe7747b89fb3 204 }
bmazzeo 28:fe7747b89fb3 205
bmazzeo 28:fe7747b89fb3 206 /* Calls the FIR filters and proceeses the audio block */
bmazzeo 28:fe7747b89fb3 207 arm_fir_f32(&S_0i, &x_0i[0], &s_0i[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 28:fe7747b89fb3 208 arm_fir_f32(&S_0q, &x_0q[0], &s_0q[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 28:fe7747b89fb3 209 arm_fir_f32(&S_1i, &x_1i[0], &s_1i[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 28:fe7747b89fb3 210 arm_fir_f32(&S_1q, &x_1q[0], &s_1q[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 28:fe7747b89fb3 211
bmazzeo 28:fe7747b89fb3 212
bmazzeo 28:fe7747b89fb3 213 float32_t s0_s1star_sum_real = 0;
bmazzeo 28:fe7747b89fb3 214 float32_t s0_s1star_sum_imag = 0;
bmazzeo 28:fe7747b89fb3 215
bmazzeo 28:fe7747b89fb3 216 /* Complex Multiply and Summation*/
bmazzeo 28:fe7747b89fb3 217 for (i=0; i<Signal_Length; i++)
bmazzeo 28:fe7747b89fb3 218 {
bmazzeo 28:fe7747b89fb3 219 s0_s1star_real[i] = s_0i[i] * s_1i[i] + s_0q[i] * s_1q[i];
bmazzeo 28:fe7747b89fb3 220 s0_s1star_imag[i] = s_0q[i] * s_1i[i] - s_0i[i] * s_1q[i];
bmazzeo 28:fe7747b89fb3 221
bmazzeo 28:fe7747b89fb3 222 s0_s1star_sum_real += s0_s1star_real[i];
bmazzeo 28:fe7747b89fb3 223 s0_s1star_sum_imag += s0_s1star_imag[i];
bmazzeo 28:fe7747b89fb3 224
bmazzeo 28:fe7747b89fb3 225 }
bmazzeo 28:fe7747b89fb3 226
bmazzeo 29:7d3fff4ac41b 227 /* Compute phase angle */
bmazzeo 29:7d3fff4ac41b 228 float32_t phase_angle = atan2(s0_s1star_sum_imag, s0_s1star_sum_real);
bmazzeo 29:7d3fff4ac41b 229
bmazzeo 29:7d3fff4ac41b 230 /* The following computes a running average - to reduce noise in the phase */
bmazzeo 29:7d3fff4ac41b 231 cycle_results[cycle_count] = phase_angle;
bmazzeo 29:7d3fff4ac41b 232 float32_t current_sum = 0;
bmazzeo 29:7d3fff4ac41b 233 for (i=0; i<CYCLE_AVERAGE_NUM; i++)
bmazzeo 29:7d3fff4ac41b 234 {
bmazzeo 29:7d3fff4ac41b 235 current_sum += cycle_results[i];
bmazzeo 29:7d3fff4ac41b 236 }
bmazzeo 29:7d3fff4ac41b 237 phase_angle = current_sum / CYCLE_AVERAGE_NUM;
bmazzeo 29:7d3fff4ac41b 238
bmazzeo 29:7d3fff4ac41b 239 /* Computes the actual DOA angle */
bmazzeo 29:7d3fff4ac41b 240 float32_t delta_t = phase_angle / Omega_C;
bmazzeo 29:7d3fff4ac41b 241 float32_t direction_angle_radians = asin(c_sound * delta_t / mic_distance);
bmazzeo 29:7d3fff4ac41b 242 float32_t direction_angle_degrees = direction_angle_radians * (180 / PI);
bmazzeo 28:fe7747b89fb3 243
bmazzeo 28:fe7747b89fb3 244 BSP_LCD_SetTextColor(LCD_COLOR_CYAN);
bmazzeo 29:7d3fff4ac41b 245 sprintf(buf, "DOA[deg]:%6.2f", direction_angle_degrees);
bmazzeo 29:7d3fff4ac41b 246 BSP_LCD_DisplayStringAt(0, 150, (uint8_t *) buf, LEFT_MODE);
bmazzeo 28:fe7747b89fb3 247
bmazzeo 29:7d3fff4ac41b 248 /*
bmazzeo 28:fe7747b89fb3 249 sprintf(buf, "PAngle:%6.2f rad", phase_angle);
bmazzeo 28:fe7747b89fb3 250 BSP_LCD_DisplayStringAt(0, 200, (uint8_t *) buf, LEFT_MODE);
bmazzeo 29:7d3fff4ac41b 251
bmazzeo 29:7d3fff4ac41b 252 BSP_LCD_SetTextColor(LCD_COLOR_CYAN);
bmazzeo 29:7d3fff4ac41b 253 sprintf(buf, "Delta_t:%9.6f seconds", delta_t);
bmazzeo 29:7d3fff4ac41b 254 BSP_LCD_DisplayStringAt(0, 225, (uint8_t *) buf, LEFT_MODE);
bmazzeo 29:7d3fff4ac41b 255 */
bmazzeo 28:fe7747b89fb3 256
bmazzeo 23:d938f87dd1ee 257 L_chan_mem_address = L_channel;
bmazzeo 23:d938f87dd1ee 258 R_chan_mem_address = R_channel;
bmazzeo 23:d938f87dd1ee 259 for (i=0; i<Signal_Length; i++)
bmazzeo 23:d938f87dd1ee 260 {
bmazzeo 24:9ac1187f9012 261 //L_audio_value = *L_chan_mem_address;
bmazzeo 28:fe7747b89fb3 262 L_audio_value = *L_chan_mem_address;
bmazzeo 24:9ac1187f9012 263 *L_chan_mem_address = L_audio_value;
bmazzeo 23:d938f87dd1ee 264 L_chan_mem_address++;
bmazzeo 23:d938f87dd1ee 265
bmazzeo 23:d938f87dd1ee 266 R_audio_value = *R_chan_mem_address;
bmazzeo 24:9ac1187f9012 267 *R_chan_mem_address = R_audio_value;
bmazzeo 23:d938f87dd1ee 268 R_chan_mem_address++;
bmazzeo 23:d938f87dd1ee 269 }
bmazzeo 23:d938f87dd1ee 270
bmazzeo 29:7d3fff4ac41b 271 cycle_count++;
bmazzeo 29:7d3fff4ac41b 272 if (cycle_count == CYCLE_AVERAGE_NUM) {cycle_count = 0;}
bmazzeo 23:d938f87dd1ee 273 }