Subversion Repositories FlightCtrl

/*

Copyright 2007, Niklas Nold

This program (files math.c and math.h) is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation;
either version 3 of the License, or (at your option) any later version.  
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 Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.

Please note: All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de
*/

#include "main.h"


const uint8_t pgm_atan[346] PROGMEM = {0,1,2,3,4,4,5,6,7,8,9,10,11,11,12,13,14,15,16,17,17,18,19,20,21,21,22,23,24,24,25,26,27,27,28,29,29,30,31,31,32,33,33,34,35,35,36,36,37,37,38,39,39,40,40,41,41,42,42,43,43,44,44,45,45,45,46,46,47,47,48,48,48,49,49,50,50,50,51,51,51,52,52,52,53,53,53,54,54,54,55,55,55,55,56,56,56,57,57,57,57,58,58,58,58,59,59,59,59,60,60,60,60,60,61,61,61,61,62,62,62,62,62,63,63,63,63,63,63,64,64,64,64,64,64,65,65,65,65,65,65,66,66,66,66,66,66,66,67,67,67,67,67,67,67,68,68,68,68,68,68,68,68,69,69,69,69,69,69,69,69,69,70,70,70,70,70,70,70,70,70,71,71,71,71,71,71,71,71,71,71,71,72,72,72,72,72,72,72,72,72,72,72,73,73,73,73,73,73,73,73,73,73,73,73,73,73,74,74,74,74,74,74,74,74,74,74,74,74,74,74,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79};

//############################################################################
// Arkustangens2 im Gradmaß
signed int atan2_i(signed int x, signed int y)
//############################################################################
{
        int i,angle;
        int8_t m;
       
        if (!x && !y) return 0;         //atan2 = 0 für x und y = 0
       
        if (y < 0) m=-1;
        else m=1;
       
        if (!x) return (90*m);          // atan2 = 90° für x = 0
       
        i = abs(((long)y*64) / x);              // Berechne i für die Lookup table (Schrittweite atan(x) ist 0,015625 -> y *64)

        if (i<346) angle = pgm_read_byte(&pgm_atan[i]); // Lookup für 1° bis 79°
        else if (i>7334) angle = 90;                                            // Grenzwert ist 90°
        else if (i>2444) angle = 89;                                            // 89° bis 80° über Wertebereiche
        else if (i>1465) angle = 88;
        else if (i>1046) angle = 87;
        else if (i>813) angle = 86;
        else if (i>664) angle = 85;
        else if (i>561) angle = 84;
        else if (i>486) angle = 83;
        else if (i>428) angle = 82;
        else if (i>382) angle = 81;
        else angle = 80; // (i>345)
       
        if (x > 0) return (angle*m);    // Quadrant I und IV
        else if ((x < 0) && (m > 0)) return (-angle + 180);     // Quadrant II
        else return (angle - 180); // x < 0 && y < 0    Quadrant III
}

/*
const uint16_t pgm_sinus_i[91] PROGMEM = {
   0,  18,  36,  54,  71,  89, 107, 125, 143, 160, 178, 195, 213, 230, 248,
 265, 282, 299, 316, 333, 350, 367, 384, 400, 416, 433, 449, 465, 481, 496,
 512, 527, 543, 558, 573, 587, 602, 616, 630, 644, 658, 672, 685, 698, 711,
 724, 737, 749, 761, 773, 784, 796, 807, 818, 828, 839, 849, 859, 868, 878,
 887, 896, 904, 912, 920, 928, 935, 943, 949, 956, 962, 968, 974, 979, 984,
 989, 994, 998,1002,1005,1008,1011,1014,1016,1018,1020,1022,1023,1023,1024,
 1024
 }; //für div 1024 = div 2^10 = (>>10)
*/  
const uint16_t pgm_sinus_i[91] PROGMEM = {
0,71,143,214,286,357,428,499,570,641,711,782,852,921,991,
1060,1129,1198,1266,1334,1401,1468,1534,1600,1666,1731,1796,1860,1923,1986,
2048,2110,2171,2231,2290,2349,2408,2465,2522,2578,2633,2687,2741,2793,2845,
2896,2946,2996,3044,3091,3138,3183,3228,3271,3314,3355,3396,3435,3474,3511,
3547,3582,3617,3650,3681,3712,3742,3770,3798,3824,3849,3873,3896,3917,3937,
3956,3974,3991,4006,4021,4034,4046,4056,4065,4074,4080,4086,4090,4094,4095,
4096
}; //für div 4096 = div 2^12 = (>>12)
 
 
//############################################################################
// Kosinusfunktion im Gradmaß
signed int cos_i(signed int winkel)
//############################################################################
{
        return (sin_i(90-winkel));
}

//############################################################################
// Sinusfunktion im Gradmaß
signed int sin_i(signed int winkel)
//############################################################################
{
 short int m,n;
 signed int sinus;
 
 if (winkel < 0)
 {
        m = -1;
        winkel = abs(winkel);
 }
 else m = +1;
 
 // Quadranten auswerten
 if (winkel <= 90) n=1;
 else if ((winkel > 90) && (winkel <= 180)) {winkel = 180 - winkel; n = 1;}
 else if ((winkel > 180) && (winkel <= 270)) {winkel = winkel - 180; n = -1;}
 else {winkel = 360 - winkel; n = -1;}  //if ((winkel > 270) && (winkel <= 360))

 sinus = pgm_read_word(&pgm_sinus_i[winkel]);

return (sinus*m*n);
}