R&J / Mbed OS 487_Laboratory_1

Dependencies:   CMSIS-DSP_for_STM32F746G BSP_DISCO_F746NG

signal_processing.cpp@30:debea332cdfe, 2020-01-03 (annotated)

Committer:
bmazzeo
Date:
Fri Jan 03 20:15:03 2020 +0000
Revision:
30:debea332cdfe
Parent:
29:7d3fff4ac41b
Child:
31:5a0235b66851
Code with more extensive commenting. Set to 2 kHz carrier. Working well.

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 30:debea332cdfe 34 #define Omega_C (2*PI*2000) // [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 30:debea332cdfe 39 ** Fpass = 10
bmazzeo 30:debea332cdfe 40 ** Fstop = 100
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 30:debea332cdfe 47 0.007104042452,0.0006973274867,0.0007323734462,0.0007667868049,0.0008037738735,
bmazzeo 30:debea332cdfe 48 0.0008398960927,0.0008782913792,0.0009155147709,0.0009549160604,0.0009931699606,
bmazzeo 30:debea332cdfe 49 0.001033760724, 0.001073651598, 0.001116432832, 0.001159087871, 0.001204611151,
bmazzeo 30:debea332cdfe 50 0.001248990768, 0.001294256072, 0.001336314948, 0.001380768954, 0.00143639138,
bmazzeo 30:debea332cdfe 51 0.001479929546, 0.001529163332, 0.001579547534, 0.001628990169, 0.001680716057,
bmazzeo 30:debea332cdfe 52 0.001731572906, 0.001784725231, 0.001837128773, 0.001891752472, 0.001945362659,
bmazzeo 30:debea332cdfe 53 0.002000829903, 0.002055054298, 0.002111451235, 0.002167258877, 0.002225656994,
bmazzeo 30:debea332cdfe 54 0.00228298991, 0.002341142856, 0.002397627104, 0.002459470183, 0.002516909502,
bmazzeo 30:debea332cdfe 55 0.002577944659, 0.002637568163, 0.002699105535, 0.002759578172, 0.002821756992,
bmazzeo 30:debea332cdfe 56 0.002882712055, 0.002945428248, 0.003007095773, 0.003070619656, 0.00313297566,
bmazzeo 30:debea332cdfe 57 0.003196678124, 0.003258993849, 0.003322744276, 0.003385637654, 0.003450219054,
bmazzeo 30:debea332cdfe 58 0.003513017204, 0.003576596035, 0.003640421201, 0.003704034258, 0.003767020069,
bmazzeo 30:debea332cdfe 59 0.003830909962, 0.00389353768, 0.00395733444, 0.004019823391, 0.004083196633,
bmazzeo 30:debea332cdfe 60 0.004145130981, 0.004207855556, 0.004269362427, 0.004331791773, 0.004392801784,
bmazzeo 30:debea332cdfe 61 0.004454457201, 0.004514472093, 0.004575279541, 0.004634896293, 0.004694812,
bmazzeo 30:debea332cdfe 62 0.004752826411, 0.004812198691, 0.004869329743, 0.004927231465, 0.004983349238,
bmazzeo 30:debea332cdfe 63 0.005039713811, 0.00509452261, 0.005149625242, 0.005203116219, 0.005256726872,
bmazzeo 30:debea332cdfe 64 0.005308494903, 0.005360371433, 0.005410598125, 0.005460814573, 0.00550937932,
bmazzeo 30:debea332cdfe 65 0.0055576046, 0.005604189355, 0.005650800653, 0.005695586558, 0.005739701446,
bmazzeo 30:debea332cdfe 66 0.005782630295, 0.005824764259, 0.00586552918, 0.005905705504, 0.005944130477,
bmazzeo 30:debea332cdfe 67 0.005981951021, 0.00601811707, 0.006053663325, 0.00608762214, 0.006120662671,
bmazzeo 30:debea332cdfe 68 0.006152128801, 0.006182731595, 0.006211698055, 0.00623987196, 0.006266170647,
bmazzeo 30:debea332cdfe 69 0.006291479338, 0.006315360311, 0.006338228006, 0.006359170191, 0.006379307248,
bmazzeo 30:debea332cdfe 70 0.00639748713, 0.006414768752, 0.006430387963, 0.00644480763, 0.006457572803,
bmazzeo 30:debea332cdfe 71 0.006469102111, 0.006479041651, 0.006487841718, 0.006494853646, 0.006500702817,
bmazzeo 30:debea332cdfe 72 0.00650490867, 0.00650779251, 0.006509374361, 0.006509374361, 0.00650779251,
bmazzeo 30:debea332cdfe 73 0.00650490867, 0.006500702817, 0.006494853646, 0.006487841718, 0.006479041651,
bmazzeo 30:debea332cdfe 74 0.006469102111, 0.006457572803, 0.00644480763, 0.006430387963, 0.006414768752,
bmazzeo 30:debea332cdfe 75 0.00639748713, 0.006379307248, 0.006359170191, 0.006338228006, 0.006315360311,
bmazzeo 30:debea332cdfe 76 0.006291479338, 0.006266170647, 0.00623987196, 0.006211698055, 0.006182731595,
bmazzeo 30:debea332cdfe 77 0.006152128801, 0.006120662671, 0.00608762214, 0.006053663325, 0.00601811707,
bmazzeo 30:debea332cdfe 78 0.005981951021, 0.005944130477, 0.005905705504, 0.00586552918, 0.005824764259,
bmazzeo 30:debea332cdfe 79 0.005782630295, 0.005739701446, 0.005695586558, 0.005650800653, 0.005604189355,
bmazzeo 30:debea332cdfe 80 0.0055576046, 0.00550937932, 0.005460814573, 0.005410598125, 0.005360371433,
bmazzeo 30:debea332cdfe 81 0.005308494903, 0.005256726872, 0.005203116219, 0.005149625242, 0.00509452261,
bmazzeo 30:debea332cdfe 82 0.005039713811, 0.004983349238, 0.004927231465, 0.004869329743, 0.004812198691,
bmazzeo 30:debea332cdfe 83 0.004752826411, 0.004694812, 0.004634896293, 0.004575279541, 0.004514472093,
bmazzeo 30:debea332cdfe 84 0.004454457201, 0.004392801784, 0.004331791773, 0.004269362427, 0.004207855556,
bmazzeo 30:debea332cdfe 85 0.004145130981, 0.004083196633, 0.004019823391, 0.00395733444, 0.00389353768,
bmazzeo 30:debea332cdfe 86 0.003830909962, 0.003767020069, 0.003704034258, 0.003640421201, 0.003576596035,
bmazzeo 30:debea332cdfe 87 0.003513017204, 0.003450219054, 0.003385637654, 0.003322744276, 0.003258993849,
bmazzeo 30:debea332cdfe 88 0.003196678124, 0.00313297566, 0.003070619656, 0.003007095773, 0.002945428248,
bmazzeo 30:debea332cdfe 89 0.002882712055, 0.002821756992, 0.002759578172, 0.002699105535, 0.002637568163,
bmazzeo 30:debea332cdfe 90 0.002577944659, 0.002516909502, 0.002459470183, 0.002397627104, 0.002341142856,
bmazzeo 30:debea332cdfe 91 0.00228298991, 0.002225656994, 0.002167258877, 0.002111451235, 0.002055054298,
bmazzeo 30:debea332cdfe 92 0.002000829903, 0.001945362659, 0.001891752472, 0.001837128773, 0.001784725231,
bmazzeo 30:debea332cdfe 93 0.001731572906, 0.001680716057, 0.001628990169, 0.001579547534, 0.001529163332,
bmazzeo 30:debea332cdfe 94 0.001479929546, 0.00143639138, 0.001380768954, 0.001336314948, 0.001294256072,
bmazzeo 30:debea332cdfe 95 0.001248990768, 0.001204611151, 0.001159087871, 0.001116432832, 0.001073651598,
bmazzeo 30:debea332cdfe 96 0.001033760724,0.0009931699606,0.0009549160604,0.0009155147709,0.0008782913792,
bmazzeo 30:debea332cdfe 97 0.0008398960927,0.0008037738735,0.0007667868049,0.0007323734462,0.0006973274867,
bmazzeo 30:debea332cdfe 98 0.007104042452
bmazzeo 25:5412779376a7 99 };
bmazzeo 25:5412779376a7 100
bmazzeo 24:9ac1187f9012 101
bmazzeo 30:debea332cdfe 102
bmazzeo 24:9ac1187f9012 103 /* -------------------------------------------------------------------
bmazzeo 24:9ac1187f9012 104 * Declare State buffer of size (numTaps + blockSize - 1)
bmazzeo 24:9ac1187f9012 105 * ------------------------------------------------------------------- */
bmazzeo 24:9ac1187f9012 106
bmazzeo 28:fe7747b89fb3 107 static float32_t S_0i_FIRstate[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
bmazzeo 28:fe7747b89fb3 108 static float32_t S_0q_FIRstate[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
bmazzeo 28:fe7747b89fb3 109 static float32_t S_1i_FIRstate[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
bmazzeo 28:fe7747b89fb3 110 static float32_t S_1q_FIRstate[AUDIO_BLOCK_SAMPLES + NUM_TAPS - 1];
bmazzeo 28:fe7747b89fb3 111
bmazzeo 28:fe7747b89fb3 112 /* Need to have multiplies pre-computed */
bmazzeo 28:fe7747b89fb3 113 static float32_t i_phase_mult_array[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 114 static float32_t q_phase_mult_array[AUDIO_BLOCK_SAMPLES];
bmazzeo 24:9ac1187f9012 115
bmazzeo 28:fe7747b89fb3 116 static float32_t x_0i[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 117 static float32_t x_0q[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 118 static float32_t x_1i[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 119 static float32_t x_1q[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 120
bmazzeo 28:fe7747b89fb3 121 static float32_t s_0i[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 122 static float32_t s_0q[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 123 static float32_t s_1i[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 124 static float32_t s_1q[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 125
bmazzeo 28:fe7747b89fb3 126 static float32_t s0_s1star_real[AUDIO_BLOCK_SAMPLES];
bmazzeo 28:fe7747b89fb3 127 static float32_t s0_s1star_imag[AUDIO_BLOCK_SAMPLES];
bmazzeo 24:9ac1187f9012 128
bmazzeo 29:7d3fff4ac41b 129 static float32_t cycle_results[CYCLE_AVERAGE_NUM];
bmazzeo 29:7d3fff4ac41b 130 uint32_t cycle_count;
bmazzeo 24:9ac1187f9012 131
bmazzeo 25:5412779376a7 132 /* Important to have the structure outside of the execution so it can be initialized */
bmazzeo 28:fe7747b89fb3 133 arm_fir_instance_f32 S_0i;
bmazzeo 28:fe7747b89fb3 134 arm_fir_instance_f32 S_0q;
bmazzeo 28:fe7747b89fb3 135 arm_fir_instance_f32 S_1i;
bmazzeo 28:fe7747b89fb3 136 arm_fir_instance_f32 S_1q;
bmazzeo 23:d938f87dd1ee 137
bmazzeo 29:7d3fff4ac41b 138
bmazzeo 29:7d3fff4ac41b 139 /* FUNCTION DEFINITIONS BELOW */
bmazzeo 29:7d3fff4ac41b 140
bmazzeo 25:5412779376a7 141 /**
bmazzeo 25:5412779376a7 142 * @brief Initialize filter structures to be used in loops later
bmazzeo 25:5412779376a7 143 * @retval None
bmazzeo 25:5412779376a7 144 */
bmazzeo 25:5412779376a7 145 void initalize_signal_processing(void) {
bmazzeo 25:5412779376a7 146
bmazzeo 25:5412779376a7 147 /* Call FIR init function to initialize the instance structure. */
bmazzeo 28:fe7747b89fb3 148 //arm_fir_init_f32(&S, NUM_TAPS, (float32_t *)&firCoeffs32[0], &firStateF32[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 28:fe7747b89fb3 149 arm_fir_init_f32(&S_0i, NUM_TAPS, (float32_t *) &LPF_coeffs[0], &S_0i_FIRstate[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 28:fe7747b89fb3 150 arm_fir_init_f32(&S_0q, NUM_TAPS, (float32_t *) &LPF_coeffs[0], &S_0q_FIRstate[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 28:fe7747b89fb3 151 arm_fir_init_f32(&S_1i, NUM_TAPS, (float32_t *) &LPF_coeffs[0], &S_1i_FIRstate[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 28:fe7747b89fb3 152 arm_fir_init_f32(&S_1q, NUM_TAPS, (float32_t *) &LPF_coeffs[0], &S_1q_FIRstate[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 25:5412779376a7 153
bmazzeo 28:fe7747b89fb3 154 uint32_t i;
bmazzeo 28:fe7747b89fb3 155 for (i=0; i<AUDIO_BLOCK_SAMPLES; i++)
bmazzeo 28:fe7747b89fb3 156 {
bmazzeo 28:fe7747b89fb3 157 i_phase_mult_array[i] = cos(omega_c * i);
bmazzeo 28:fe7747b89fb3 158 q_phase_mult_array[i] = -sin(omega_c * i);
bmazzeo 28:fe7747b89fb3 159 }
bmazzeo 29:7d3fff4ac41b 160
bmazzeo 29:7d3fff4ac41b 161 for (i=0; i<CYCLE_AVERAGE_NUM; i++)
bmazzeo 29:7d3fff4ac41b 162 {
bmazzeo 29:7d3fff4ac41b 163 cycle_results[i] = 0;
bmazzeo 29:7d3fff4ac41b 164 }
bmazzeo 29:7d3fff4ac41b 165 cycle_count = 0;
bmazzeo 29:7d3fff4ac41b 166
bmazzeo 25:5412779376a7 167 }
bmazzeo 23:d938f87dd1ee 168
bmazzeo 23:d938f87dd1ee 169 /**
bmazzeo 23:d938f87dd1ee 170 * @brief Process audio channel signals
bmazzeo 23:d938f87dd1ee 171 * @param L_channel: Pointer to Left channel data (float)
bmazzeo 23:d938f87dd1ee 172 * @param R_channel: Pointer to Right channel data (float)
bmazzeo 23:d938f87dd1ee 173 * @param Signal_Length: length of data to process
bmazzeo 23:d938f87dd1ee 174 * @retval None
bmazzeo 23:d938f87dd1ee 175 */
bmazzeo 23:d938f87dd1ee 176 void process_audio_channel_signals(float* L_channel, float* R_channel, uint16_t Signal_Length)
bmazzeo 23:d938f87dd1ee 177 {
bmazzeo 28:fe7747b89fb3 178 char buf[40];
bmazzeo 29:7d3fff4ac41b 179 /*
bmazzeo 23:d938f87dd1ee 180 BSP_LCD_SetTextColor(LCD_COLOR_CYAN);
bmazzeo 23:d938f87dd1ee 181 sprintf(buf, "Processing Signals" );
bmazzeo 23:d938f87dd1ee 182 BSP_LCD_DisplayStringAt(0, 150, (uint8_t *) buf, LEFT_MODE);
bmazzeo 29:7d3fff4ac41b 183 */
bmazzeo 28:fe7747b89fb3 184
bmazzeo 28:fe7747b89fb3 185 uint32_t i;
bmazzeo 24:9ac1187f9012 186
bmazzeo 28:fe7747b89fb3 187 /* Need these addresses */
bmazzeo 28:fe7747b89fb3 188 float32_t* L_chan_mem_address = L_channel;
bmazzeo 28:fe7747b89fb3 189 float32_t* R_chan_mem_address = R_channel;
bmazzeo 24:9ac1187f9012 190 float32_t L_audio_value;
bmazzeo 24:9ac1187f9012 191 float32_t R_audio_value;
bmazzeo 28:fe7747b89fb3 192
bmazzeo 28:fe7747b89fb3 193 for (i=0; i<Signal_Length; i++)
bmazzeo 28:fe7747b89fb3 194 {
bmazzeo 28:fe7747b89fb3 195 L_audio_value = *L_chan_mem_address;
bmazzeo 28:fe7747b89fb3 196 x_0i[i] = L_audio_value * i_phase_mult_array[i];
bmazzeo 28:fe7747b89fb3 197 x_0q[i] = L_audio_value * q_phase_mult_array[i];
bmazzeo 23:d938f87dd1ee 198
bmazzeo 28:fe7747b89fb3 199 R_audio_value = *R_chan_mem_address;
bmazzeo 28:fe7747b89fb3 200 x_1i[i] = R_audio_value * i_phase_mult_array[i];
bmazzeo 28:fe7747b89fb3 201 x_1q[i] = R_audio_value * q_phase_mult_array[i];
bmazzeo 28:fe7747b89fb3 202
bmazzeo 28:fe7747b89fb3 203 L_chan_mem_address++;
bmazzeo 28:fe7747b89fb3 204 R_chan_mem_address++;
bmazzeo 28:fe7747b89fb3 205 }
bmazzeo 28:fe7747b89fb3 206
bmazzeo 28:fe7747b89fb3 207 /* Calls the FIR filters and proceeses the audio block */
bmazzeo 28:fe7747b89fb3 208 arm_fir_f32(&S_0i, &x_0i[0], &s_0i[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 28:fe7747b89fb3 209 arm_fir_f32(&S_0q, &x_0q[0], &s_0q[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 28:fe7747b89fb3 210 arm_fir_f32(&S_1i, &x_1i[0], &s_1i[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 28:fe7747b89fb3 211 arm_fir_f32(&S_1q, &x_1q[0], &s_1q[0], AUDIO_BLOCK_SAMPLES);
bmazzeo 28:fe7747b89fb3 212
bmazzeo 28:fe7747b89fb3 213
bmazzeo 28:fe7747b89fb3 214 float32_t s0_s1star_sum_real = 0;
bmazzeo 28:fe7747b89fb3 215 float32_t s0_s1star_sum_imag = 0;
bmazzeo 28:fe7747b89fb3 216
bmazzeo 28:fe7747b89fb3 217 /* Complex Multiply and Summation*/
bmazzeo 28:fe7747b89fb3 218 for (i=0; i<Signal_Length; i++)
bmazzeo 28:fe7747b89fb3 219 {
bmazzeo 28:fe7747b89fb3 220 s0_s1star_real[i] = s_0i[i] * s_1i[i] + s_0q[i] * s_1q[i];
bmazzeo 28:fe7747b89fb3 221 s0_s1star_imag[i] = s_0q[i] * s_1i[i] - s_0i[i] * s_1q[i];
bmazzeo 28:fe7747b89fb3 222
bmazzeo 28:fe7747b89fb3 223 s0_s1star_sum_real += s0_s1star_real[i];
bmazzeo 28:fe7747b89fb3 224 s0_s1star_sum_imag += s0_s1star_imag[i];
bmazzeo 28:fe7747b89fb3 225
bmazzeo 28:fe7747b89fb3 226 }
bmazzeo 28:fe7747b89fb3 227
bmazzeo 29:7d3fff4ac41b 228 /* Compute phase angle */
bmazzeo 29:7d3fff4ac41b 229 float32_t phase_angle = atan2(s0_s1star_sum_imag, s0_s1star_sum_real);
bmazzeo 29:7d3fff4ac41b 230
bmazzeo 29:7d3fff4ac41b 231 /* The following computes a running average - to reduce noise in the phase */
bmazzeo 29:7d3fff4ac41b 232 cycle_results[cycle_count] = phase_angle;
bmazzeo 29:7d3fff4ac41b 233 float32_t current_sum = 0;
bmazzeo 29:7d3fff4ac41b 234 for (i=0; i<CYCLE_AVERAGE_NUM; i++)
bmazzeo 29:7d3fff4ac41b 235 {
bmazzeo 29:7d3fff4ac41b 236 current_sum += cycle_results[i];
bmazzeo 29:7d3fff4ac41b 237 }
bmazzeo 29:7d3fff4ac41b 238 phase_angle = current_sum / CYCLE_AVERAGE_NUM;
bmazzeo 29:7d3fff4ac41b 239
bmazzeo 29:7d3fff4ac41b 240 /* Computes the actual DOA angle */
bmazzeo 29:7d3fff4ac41b 241 float32_t delta_t = phase_angle / Omega_C;
bmazzeo 29:7d3fff4ac41b 242 float32_t direction_angle_radians = asin(c_sound * delta_t / mic_distance);
bmazzeo 29:7d3fff4ac41b 243 float32_t direction_angle_degrees = direction_angle_radians * (180 / PI);
bmazzeo 28:fe7747b89fb3 244
bmazzeo 28:fe7747b89fb3 245 BSP_LCD_SetTextColor(LCD_COLOR_CYAN);
bmazzeo 29:7d3fff4ac41b 246 sprintf(buf, "DOA[deg]:%6.2f", direction_angle_degrees);
bmazzeo 29:7d3fff4ac41b 247 BSP_LCD_DisplayStringAt(0, 150, (uint8_t *) buf, LEFT_MODE);
bmazzeo 28:fe7747b89fb3 248
bmazzeo 29:7d3fff4ac41b 249 /*
bmazzeo 28:fe7747b89fb3 250 sprintf(buf, "PAngle:%6.2f rad", phase_angle);
bmazzeo 28:fe7747b89fb3 251 BSP_LCD_DisplayStringAt(0, 200, (uint8_t *) buf, LEFT_MODE);
bmazzeo 29:7d3fff4ac41b 252
bmazzeo 29:7d3fff4ac41b 253 BSP_LCD_SetTextColor(LCD_COLOR_CYAN);
bmazzeo 29:7d3fff4ac41b 254 sprintf(buf, "Delta_t:%9.6f seconds", delta_t);
bmazzeo 29:7d3fff4ac41b 255 BSP_LCD_DisplayStringAt(0, 225, (uint8_t *) buf, LEFT_MODE);
bmazzeo 29:7d3fff4ac41b 256 */
bmazzeo 28:fe7747b89fb3 257
bmazzeo 23:d938f87dd1ee 258 L_chan_mem_address = L_channel;
bmazzeo 23:d938f87dd1ee 259 R_chan_mem_address = R_channel;
bmazzeo 23:d938f87dd1ee 260 for (i=0; i<Signal_Length; i++)
bmazzeo 23:d938f87dd1ee 261 {
bmazzeo 24:9ac1187f9012 262 //L_audio_value = *L_chan_mem_address;
bmazzeo 28:fe7747b89fb3 263 L_audio_value = *L_chan_mem_address;
bmazzeo 24:9ac1187f9012 264 *L_chan_mem_address = L_audio_value;
bmazzeo 23:d938f87dd1ee 265 L_chan_mem_address++;
bmazzeo 23:d938f87dd1ee 266
bmazzeo 23:d938f87dd1ee 267 R_audio_value = *R_chan_mem_address;
bmazzeo 24:9ac1187f9012 268 *R_chan_mem_address = R_audio_value;
bmazzeo 23:d938f87dd1ee 269 R_chan_mem_address++;
bmazzeo 23:d938f87dd1ee 270 }
bmazzeo 23:d938f87dd1ee 271
bmazzeo 29:7d3fff4ac41b 272 cycle_count++;
bmazzeo 29:7d3fff4ac41b 273 if (cycle_count == CYCLE_AVERAGE_NUM) {cycle_count = 0;}
bmazzeo 23:d938f87dd1ee 274 }