pico-sdr/src/cordic.h

50 lines
1 KiB
C

#pragma once
/*
* Copyright (c) 2020 STMicroelectronics.
* All rights reserved.
*
* This software component is licensed by ST under BSD 3-Clause license,
* the "License"; You may not use this file except in compliance with the
* License. You may obtain a copy of the License at:
*
* https://opensource.org/licenses/BSD-3-Clause
*/
#define CORDIC_MAXITER 9
#define CORDIC_PI 0x10000000
static int CORDIC_ZTBL[] = { 0x04000000, 0x025C80A4, 0x013F670B, 0x00A2223B, 0x005161A8, 0x0028BAFC,
0x00145EC4, 0x000A2F8B, 0x000517CA, 0x00028BE6, 0x000145F3, 0x0000A2FA,
0x0000517D, 0x000028BE, 0x0000145F, 0x00000A30 };
inline static __attribute__((__unused__)) int fast_atan2(int y, int x)
{
int k, tx, z = 0, fl = 0;
if (x < 0) {
fl = ((y > 0) ? +1 : -1);
x = -x;
y = -y;
}
for (k = 0; k < CORDIC_MAXITER; k++) {
tx = x;
if (y <= 0) {
x -= (y >> k);
y += (tx >> k);
z -= CORDIC_ZTBL[k];
} else {
x += (y >> k);
y -= (tx >> k);
z += CORDIC_ZTBL[k];
}
}
if (fl != 0) {
z += fl * CORDIC_PI;
}
return z;
}