Отправлено Yuriy 21 августа 2002 г. 06:28 В ответ на: Подскажите плиз алгоритм детектирования 1000гц в реальном времени? отправлено Эсперыч 20 августа 2002 г. 15:49

//-------------------------------------------------------------------
//      timer1.c
//
//  This is just an example.
//
//  search for one of four frequencies: 256, 600, 1200, 800 Hz
//
//-------------------------------------------------------------------
#define CORR_LENGTH     200
#define CORR_THRESHOLD  65
//-----------------------------------------------------------
// sampling frequency = 9600 Hz
//
//      256         600         1200          800
//  sin,  cos,   sin,  cos,   sin,  cos,   sin,  cos,
//
U8 corrtab_preamble[256] = {
    0x7F, 0xFF,  0x7F, 0xF8,  0x78, 0xE1,  0x7E, 0xF0,
    0xFF, 0xC0,  0x80, 0x07,  0x78, 0xE1,  0x07, 0x3F,
    0xE0, 0x00,  0x7F, 0xF8,  0x78, 0xE1,  0xE0, 0x03,
    0x00, 0x07,  0x80, 0x07,  0x78, 0xE1,  0x7E, 0xF0,
    0x03, 0xFF,  0x7F, 0xF8,  0x78, 0xE1,  0x07, 0x3F,
    0xFF, 0xFE,  0x80, 0x07,  0x78, 0xE1,  0xE0, 0x03,
    0xFF, 0x00,  0x7F, 0xF8,  0x78, 0xE1,  0x7E, 0xF0,
    0x80, 0x00,  0x80, 0x07,  0x78, 0xE1,  0x07, 0x3F,
    0x00, 0x3F,  0x7F, 0xF8,  0x78, 0xE1,  0xE0, 0x03,
    0x0F, 0xFF,  0x80, 0x07,  0x78, 0xE1,  0x7E, 0xF0,
    0xFF, 0xF8,  0x7F, 0xF8,  0x78, 0xE1,  0x07, 0x3F,
    0xFC, 0x00,  0x80, 0x07,  0x78, 0xE1,  0xE0, 0x03,
    0x00, 0x00,  0x7F, 0xF8,  0x78, 0xE1,  0x7E, 0xF0,
    0x00, 0xFF,  0x80, 0x07,  0x78, 0xE1,  0x07, 0x3F,
    0x7F, 0xFF,  0x7F, 0xF8,  0x78, 0xE1,  0xE0, 0x03,
    0xFF, 0xC0,  0x80, 0x07,  0x78, 0xE1,  0x7E, 0xF0,
    0xF0, 0x00,  0x7F, 0xF8,  0x78, 0xE1,  0x07, 0x3F,
    0x00, 0x07,  0x80, 0x07,  0x78, 0xE1,  0xE0, 0x03,
    0x01, 0xFF,  0x7F, 0xF8,  0x78, 0xE1,  0x7E, 0xF0,
    0xFF, 0xFF,  0x80, 0x07,  0x78, 0xE1,  0x07, 0x3F,
    0xFF, 0x00,  0x7F, 0xF8,  0x78, 0xE1,  0xE0, 0x03,
    0x80, 0x00,  0x80, 0x07,  0x78, 0xE1,  0x7E, 0xF0,
    0x00, 0x1F,  0x7F, 0xF8,  0x78, 0xE1,  0x07, 0x3F,
    0x0F, 0xFF,  0x80, 0x07,  0x78, 0xE1,  0xE0, 0x03,
    0xFF, 0xF8,  0x7F, 0xF8,  0x78, 0xE1,  0x7E, 0xF0,
    0xFE, 0x00,  0x80, 0x07,  0x78, 0xE1,  0x07, 0x3F,
    0x00, 0x00,  0x7F, 0xF8,  0x78, 0xE1,  0xE0, 0x03,
    0x00, 0xFF,  0x80, 0x07,  0x78, 0xE1,  0x7E, 0xF0,
    0x3F, 0xFF,  0x7F, 0xF8,  0x78, 0xE1,  0x07, 0x3F,
    0xFF, 0xE0,  0x80, 0x07,  0x78, 0xE1,  0xE0, 0x03,
    0xF0, 0x00,  0x7F, 0xF8,  0x78, 0xE1,  0x7E, 0xF0,
    0x00, 0x03,  0x80, 0x07,  0x78, 0xE1,  0x07, 0x3F};
//-----------------------------------------------------------
// Quantity of 1 in binary code
//
U8 quontity_of_ones[256] = {
    0,1,1,2,1,2,2,3,1,2,2,3,2,3,3,4,
    1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,
    1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,
    2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
    1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,
    2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
    2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
    3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
    1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,
    2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
    2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
    3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
    2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
    3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
    3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
    4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8};
// -------------------------------------------------------//-------------------------------------------------------------------
volatile U8 bit_count;
volatile U8 corr_counter;
volatile U8 corr_complete = 0;          // 1 - correlation complete
volatile U8 corr_coeff[8];              // sin & cos coefficients
volatile U8 *bpf_corr_table;            // pointer to the sin/cos table
//-------------------------------------------------------------------
//
//
interrupt timer_interrupt(void)
{
    static U8 air_8_samples;
    {
        static U8 air_temp_8;
        air_8_samples <<= 1;
        if(tst_bit(INPUT_BIT))   air_8_samples++;
        if (bit_count > 7)          // >= 8
        {
            bit_count = 0;
            air_temp_8 = air_8_samples;
        }
        if (corr_counter > 7)       // >= 8  Skip first 8 bad samples
        {
            // Next line calculates sin(cos) coefficient for last 8 samples
            corr_coeff[bit_count] += quontity_of_ones[air_temp_8 ^ (*bpf_corr_table)];
            bpf_corr_table++;           // take next value from table
            bit_count++;
            if (corr_counter >= (CORR_LENGTH+8)) // get all samples
            {
                disable_timer_interrupt();
                corr_complete = 1;      // Set end of single correlation flag
            }
        }
        corr_counter++;
    }
}
U8 search_freq (void)
{
    U8 i;
    U8 *pcorr;
    bit_count = 0;
    corr_counter = 0;
    corr_complete = 0;
    for (i = 0, pcorr = corr_coeff; i < 8; i++)
        *pcorr++ = 0;                           // Clear corr_coeff[];
    bpf_corr_table = corrtab_preamble;
    start_timer_interrupt(9600);
    while (!corr_complete)              // wait for end of correlation cycle
        ;
    for(i = 0, pcorr = corr_coeff; i < 4; i++)
    {
        U8 res;
        if (*pcorr > CORR_LENGTH/2) res  = *pcorr - CORR_LENGTH/2;
        else                        res  = CORR_LENGTH/2 - *pcorr;
        pcorr++;                        // pcorr = sin^2
        if (*pcorr > CORR_LENGTH/2) res += *pcorr - CORR_LENGTH/2;
        else                        res += CORR_LENGTH/2 - *pcorr;
        pcorr++;                        // pcorr = sin^2+cos^2
        if (res > CORR_THRESHOLD) return (i + 1);
    }
    return 0;
}
//-------------------------------------------------------------------