51 lines
1 KiB
C
51 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;
|
||
|
|
}
|