• CORDIC

    Paul Gould03/26/2020 at 16:48 0 comments

    Read ADC

        aa1 = ADC_SAR_GetResult16(0u);
        bb1 = ADC_SAR_GetResult16(1u);
        cc1 = ADC_SAR_GetResult16(2u);
        dd1 = ADC_SAR_GetResult16(3u);

    Do CORDIC Calculation

    int cordic(int32 aa, int32 bb, int32 cc, int32 dd)
    {
        int hor, ver;
        hor = aa - cc;
        ver = bb - dd;
        double abs_val_sq = (hor * hor) + (ver * ver);
        double inv_amp = 1.0 / (sqrt(abs_val_sq));
        double hor_f = ((double)hor) * inv_amp;
        double ver_f = ((double)ver) * inv_amp;
        double hor_s = asin(hor_f);
        double ver_s = acos(ver_f);
        double ang_r = 0.0;
        double ang = 0.0;
        int ang_cordic=0;
        
        if (hor_s>0)
        {
            if (ver_s > pid2)
                ang_r = ver_s + (pi-hor_s);
            else
                ang_r =  ver_s + hor_s;
        }
        else
        {
            if (ver_s > pid2)
                ang_r = pi + (pi - ver_s) + (pi - hor_s);
            else
                ang_r =  pi + pi + (pi - ver_s) + (pi + hor_s);
        }
        
        ang = ang_r * rad_deg;
            
        if (abs_val_sq > 100000) // good (Todo crc or party)
        {
            ang_cordic = (int)(ang_r * rad_12); // | 0x8000;  
        }
        else // bad
        {
            ang_cordic =  ((int)(ang_r * rad_12));
        }
        return (ang_cordic);
    }