Rev 1199 | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1199 | - | 1 | /**************************************************************************** |
| 1437 | - | 2 | * Copyright (C) 2011-2012 by Claas Anders "CaScAdE" Rathje * |
| 1199 | - | 3 | * admiralcascade@gmail.com * |
| 4 | * Project-URL: http://www.mylifesucks.de/oss/c-epilepsy/ * |
||
| 5 | * * |
||
| 6 | * This program is free software; you can redistribute it and/or modify * |
||
| 7 | * it under the terms of the GNU General Public License as published by * |
||
| 8 | * the Free Software Foundation; either version 2 of the License. * |
||
| 9 | * * |
||
| 10 | * This program is distributed in the hope that it will be useful, * |
||
| 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
||
| 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
||
| 13 | * GNU General Public License for more details. * |
||
| 14 | * * |
||
| 15 | * You should have received a copy of the GNU General Public License * |
||
| 16 | * along with this program; if not, write to the * |
||
| 17 | * Free Software Foundation, Inc., * |
||
| 18 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
||
| 19 | ****************************************************************************/ |
||
| 20 | |||
| 21 | #include "integer_math.h" |
||
| 22 | |||
| 23 | /* |
||
| 24 | CORDIC atan2 generated by http://vivara.net/cgi-bin/cordic.cgi?range=3600&n=10&size=int16_t |
||
| 25 | |||
| 26 | send bugs to Mark Rages <markrages@gmail.com> |
||
| 27 | |||
| 28 | result is one revolution per 3600 counts. |
||
| 29 | |||
| 30 | Performance test (error as percent of revolution): |
||
| 31 | Maximum: +0.088% |
||
| 32 | Mean: 0.006% |
||
| 33 | Minimum: -0.085% |
||
| 34 | mean(abs): 0.029% |
||
| 35 | rms: 0.001% |
||
| 36 | */ |
||
| 37 | |||
| 38 | int16_t atan2_fp(int16_t y, int16_t x) { |
||
| 39 | |||
| 40 | const int16_t cordic[] = {450, 266, 140, 71, 36, 18, 9, 4, 2, 1}; |
||
| 41 | int16_t theta; |
||
| 42 | int16_t i; |
||
| 43 | |||
| 44 | if (x < 0) { |
||
| 45 | theta = 1800; |
||
| 46 | x = -x; |
||
| 47 | y = -y; |
||
| 48 | } else { |
||
| 49 | theta = 0; |
||
| 50 | } |
||
| 51 | |||
| 52 | for (i = 0; i < 10; i++) { |
||
| 53 | int16_t last_x; |
||
| 54 | |||
| 55 | last_x = x; |
||
| 56 | |||
| 57 | if (y < 0) { // sign=1 |
||
| 58 | x -= y >> i; |
||
| 59 | y += last_x >> i; |
||
| 60 | theta -= cordic[i]; |
||
| 61 | } else { |
||
| 62 | x += y >> i; |
||
| 63 | y -= last_x >> i; |
||
| 64 | theta += cordic[i]; |
||
| 65 | } |
||
| 66 | } |
||
| 67 | |||
| 68 | #ifdef CORDIC_DEBUG |
||
| 69 | printf("%d", theta); |
||
| 70 | printf(" %lld\n", ((long long int)x) * 155L / 256); |
||
| 71 | #endif |
||
| 72 | |||
| 73 | return theta % 3600; |
||
| 74 | } |