Ura

Dependencies:   CMSIS_DSP_401 mbed

main.cpp

Committer:
Sergeev
Date:
2014-11-29
Revision:
9:748d94f021f6
Parent:
8:027ed43684af

File content as of revision 9:748d94f021f6:

#include "mbed.h"
#include <ctype.h>
#include "arm_math.h"
#include "arm_const_structs.h"
#include "stm32f4xx_hal.h"
#include <stdio.h>
 
 
/* SPI handler declaration */
SPI_HandleTypeDef SpiHandle;//pemennaya dlia hraneniya 
 
void arm_cfft_f32(
   const arm_cfft_instance_f32 * S,
   float32_t * p1,
   uint8_t ifftFlag,
   uint8_t bitReverseFlag);//fynktsiya

Serial pc(USBTX, USBRX);//:D ПРИВЕТ!!!!
 
AnalogIn   left(A2);//microfony
AnalogIn   right(A3);
AnalogIn   center(A3);

int flag = 0;
 
const int SAMPLE_RATE_HZ = 40000;             // Sample rate of the audio in hertz.
const int FFT_SIZE = 16;                // Size of the FFT.
 
const static arm_cfft_instance_f32 *S1;//ykazateli na prametru preobrazovaniya fyrie
const static arm_cfft_instance_f32 *S2;
const static arm_cfft_instance_f32 *S3;
//static arm_cfft_radix2_instance_f32 *S;
Ticker samplingTimer;
float samples[FFT_SIZE*2];//dannue s analog vhodov
float samples2[FFT_SIZE*2];
float samples3[FFT_SIZE*2];

float magnitudes[FFT_SIZE];
float magnitudes2[FFT_SIZE];
float magnitudes3[FFT_SIZE];
int sampleCounter = 0;

const int SAMPLE_QUANTITY = 1000000/(16*(1000000/SAMPLE_RATE_HZ));//dlitelnost sempla po vremeni v milisek

float magnitudes_per_second[SAMPLE_QUANTITY];//sohran magnitydy
int index_of_magnitudes[SAMPLE_QUANTITY];//mesto v masive magnitydu
int index_of_samples;

float magnitudes_per_second2[SAMPLE_QUANTITY];//sohran magnitydy
int index_of_magnitudes2[SAMPLE_QUANTITY];//mesto v masive magnitydu
int index_of_samples2;

float magnitudes_per_second3[SAMPLE_QUANTITY];//sohran magnitydy
int index_of_magnitudes3[SAMPLE_QUANTITY];//mesto v masive magnitydu
int index_of_samples3;
            
void samplingCallback()
{   
    // Read from the ADC and store the sample data
    samples[sampleCounter] = 1000*left.read();
    samples2[sampleCounter] = 1000*left.read();
    samples3[sampleCounter] = 1000*left.read();
    // Complex FFT functions require a coefficient for the imaginary part of the input.
    // Since we only have real data, set this coefficient to zero.
    samples[sampleCounter+1] = 0.0;
    samples2[sampleCounter+1] = 0.0;
    samples3[sampleCounter+1] = 0.0;
    // Update sample buffer position and stop after the buffer is filled
       
    sampleCounter += 2;//pri razbitti dannuh ostavliaem mesto dila mnimoy 4sti
   
    if (sampleCounter >= FFT_SIZE*2) {
        samplingTimer.detach();
    }//obrabotano sobutie taimera
}
 
void samplingBegin()
{
    // Reset sample buffer position and start callback at necessary rate.
    sampleCounter = 0;
    samplingTimer.attach_us(&samplingCallback, (float)(1000000/SAMPLE_RATE_HZ));//na4ala poly4eniya dannuh s microfona po taimery
}
 
bool samplingIsDone()
{
    return sampleCounter >= FFT_SIZE*2;//koli4estvo poly4enuh semplov
}
 
