| 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; |
| } |