Nikolay Sergeev / Mbed 2 deprecated fir_f32

Ura

Dependencies:   CMSIS_DSP_401 mbed

main.cpp@8:027ed43684af, 2014-11-27 (annotated)

Committer:
Sergeev
Date:
Thu Nov 27 16:24:23 2014 +0000
Revision:
8:027ed43684af
Parent:
7:a4cff60f035b
Child:
9:748d94f021f6
fghfd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Sergeev 0:b712470aea1d 1 #include "mbed.h"
Sergeev 1:9b1df0b2507d 2 #include <ctype.h>
Sergeev 0:b712470aea1d 3 #include "arm_math.h"
Sergeev 1:9b1df0b2507d 4 #include "arm_const_structs.h"
Sergeev 7:a4cff60f035b 5 #include "stm32f4xx_hal.h"
Sergeev 7:a4cff60f035b 6 #include <stdio.h>
Sergeev 7:a4cff60f035b 7
Sergeev 7:a4cff60f035b 8
Sergeev 7:a4cff60f035b 9 /* SPI handler declaration */
Sergeev 7:a4cff60f035b 10 SPI_HandleTypeDef SpiHandle;
Sergeev 7:a4cff60f035b 11
Sergeev 1:9b1df0b2507d 12
Sergeev 1:9b1df0b2507d 13 void arm_cfft_f32(
Sergeev 1:9b1df0b2507d 14 const arm_cfft_instance_f32 * S,
Sergeev 1:9b1df0b2507d 15 float32_t * p1,
Sergeev 1:9b1df0b2507d 16 uint8_t ifftFlag,
Sergeev 1:9b1df0b2507d 17 uint8_t bitReverseFlag);
Sergeev 0:b712470aea1d 18
Sergeev 5:173ae8477ae1 19 Serial pc(USBTX, USBRX);//:D ПРИВЕТ!!!!
Sergeev 1:9b1df0b2507d 20
Sergeev 1:9b1df0b2507d 21 AnalogIn left(A2);
Sergeev 1:9b1df0b2507d 22 AnalogIn right(A3);
Sergeev 5:173ae8477ae1 23 AnalogIn center(A3);
Sergeev 7:a4cff60f035b 24
Sergeev 7:a4cff60f035b 25 int flag = 0;
Sergeev 1:9b1df0b2507d 26
Sergeev 8:027ed43684af 27 const int SAMPLE_RATE_HZ = 40000; // Sample rate of the audio in hertz.
Sergeev 6:0de27f5ec409 28 const int FFT_SIZE = 16; // Size of the FFT.
Sergeev 1:9b1df0b2507d 29
Sergeev 6:0de27f5ec409 30 const static arm_cfft_instance_f32 *S1;
Sergeev 6:0de27f5ec409 31 const static arm_cfft_instance_f32 *S2;
Sergeev 6:0de27f5ec409 32 const static arm_cfft_instance_f32 *S3;
Sergeev 3:1f56b8f439a1 33 //static arm_cfft_radix2_instance_f32 *S;
Sergeev 1:9b1df0b2507d 34 Ticker samplingTimer;
Sergeev 1:9b1df0b2507d 35 float samples[FFT_SIZE*2];
Sergeev 5:173ae8477ae1 36 float samples2[FFT_SIZE*2];
Sergeev 5:173ae8477ae1 37 float samples3[FFT_SIZE*2];
Sergeev 5:173ae8477ae1 38
Sergeev 1:9b1df0b2507d 39 float magnitudes[FFT_SIZE];
Sergeev 5:173ae8477ae1 40 float magnitudes2[FFT_SIZE];
Sergeev 5:173ae8477ae1 41 float magnitudes3[FFT_SIZE];
Sergeev 1:9b1df0b2507d 42 int sampleCounter = 0;
Sergeev 5:173ae8477ae1 43
Sergeev 8:027ed43684af 44 const int SAMPLE_QUANTITY = 1000000/(16*(1000000/SAMPLE_RATE_HZ));
Sergeev 8:027ed43684af 45
Sergeev 8:027ed43684af 46 float magnitudes_per_second[SAMPLE_QUANTITY];
Sergeev 8:027ed43684af 47 int index_of_magnitudes[SAMPLE_QUANTITY];
Sergeev 8:027ed43684af 48 int index_of_samples;
Sergeev 8:027ed43684af 49
Sergeev 1:9b1df0b2507d 50 void samplingCallback()
Sergeev 3:1f56b8f439a1 51 {
Sergeev 1:9b1df0b2507d 52 // Read from the ADC and store the sample data
Sergeev 3:1f56b8f439a1 53 samples[sampleCounter] = 1000*left.read();
Sergeev 5:173ae8477ae1 54 samples2[sampleCounter] = 1000*left.read();
Sergeev 5:173ae8477ae1 55 samples3[sampleCounter] = 1000*left.read();
Sergeev 1:9b1df0b2507d 56 // Complex FFT functions require a coefficient for the imaginary part of the input.
Sergeev 1:9b1df0b2507d 57 // Since we only have real data, set this coefficient to zero.
Sergeev 1:9b1df0b2507d 58 samples[sampleCounter+1] = 0.0;
Sergeev 5:173ae8477ae1 59 samples2[sampleCounter+1] = 0.0;
Sergeev 5:173ae8477ae1 60 samples3[sampleCounter+1] = 0.0;
Sergeev 1:9b1df0b2507d 61 // Update sample buffer position and stop after the buffer is filled
Sergeev 3:1f56b8f439a1 62
Sergeev 1:9b1df0b2507d 63 sampleCounter += 2;
Sergeev 2:707eb5bc5dfc 64
Sergeev 1:9b1df0b2507d 65 if (sampleCounter >= FFT_SIZE*2) {
Sergeev 1:9b1df0b2507d 66 samplingTimer.detach();
Sergeev 1:9b1df0b2507d 67 }
Sergeev 1:9b1df0b2507d 68 }
Sergeev 1:9b1df0b2507d 69
Sergeev 1:9b1df0b2507d 70 void samplingBegin()
Sergeev 1:9b1df0b2507d 71 {
Sergeev 1:9b1df0b2507d 72 // Reset sample buffer position and start callback at necessary rate.
Sergeev 1:9b1df0b2507d 73 sampleCounter = 0;
Sergeev 3:1f56b8f439a1 74 samplingTimer.attach_us(&samplingCallback, (float)(1000000/SAMPLE_RATE_HZ));
Sergeev 1:9b1df0b2507d 75 }
Sergeev 1:9b1df0b2507d 76
Sergeev 1:9b1df0b2507d 77 bool samplingIsDone()
Sergeev 1:9b1df0b2507d 78 {
Sergeev 1:9b1df0b2507d 79 return sampleCounter >= FFT_SIZE*2;
Sergeev 0:b712470aea1d 80 }
Sergeev 1:9b1df0b2507d 81
Sergeev 1:9b1df0b2507d 82 int main()
Sergeev 0:b712470aea1d 83 {
Sergeev 7:a4cff60f035b 84
Sergeev 7:a4cff60f035b 85 static uint32_t cnt1=0;
Sergeev 7:a4cff60f035b 86
Sergeev 1:9b1df0b2507d 87 // Init arm_ccft_32
Sergeev 1:9b1df0b2507d 88 switch (FFT_SIZE)
Sergeev 1:9b1df0b2507d 89 {
Sergeev 1:9b1df0b2507d 90 case 16:
Sergeev 6:0de27f5ec409 91 S1 = & arm_cfft_sR_f32_len16;
Sergeev 6:0de27f5ec409 92 S2 = & arm_cfft_sR_f32_len16;
Sergeev 6:0de27f5ec409 93 S3 = & arm_cfft_sR_f32_len16;
Sergeev 1:9b1df0b2507d 94 break;
Sergeev 1:9b1df0b2507d 95 case 32:
Sergeev 6:0de27f5ec409 96 S1 = & arm_cfft_sR_f32_len32;
Sergeev 6:0de27f5ec409 97 S2 = & arm_cfft_sR_f32_len32;
Sergeev 6:0de27f5ec409 98 S3 = & arm_cfft_sR_f32_len32;
Sergeev 1:9b1df0b2507d 99 break;
Sergeev 1:9b1df0b2507d 100 case 64:
Sergeev 6:0de27f5ec409 101 S1 = & arm_cfft_sR_f32_len64;
Sergeev 6:0de27f5ec409 102 S2 = & arm_cfft_sR_f32_len64;
Sergeev 6:0de27f5ec409 103 S3 = & arm_cfft_sR_f32_len64;
Sergeev 1:9b1df0b2507d 104 break;
Sergeev 1:9b1df0b2507d 105 case 128:
Sergeev 6:0de27f5ec409 106 S1 = & arm_cfft_sR_f32_len128;
Sergeev 6:0de27f5ec409 107 S2 = & arm_cfft_sR_f32_len128;
Sergeev 6:0de27f5ec409 108 S3 = & arm_cfft_sR_f32_len128;
Sergeev 1:9b1df0b2507d 109 break;
Sergeev 1:9b1df0b2507d 110 case 256:
Sergeev 6:0de27f5ec409 111 S1 = & arm_cfft_sR_f32_len256;
Sergeev 6:0de27f5ec409 112 S2 = & arm_cfft_sR_f32_len256;
Sergeev 6:0de27f5ec409 113 S3 = & arm_cfft_sR_f32_len256;
Sergeev 1:9b1df0b2507d 114 break;
Sergeev 1:9b1df0b2507d 115 case 512:
Sergeev 6:0de27f5ec409 116 S1 = & arm_cfft_sR_f32_len512;
Sergeev 6:0de27f5ec409 117 S2 = & arm_cfft_sR_f32_len512;
Sergeev 6:0de27f5ec409 118 S3 = & arm_cfft_sR_f32_len512;
Sergeev 1:9b1df0b2507d 119 break;
Sergeev 1:9b1df0b2507d 120 case 1024:
Sergeev 6:0de27f5ec409 121 S1 = & arm_cfft_sR_f32_len1024;
Sergeev 6:0de27f5ec409 122 S2 = & arm_cfft_sR_f32_len1024;
Sergeev 6:0de27f5ec409 123 S3 = & arm_cfft_sR_f32_len1024;
Sergeev 1:9b1df0b2507d 124 break;
Sergeev 1:9b1df0b2507d 125 case 2048:
Sergeev 6:0de27f5ec409 126 S1 = & arm_cfft_sR_f32_len2048;
Sergeev 6:0de27f5ec409 127 S2 = & arm_cfft_sR_f32_len2048;
Sergeev 6:0de27f5ec409 128 S3 = & arm_cfft_sR_f32_len2048;
Sergeev 1:9b1df0b2507d 129 break;
Sergeev 1:9b1df0b2507d 130 case 4096:
Sergeev 6:0de27f5ec409 131 S1 = & arm_cfft_sR_f32_len4096;
Sergeev 6:0de27f5ec409 132 S2 = & arm_cfft_sR_f32_len4096;
Sergeev 6:0de27f5ec409 133 S3 = & arm_cfft_sR_f32_len4096;
Sergeev 1:9b1df0b2507d 134 break;
Sergeev 1:9b1df0b2507d 135 }
Sergeev 1:9b1df0b2507d 136 float maxValue = 0.0f;
Sergeev 5:173ae8477ae1 137 float maxValue2 = 0.0f;
Sergeev 5:173ae8477ae1 138 float maxValue3 = 0.0f;
Sergeev 5:173ae8477ae1 139
Sergeev 1:9b1df0b2507d 140 unsigned int testIndex = 0;
Sergeev 5:173ae8477ae1 141 unsigned int testIndex2 = 0;
Sergeev 5:173ae8477ae1 142 unsigned int testIndex3 = 0;
Sergeev 1:9b1df0b2507d 143
Sergeev 1:9b1df0b2507d 144 // Begin sampling audio
Sergeev 1:9b1df0b2507d 145 samplingBegin();
Sergeev 1:9b1df0b2507d 146
Sergeev 1:9b1df0b2507d 147 while(1)
Sergeev 1:9b1df0b2507d 148 // Calculate FFT if a full sample is available.
Sergeev 1:9b1df0b2507d 149 if (samplingIsDone())
Sergeev 3:1f56b8f439a1 150 {
Sergeev 1:9b1df0b2507d 151 // Run FFT on sample data.
Sergeev 6:0de27f5ec409 152 arm_cfft_f32(S1, samples, 0, 1);
Sergeev 6:0de27f5ec409 153 arm_cfft_f32(S2, samples2, 0, 1);
Sergeev 6:0de27f5ec409 154 arm_cfft_f32(S3, samples3, 0, 1);
Sergeev 7:a4cff60f035b 155 samples[0]=0;
Sergeev 7:a4cff60f035b 156 samples2[0]=0;
Sergeev 7:a4cff60f035b 157 samples3[0]=0;
Sergeev 7:a4cff60f035b 158
Sergeev 1:9b1df0b2507d 159 arm_cmplx_mag_f32(samples, magnitudes, FFT_SIZE);
Sergeev 5:173ae8477ae1 160 arm_cmplx_mag_f32(samples2, magnitudes2, FFT_SIZE);
Sergeev 5:173ae8477ae1 161 arm_cmplx_mag_f32(samples3, magnitudes3, FFT_SIZE);
Sergeev 5:173ae8477ae1 162
Sergeev 1:9b1df0b2507d 163 arm_max_f32(magnitudes, FFT_SIZE, &maxValue, &testIndex);
Sergeev 5:173ae8477ae1 164 arm_max_f32(magnitudes2, FFT_SIZE, &maxValue2, &testIndex2);
Sergeev 5:173ae8477ae1 165 arm_max_f32(magnitudes3, FFT_SIZE, &maxValue3, &testIndex3);
Sergeev 7:a4cff60f035b 166
Sergeev 8:027ed43684af 167
Sergeev 8:027ed43684af 168
Sergeev 7:a4cff60f035b 169 {
Sergeev 7:a4cff60f035b 170 if (HAL_GetTick() > (cnt1 + 1000))
Sergeev 7:a4cff60f035b 171 {
Sergeev 7:a4cff60f035b 172 cnt1=HAL_GetTick();
Sergeev 7:a4cff60f035b 173 if (flag==0){
Sergeev 7:a4cff60f035b 174 pc.printf(" MAX value at magnitudes 1[%d] : %+8.2f\r\n", testIndex, maxValue);//Я НЯШКА А АНЯ ВЛАСЮК КОЗА:DDDDDDDDDDDDD
Sergeev 7:a4cff60f035b 175 pc.printf(" MAX value at magnitudes 2[%d] : %+8.2f\r\n", testIndex2, maxValue2);
Sergeev 7:a4cff60f035b 176 pc.printf(" MAX value at magnitudes 3[%d] : %+8.2f\r\n", testIndex2, maxValue3);
Sergeev 7:a4cff60f035b 177 flag++;
Sergeev 7:a4cff60f035b 178 }
Sergeev 7:a4cff60f035b 179 else if (flag==1){
Sergeev 7:a4cff60f035b 180
Sergeev 7:a4cff60f035b 181 flag++;
Sergeev 7:a4cff60f035b 182 }
Sergeev 7:a4cff60f035b 183 else if (flag==2){
Sergeev 7:a4cff60f035b 184
Sergeev 7:a4cff60f035b 185 flag=0;
Sergeev 7:a4cff60f035b 186 }
Sergeev 7:a4cff60f035b 187 }
Sergeev 7:a4cff60f035b 188
Sergeev 5:173ae8477ae1 189
Sergeev 5:173ae8477ae1 190 double delta_t = 0;
Sergeev 5:173ae8477ae1 191 if ((testIndex < testIndex2) and (testIndex < testIndex3) and (testIndex2 < testIndex3))
Sergeev 5:173ae8477ae1 192 delta_t = (testIndex3 - testIndex)*0.000025;
Sergeev 5:173ae8477ae1 193 else if ((testIndex < testIndex2)and(testIndex < testIndex3)and(testIndex2 > testIndex3))
Sergeev 5:173ae8477ae1 194 delta_t = (testIndex2 - testIndex)*0.000025;
Sergeev 5:173ae8477ae1 195 else if ((testIndex2 < testIndex3)and(testIndex2 < testIndex)and(testIndex < testIndex3))
Sergeev 5:173ae8477ae1 196 delta_t = (testIndex3 - testIndex2)*0.000025;
Sergeev 5:173ae8477ae1 197 else if ((testIndex2 < testIndex3)and(testIndex2 < testIndex)and(testIndex > testIndex3))
Sergeev 5:173ae8477ae1 198 delta_t = (testIndex - testIndex3)*0.000025;
Sergeev 5:173ae8477ae1 199 else if (testIndex2 < testIndex)
Sergeev 5:173ae8477ae1 200 delta_t = (testIndex - testIndex3)*0.000025;
Sergeev 5:173ae8477ae1 201 else
Sergeev 5:173ae8477ae1 202 delta_t = (testIndex - testIndex2)*0.000025;
Sergeev 5:173ae8477ae1 203 double fi = asin(330*delta_t/0.33);
Sergeev 7:a4cff60f035b 204
Sergeev 8:027ed43684af 205
Sergeev 8:027ed43684af 206 magnitudes_per_second[index_of_samples]=maxValue;
Sergeev 8:027ed43684af 207 index_of_magnitudes[testIndex]=testIndex;
Sergeev 5:173ae8477ae1 208
Sergeev 7:a4cff60f035b 209 if (HAL_GetTick() > (cnt1 + 1000))
Sergeev 7:a4cff60f035b 210 {
Sergeev 7:a4cff60f035b 211 cnt1=HAL_GetTick();
Sergeev 7:a4cff60f035b 212 if (flag==0){
Sergeev 7:a4cff60f035b 213 pc.printf(" MAX value at magnitudes 1[%d] : %+8.2f\r\n", testIndex, maxValue);//Я НЯШКА А АНЯ ВЛАСЮК КОЗА:DDDDDDDDDDDDD
Sergeev 7:a4cff60f035b 214 pc.printf(" MAX value at magnitudes 2[%d] : %+8.2f\r\n", testIndex2, maxValue2);
Sergeev 7:a4cff60f035b 215 pc.printf(" MAX value at magnitudes 3[%d] : %+8.2f\r\n", testIndex2, maxValue3);
Sergeev 7:a4cff60f035b 216 pc.printf(" Angle is : %+8.2f\r\n", fi);
Sergeev 8:027ed43684af 217 index_of_samples = 0;
Sergeev 7:a4cff60f035b 218 flag++;
Sergeev 7:a4cff60f035b 219 }
Sergeev 7:a4cff60f035b 220 else if (flag==1){
Sergeev 7:a4cff60f035b 221
Sergeev 7:a4cff60f035b 222 flag++;
Sergeev 7:a4cff60f035b 223 }
Sergeev 7:a4cff60f035b 224 else if (flag==2){
Sergeev 7:a4cff60f035b 225
Sergeev 7:a4cff60f035b 226 flag=0;
Sergeev 7:a4cff60f035b 227 }
Sergeev 7:a4cff60f035b 228 }
Sergeev 8:027ed43684af 229 else
Sergeev 8:027ed43684af 230 index_of_samples++;
Sergeev 7:a4cff60f035b 231
Sergeev 7:a4cff60f035b 232
Sergeev 1:9b1df0b2507d 233 samplingBegin();
Sergeev 1:9b1df0b2507d 234 }
Sergeev 1:9b1df0b2507d 235 }
Sergeev 6:0de27f5ec409 236 }