Subversion Repositories Projects

/****************************************************************************
 *   Copyright (C) 2011-2012 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;
}