• ### 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);
}```