R&J
lab 1 code
Dependencies: CMSIS-DSP_for_STM32F746G BSP_DISCO_F746NG
Diff: signal_processing.cpp
- Revision:
- 29:7d3fff4ac41b
- Parent:
- 28:fe7747b89fb3
- Child:
- 30:debea332cdfe
--- a/signal_processing.cpp Wed Jan 01 20:47:48 2020 +0000
+++ b/signal_processing.cpp Wed Jan 01 23:20:43 2020 +0000
@@ -25,11 +25,13 @@
#define AUDIO_BLOCK_SAMPLES ((uint32_t)128) // Number of samples (L and R) in audio block (each samples is 16 bits)
#define NUM_TAPS 64
+#define CYCLE_AVERAGE_NUM 50
+
#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_S (2*PI*16000) // [rad Hz] Sample rate
+#define Omega_C (2*PI*500) // [rad Hz] Carrier frequency
#define omega_c 2*PI*Omega_C/Omega_S // Hilbert Transform Frequency
/* ----------------------------------------------------------------------
@@ -42,58 +44,58 @@
** ------------------------------------------------------------------- */
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
+ -0.0007800915628,-0.0001674496889,-0.0001849002729,-0.0002029328898,-0.0002218717564,
+ -0.0002413307957,-0.0002615691628,-0.0002821780508,-0.0003034212859,-0.000324951252,
+ -0.0003471312812,-0.000369623187,-0.000392670423,-0.0004156443756,-0.0004385936772,
+ -0.0004611753393,-0.0004842103226,-0.0005074346554,-0.0005290624686,-0.0005512404023,
+ -0.0005724770599,-0.0005929443287,-0.0006127390661,-0.0006314100465,-0.0006491405657,
+ -0.0006653870223,-0.000680299243,-0.0006933949771,-0.0007049014093,-0.0007143636467,
+ -0.0007218476967,-0.0007267798646,-0.0007293550298,-0.0007292577648,-0.0007265052409,
+ -0.0007203418063,-0.0007115086773,-0.0006989029353,-0.0006829997874,-0.0006631698925,
+ -0.000639601436,-0.0006118300371,-0.0005799498758,-0.0005434799823,-0.0005025339779,
+ -0.0004567068536,-0.0004061603686,-0.0003504161141,-0.0002896062215,-0.0002233447594,
+ -0.0001518396166,-7.459068001e-05,8.225716556e-06,9.678629431e-05,0.0001911890868,
+ 0.0002915640071,0.0003978571913,0.0005103156436,0.0006287195138,0.0007533723838,
+ 0.00088403275, 0.00102098356, 0.001163926558, 0.001313094515, 0.001468220609,
+ 0.00162948342, 0.001796556171, 0.00196959055, 0.002148323692, 0.002332823817,
+ 0.002522682771, 0.002718108008, 0.002918580314, 0.003124272451, 0.003334705485,
+ 0.003549913643, 0.003769469913, 0.003993340768, 0.004221097566, 0.00445264997,
+ 0.004687562119, 0.004925743211, 0.005166693125, 0.005410279613, 0.00565600628,
+ 0.005903759971, 0.006152981427, 0.006403496955, 0.006654826459, 0.006906681694,
+ 0.007158623077, 0.00741035305, 0.007661380339, 0.007911451161, 0.00816002395,
+ 0.008406852372, 0.008651374839, 0.008893367834, 0.009132288396, 0.009367863648,
+ 0.00959957391, 0.00982714165, 0.01005008724, 0.01026809961, 0.01048072334,
+ 0.01068769302, 0.01088850573, 0.01108295284, 0.01127053425, 0.01145104971,
+ 0.01162407082, 0.01178936474, 0.01194655988, 0.01209541038, 0.01223561913,
+ 0.01236695051, 0.01248912979, 0.01260196976, 0.01270521339, 0.01279872097,
+ 0.01288224943, 0.01295570657, 0.01301891916, 0.01307178196, 0.01311419997,
+ 0.01314606518, 0.01316735335, 0.01317800116, 0.01317800116, 0.01316735335,
+ 0.01314606518, 0.01311419997, 0.01307178196, 0.01301891916, 0.01295570657,
+ 0.01288224943, 0.01279872097, 0.01270521339, 0.01260196976, 0.01248912979,
+ 0.01236695051, 0.01223561913, 0.01209541038, 0.01194655988, 0.01178936474,
+ 0.01162407082, 0.01145104971, 0.01127053425, 0.01108295284, 0.01088850573,
+ 0.01068769302, 0.01048072334, 0.01026809961, 0.01005008724, 0.00982714165,
+ 0.00959957391, 0.009367863648, 0.009132288396, 0.008893367834, 0.008651374839,
+ 0.008406852372, 0.00816002395, 0.007911451161, 0.007661380339, 0.00741035305,
+ 0.007158623077, 0.006906681694, 0.006654826459, 0.006403496955, 0.006152981427,
+ 0.005903759971, 0.00565600628, 0.005410279613, 0.005166693125, 0.004925743211,
+ 0.004687562119, 0.00445264997, 0.004221097566, 0.003993340768, 0.003769469913,
+ 0.003549913643, 0.003334705485, 0.003124272451, 0.002918580314, 0.002718108008,
+ 0.002522682771, 0.002332823817, 0.002148323692, 0.00196959055, 0.001796556171,
+ 0.00162948342, 0.001468220609, 0.001313094515, 0.001163926558, 0.00102098356,
+ 0.00088403275,0.0007533723838,0.0006287195138,0.0005103156436,0.0003978571913,
+ 0.0002915640071,0.0001911890868,9.678629431e-05,8.225716556e-06,-7.459068001e-05,
+ -0.0001518396166,-0.0002233447594,-0.0002896062215,-0.0003504161141,-0.0004061603686,
+ -0.0004567068536,-0.0005025339779,-0.0005434799823,-0.0005799498758,-0.0006118300371,
+ -0.000639601436,-0.0006631698925,-0.0006829997874,-0.0006989029353,-0.0007115086773,
+ -0.0007203418063,-0.0007265052409,-0.0007292577648,-0.0007293550298,-0.0007267798646,
+ -0.0007218476967,-0.0007143636467,-0.0007049014093,-0.0006933949771,-0.000680299243,
+ -0.0006653870223,-0.0006491405657,-0.0006314100465,-0.0006127390661,-0.0005929443287,
+ -0.0005724770599,-0.0005512404023,-0.0005290624686,-0.0005074346554,-0.0004842103226,
+ -0.0004611753393,-0.0004385936772,-0.0004156443756,-0.000392670423,-0.000369623187,
+ -0.0003471312812,-0.000324951252,-0.0003034212859,-0.0002821780508,-0.0002615691628,
+ -0.0002413307957,-0.0002218717564,-0.0002029328898,-0.0001849002729,-0.0001674496889,
+ -0.0007800915628
};
@@ -123,6 +125,8 @@
static float32_t s0_s1star_real[AUDIO_BLOCK_SAMPLES];
static float32_t s0_s1star_imag[AUDIO_BLOCK_SAMPLES];
+static float32_t cycle_results[CYCLE_AVERAGE_NUM];
+uint32_t cycle_count;
/* Important to have the structure outside of the execution so it can be initialized */
arm_fir_instance_f32 S_0i;
@@ -130,6 +134,9 @@
arm_fir_instance_f32 S_1i;
arm_fir_instance_f32 S_1q;
+
+/* FUNCTION DEFINITIONS BELOW */
+
/**
* @brief Initialize filter structures to be used in loops later
* @retval None
@@ -149,6 +156,13 @@
i_phase_mult_array[i] = cos(omega_c * i);
q_phase_mult_array[i] = -sin(omega_c * i);
}
+
+ for (i=0; i<CYCLE_AVERAGE_NUM; i++)
+ {
+ cycle_results[i] = 0;
+ }
+ cycle_count = 0;
+
}
/**
@@ -161,9 +175,11 @@
void process_audio_channel_signals(float* L_channel, float* R_channel, uint16_t Signal_Length)
{
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;
@@ -208,17 +224,35 @@
}
- /* 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);
+ /* Compute phase angle */
+ float32_t phase_angle = atan2(s0_s1star_sum_imag, s0_s1star_sum_real);
+
+ /* The following computes a running average - to reduce noise in the phase */
+ cycle_results[cycle_count] = phase_angle;
+ float32_t current_sum = 0;
+ for (i=0; i<CYCLE_AVERAGE_NUM; i++)
+ {
+ current_sum += cycle_results[i];
+ }
+ phase_angle = current_sum / CYCLE_AVERAGE_NUM;
+
+ /* Computes the actual DOA angle */
+ float32_t delta_t = phase_angle / Omega_C;
+ float32_t direction_angle_radians = asin(c_sound * delta_t / mic_distance);
+ 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, "DOA[deg]:%6.2f", direction_angle_degrees);
+ BSP_LCD_DisplayStringAt(0, 150, (uint8_t *) buf, LEFT_MODE);
+ /*
sprintf(buf, "PAngle:%6.2f rad", phase_angle);
BSP_LCD_DisplayStringAt(0, 200, (uint8_t *) buf, LEFT_MODE);
+
+ BSP_LCD_SetTextColor(LCD_COLOR_CYAN);
+ sprintf(buf, "Delta_t:%9.6f seconds", delta_t);
+ BSP_LCD_DisplayStringAt(0, 225, (uint8_t *) buf, LEFT_MODE);
+ */
L_chan_mem_address = L_channel;
R_chan_mem_address = R_channel;
@@ -234,4 +268,6 @@
R_chan_mem_address++;
}
+ cycle_count++;
+ if (cycle_count == CYCLE_AVERAGE_NUM) {cycle_count = 0;}
}