int main()
{
        
    static uint32_t cnt1=0;//s4et4ik vremeni
   
    // Init arm_ccft_32
    switch (FFT_SIZE)//vupolniaem operatsiy pobitovogo i
    {
    case 16:
        S1 = & arm_cfft_sR_f32_len16;
        S2 = & arm_cfft_sR_f32_len16;
        S3 = & arm_cfft_sR_f32_len16;
        break;
    case 32:
        S1 = & arm_cfft_sR_f32_len32;
        S2 = & arm_cfft_sR_f32_len32;
        S3 = & arm_cfft_sR_f32_len32;
        break;
    case 64:
        S1 = & arm_cfft_sR_f32_len64;
        S2 = & arm_cfft_sR_f32_len64;
        S3 = & arm_cfft_sR_f32_len64;
        break;
    case 128:
        S1 = & arm_cfft_sR_f32_len128;
        S2 = & arm_cfft_sR_f32_len128;
        S3 = & arm_cfft_sR_f32_len128;
        break;
    case 256:
        S1 = & arm_cfft_sR_f32_len256;
        S2 = & arm_cfft_sR_f32_len256;
        S3 = & arm_cfft_sR_f32_len256;
        break;
    case 512:
        S1 = & arm_cfft_sR_f32_len512;
        S2 = & arm_cfft_sR_f32_len512;
        S3 = & arm_cfft_sR_f32_len512;
        break;
    case 1024:
        S1 = & arm_cfft_sR_f32_len1024;
        S2 = & arm_cfft_sR_f32_len1024;
        S3 = & arm_cfft_sR_f32_len1024;
        break;
    case 2048:
        S1 = & arm_cfft_sR_f32_len2048;
        S2 = & arm_cfft_sR_f32_len2048;
        S3 = & arm_cfft_sR_f32_len2048;
        break;
    case 4096:
        S1 = & arm_cfft_sR_f32_len4096;
        S2 = & arm_cfft_sR_f32_len4096;
        S3 = & arm_cfft_sR_f32_len4096;
        break;
    }
    float maxValue = 0.0f;
    float maxValue2 = 0.0f;
    float maxValue3 = 0.0f;
   
    unsigned int testIndex = 0;
    unsigned int testIndex2 = 0;
    unsigned int testIndex3 = 0;
 
    // Begin sampling audio
    samplingBegin();
 
    while(1)
        // Calculate FFT if a full sample is available. vu4islenie bustrogo fyrie preobrazovaniya /rez v samples
        if (samplingIsDone())
        {           
            // Run FFT on sample data.
            arm_cfft_f32(S1, samples, 0, 1);
            arm_cfft_f32(S2, samples2, 0, 1);
            arm_cfft_f32(S3, samples3, 0, 1);
            samples[0]=0;
            samples2[0]=0;
            samples3[0]=0;
          
            arm_cmplx_mag_f32(samples, magnitudes, FFT_SIZE);
            arm_cmplx_mag_f32(samples2, magnitudes2, FFT_SIZE);
            arm_cmplx_mag_f32(samples3, magnitudes3, FFT_SIZE);
           
            arm_max_f32(magnitudes, FFT_SIZE, &maxValue, &testIndex);
            arm_max_f32(magnitudes2, FFT_SIZE, &maxValue2, &testIndex2);
            arm_max_f32(magnitudes3, FFT_SIZE, &maxValue3, &testIndex3);
           

           
                {
       if (HAL_GetTick() > (cnt1 + 1000))
        {
            cnt1=HAL_GetTick();
            if (flag==0){
                pc.printf("     MAX value at magnitudes 1[%d] : %+8.2f\r\n", testIndex, maxValue);
                pc.printf("     MAX value at magnitudes 2[%d] : %+8.2f\r\n", testIndex2, maxValue2);
                pc.printf("     MAX value at magnitudes 3[%d] : %+8.2f\r\n", testIndex3, maxValue3);
                flag++;
                }
            else if (flag==1){

                flag++;
                }
            else if (flag==2){

                flag=0;
                }
        }
           
           
            double delta_t = 0;
            if ((testIndex < testIndex2) and (testIndex < testIndex3) and (testIndex2 < testIndex3))
                delta_t = (testIndex3 - testIndex)*0.000025;
            else if ((testIndex < testIndex2)and(testIndex < testIndex3)and(testIndex2 > testIndex3))
                delta_t = (testIndex2 - testIndex)*0.000025;
            else if ((testIndex2 < testIndex3)and(testIndex2 < testIndex)and(testIndex < testIndex3))
                delta_t = (testIndex3 - testIndex2)*0.000025;
            else if ((testIndex2 < testIndex3)and(testIndex2 < testIndex)and(testIndex > testIndex3))
                delta_t = (testIndex - testIndex3)*0.000025;
            else if (testIndex2 < testIndex)
                delta_t = (testIndex - testIndex3)*0.000025;
            else
                delta_t = (testIndex - testIndex2)*0.000025;
            double fi = asin(330*delta_t/0.33);


magnitudes_per_second[index_of_samples]=maxValue;
index_of_magnitudes[testIndex]=testIndex;

magnitudes_per_second2[index_of_samples2]=maxValue2;
index_of_magnitudes2[testIndex2]=testIndex2;

magnitudes_per_second3[index_of_samples3]=maxValue3;
index_of_magnitudes3[testIndex3]=testIndex3;
           
       if (HAL_GetTick() > (cnt1 + 1000))
        {
            cnt1=HAL_GetTick();
            if (flag==0){
                float max_magnitude_per_second = 0;
               
                float max_magnitude_per_second2 = 0;
               
                float max_magnitude_per_second3 = 0;
               
               
                unsigned int index_max_of_mag;
                unsigned int index_max_of_mag2;
                unsigned int index_max_of_mag3;
               
                arm_max_f32(magnitudes_per_second, FFT_SIZE, &max_magnitude_per_second, &index_max_of_mag);
                arm_max_f32(magnitudes_per_second2, FFT_SIZE, &max_magnitude_per_second2, &index_max_of_mag2);
                arm_max_f32(magnitudes, FFT_SIZE, &max_magnitude_per_second3, &index_max_of_mag3);
               
                pc.printf("     MAX value at magnitudes 1[%d] : %+8.2f\r\n", index_max_of_mag, max_magnitude_per_second);//Я НЯШКА А АНЯ ВЛАСЮК КОЗА:DDDDDDDDDDDDD
                pc.printf("     MAX value at magnitudes 2[%d] : %+8.2f\r\n", index_max_of_mag2, max_magnitude_per_second2);
                pc.printf("     MAX value at magnitudes 3[%d] : %+8.2f\r\n", index_max_of_mag3, max_magnitude_per_second3);
                pc.printf("     Angle is  : %+8.2f\r\n", fi);
                index_of_samples = 0;
                flag++;
                }
            else if (flag==1){

                flag++;
                }
            else if (flag==2){

                flag=0;
                }
        }
        else
        index_of_samples++;
        index_of_samples2++;
        index_of_samples3++;


            samplingBegin();
        }
    }
}