WebSVN - FlightCtrl - Rev 155 - /branches/salvo

/*
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 11.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);

}

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/math.c @ 536 - Rev 155