0,0 → 1,74 |
/**************************************************************************** |
* Copyright (C) 2011 by Claas Anders "CaScAdE" Rathje * |
* admiralcascade@gmail.com * |
* Project-URL: http://www.mylifesucks.de/oss/c-epilepsy/ * |
* * |
* This program is free software; you can redistribute it and/or modify * |
* it under the terms of the GNU General Public License as published by * |
* the Free Software Foundation; either version 2 of the License. * |
* * |
* This program is distributed in the hope that it will be useful, * |
* but WITHOUT ANY WARRANTY; without even the implied warranty of * |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
* GNU General Public License for more details. * |
* * |
* You should have received a copy of the GNU General Public License * |
* along with this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
****************************************************************************/ |
|
#include "integer_math.h" |
|
/* |
CORDIC atan2 generated by http://vivara.net/cgi-bin/cordic.cgi?range=3600&n=10&size=int16_t |
|
send bugs to Mark Rages <markrages@gmail.com> |
|
result is one revolution per 3600 counts. |
|
Performance test (error as percent of revolution): |
Maximum: +0.088% |
Mean: 0.006% |
Minimum: -0.085% |
mean(abs): 0.029% |
rms: 0.001% |
*/ |
|
int16_t atan2_fp(int16_t y, int16_t x) { |
|
const int16_t cordic[] = {450, 266, 140, 71, 36, 18, 9, 4, 2, 1}; |
int16_t theta; |
int16_t i; |
|
if (x < 0) { |
theta = 1800; |
x = -x; |
y = -y; |
} else { |
theta = 0; |
} |
|
for (i = 0; i < 10; i++) { |
int16_t last_x; |
|
last_x = x; |
|
if (y < 0) { // sign=1 |
x -= y >> i; |
y += last_x >> i; |
theta -= cordic[i]; |
} else { |
x += y >> i; |
y -= last_x >> i; |
theta += cordic[i]; |
} |
} |
|
#ifdef CORDIC_DEBUG |
printf("%d", theta); |
printf(" %lld\n", ((long long int)x) * 155L / 256); |
#endif |
|
return theta % 3600; |
} |