Subversion Repositories FlightCtrl

Rev

Rev 155 | Blame | Last modification | View Log | RSS feed

/*
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 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 General Public License for more details. You should have received a copy of the GNU 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
*/

/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Peter Muehlenbrock
Winkelfunktionen sin, cos und arctan in
brute-force Art: Sehr Schnell, nicht sonderlich genau, aber ausreichend
Stand 20.9.2007
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/

#include "main.h"


// arctan Funktion: Eingabewert x,y Rueckgabe =arctan(x,y) in grad
int arctan_i(long signed int x, long signed int y)
{
        short int change_xy = 0;
        signed int i;
        long signed int wert;
        int return_value;

        if ((abs(x)) > (abs(y)))  // x,y Werte vertauschen damit arctan <45 grad bleibt
        {      
                change_xy       = 1;
                i                       = x;
                x                       = y;
                y                       = i;
        }
       
        // Quadranten ermitteln

        // Wert durch lineare Interpolation ermitteln
        wert= abs((x*1000)/y);

        if (wert <=268) //0...0.0,268  entsprechend 0..15 Grad
        {
                return_value = (int)((wert*100)/(268-0)*(15-0)/100) +0;
        }
        else if (wert <=578) //0,268...0.0,568  entsprechend 15..30 Grad
        {      
                return_value = (int)((((wert-268)*100)/(578-268)*(30-15))/100) +15;            
        }
        else //0,568...1  entsprechend 30..45 Grad
        {      
                return_value = (int)((((wert-578)*50)/(1000-578)*(45-30))/50) +30;             
        }

        if (change_xy == 0)  return_value = 90-return_value; //Quadrant 45..90 Grad
        if ((x >= 0) && (y <0)) return_value = - return_value;
        else if ((x < 0) && (y >= 0)) return_value = - return_value;

return return_value;
}


// cosinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =cos(winkel)*1000
signed int cos_i(signed int winkel)
{
  winkel = sin_i(90-winkel);
  return winkel;
}


// sinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =sin(winkel)*1000
signed int sin_i(signed int winkel)
{
 short int m;
 winkel = winkel % 360;
 // Quadranten auswerten
 if ((winkel >= 90 ) && (winkel <180))
 {
        winkel = 180 - winkel;
        m = 1;
 }
 else if ((winkel >= 180 ) && (winkel <270))
 {
        winkel = winkel - 180;
        m = -1;
 }
 else if ((winkel >= 270 ) && (winkel < 360))
 {
        winkel = 360 -winkel ;
        m = -1;
 }
 else if ((winkel < 0) && (winkel > -90))
 {
        winkel = (abs(winkel));
        m = -1;
 }
else if ((winkel < -90) && (winkel > -180))
 {
        winkel = 180-(abs(winkel));
        m = -1;
 }
else if ((winkel < -180) && (winkel > -270))
 {
        winkel = (abs(winkel))-180;
        m = +1;
 }
else if ((winkel < -270) && (winkel > -360))
 {
        winkel = 360-abs(winkel);
        m = +1;
 }
 else  
 {
        m = +1;
 }


 //Aus Tabelle Werte holen
        switch (winkel)
        {
                case 1:
                        winkel = 17;
                        break;
                case 2:
                        winkel = 35;
                        break;
                case 3:
                        winkel = 52;
                        break;
                case 4:
                        winkel = 70;
                        break;
                case 5:
                        winkel = 87;
                        break;
                case 6:
                        winkel = 105;
                        break;
                case 7:
                        winkel = 122;
                        break;
                case 8:
                        winkel = 139;
                        break;
                case 9:
                        winkel = 156;
                        break;
                case 10:
                        winkel = 174;
                        break;
                case 11:
                        winkel = 191;
                        break;
                case 12:
                        winkel = 208;
                        break;
                case 13:
                        winkel = 225;
                        break;
                case 14:
                        winkel = 242;
                        break;
                case 15:
                        winkel = 259;
                        break;
                case 16:
                        winkel = 276;
                        break;
                case 17:
                        winkel = 292;
                        break;
                case 18:
                        winkel = 309;
                        break;
                case 19:
                        winkel = 326;
                        break;
                case 20:
                        winkel = 342;
                        break;
                case 21:
                        winkel = 359;
                        break;
                case 22:
                        winkel = 375;
                        break;
                case 23:
                        winkel = 391;
                        break;
                case 24:
                        winkel = 407;
                        break;
                case 25:
                        winkel = 423;
                        break;
                case 26:
                        winkel = 438;
                        break;
                case 27:
                        winkel = 454;
                        break;
                case 28:
                        winkel = 469;
                        break;
                case 29:
                        winkel = 485;
                        break;
                case 30:
                        winkel = 500;
                        break;
                case 31:
                        winkel = 515;
                        break;
                case 32:
                        winkel = 530;
                        break;
                case 33:
                        winkel = 545;
                        break;
                case 34:
                        winkel = 559;
                        break;
                case 35:
                        winkel = 574;
                        break;
                case 36:
                        winkel = 588;
                        break;
                case 37:
                        winkel = 602;
                        break;
                case 38:
                        winkel = 616;
                        break;
                case 39:
                        winkel = 630;
                        break;
                case 40:
                        winkel = 643;
                        break;
                case 41:
                        winkel = 656;
                        break;
                case 42:
                        winkel = 682;
                        break;
                case 43:
                        winkel = 682;
                        break;
                case 44:
                        winkel = 695;
                        break;
                case 45:
                        winkel = 707;
                        break;
                case 46:
                        winkel = 719;
                        break;
                case 47:
                        winkel = 731;
                case 48:
                        winkel = 743;
                        break;
                case 49:
                        winkel = 755;
                        break;
                case 50:
                        winkel = 766;
                        break;
                case 51:
                        winkel = 777;
                        break;
                case 52:
                        winkel = 788;
                        break;
                case 53:
                        winkel = 799;
                        break;
                case 54:
                        winkel = 809;
                        break;
                case 55:
                        winkel = 819;
                        break;
                case 56:
                        winkel = 829;
                        break;
                case 57:
                        winkel = 839;
                        break;
                case 58:
                        winkel = 848;
                        break;
                case 59:
                        winkel = 857;
                        break;
                case 60:
                        winkel = 866;
                        break;
                case 61:
                        winkel = 875;
                        break;
                case 62:
                        winkel = 883;
                        break;
                case 63:
                        winkel = 891;
                        break;
                case 64:
                        winkel = 899;
                        break;
                case 65:
                        winkel = 906;
                        break;
                case 66:
                        winkel = 914;
                        break;
                case 67:
                        winkel = 921;
                        break;
                case 68:
                        winkel = 927;
                        break;
                case 69:
                        winkel = 934;
                        break;
                case 70:
                        winkel = 940;
                        break;
                case 71:
                        winkel = 946;
                        break;
                case 72:
                        winkel = 951;
                        break;
                case 73:
                        winkel = 956;
                        break;
                case 74:
                        winkel = 961;
                        break;
                case 75:
                        winkel = 966;
                        break;
                case 76:
                        winkel = 970;
                        break;
                case 77:
                        winkel = 974;
                        break;
                case 78:
                        winkel = 978;
                        break;
                case 79:
                        winkel = 982;
                        break;
                case 80:
                        winkel = 985;
                        break;
                case 81:
                        winkel = 988;
                        break;
                case 82:
                        winkel = 990;
                        break;
                case 83:
                        winkel = 993;
                        break;
                case 84:
                        winkel = 995;
                        break;
                case 85:
                        winkel = 996;
                        break;
                case 86:
                        winkel = 998;
                        break;
                case 87:
                        winkel = 999;
                        break;
                case 88:
                        winkel = 999;
                        break;
                case 89:
                        winkel = 1000;
                        break;
                case 90:
                        winkel = 1000;
                        break;
                default:
                        winkel = 0;
                        break;

        }
return (winkel*m);

}