WebSVN - FlightCtrl - Blame - Rev 155 - /branches/salvo

Rev	Author	Line No.	Line
149	salvo	1	/*
		2	This program (files math.c and math.h) is free software; you can redistribute it and/or modify
		3	it under the terms of the GNU General Public License as published by the Free Software Foundation;
		4	either version 3 of the License, or (at your option) any later version.
		5	This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
		6	without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
		7	GNU General Public License for more details. You should have received a copy of the GNU General Public License
		8	along with this program. If not, see <http://www.gnu.org/licenses/>.
		9
		10	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
		11	*/
		12	/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		13	Peter Muehlenbrock
		14	Winkelfunktionen sin, cos und arctan in
		15	brute-force Art: Sehr Schnell, nicht sonderlich genau, aber ausreichend
		16	Stand 11.9.2007
		17	++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		18	*/
155	salvo	19	#include "main.h"
149	salvo	20
155	salvo	21
149	salvo	22	// arctan Funktion: Eingabewert x,y Rueckgabe =arctan(x,y) in grad
155	salvo	23	int arctan_i(long signed int x, long signed int y)
149	salvo	24	{
155	salvo	25	short int change_xy = 0;
149	salvo	26	signed int i;
155	salvo	27	long signed int wert;
		28	int return_value;
149	salvo	29
155	salvo	30	if ((abs(x)) > (abs(y))) // x,y Werte vertauschen damit arctan <45 grad bleibt
149	salvo	31	{
155	salvo	32	change_xy = 1;
		33	i = x;
		34	x = y;
		35	y = i;
149	salvo	36	}
155	salvo	37
		38	// Quadranten ermitteln
149	salvo	39
155	salvo	40	// Wert durch lineare Interpolation ermitteln
		41	wert= abs((x*1000)/y);
		42
		43	if (wert <=268) //0...0.0,268 entsprechend 0..15 Grad
		44	{
		45	return_value = (int)((wert100)/(268-0)(15-0)/100) +0;
		46	}
		47	else if (wert <=578) //0,268...0.0,568 entsprechend 15..30 Grad
		48	{
		49	return_value = (int)((((wert-268)100)/(578-268)(30-15))/100) +15;
		50	}
		51	else //0,568...1 entsprechend 30..45 Grad
		52	{
		53	return_value = (int)((((wert-578)50)/(1000-578)(45-30))/50) +30;
		54	}
		55
		56	if (change_xy > 0) return_value = 90-return_value; //Quadrant 45..90 Grad
		57	if ((x > 0) && (y <0)) return_value = - return_value;
		58	else if ((x < 0) && (y > 0)) return_value = - return_value;
		59
		60	return return_value;
149	salvo	61	}
		62
		63
		64	// cosinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =cos(winkel)*1000
		65	signed int cos_i(signed int winkel)
		66	{
		67	winkel = sin_i(90-winkel);
		68	return winkel;
		69	}
		70
		71
		72	// sinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =sin(winkel)*1000
		73	signed int sin_i(signed int winkel)
		74	{
		75	short int m;
		76	winkel = winkel % 360;
		77	// Quadranten auswerten
		78	if ((winkel >= 90 ) && (winkel <180))
		79	{
		80	winkel = 180 - winkel;
		81	m = 1;
		82	}
		83	else if ((winkel >= 180 ) && (winkel <270))
		84	{
		85	winkel = winkel - 180;
		86	m = -1;
		87	}
		88	else if ((winkel >= 270 ) && (winkel < 360))
		89	{
		90	winkel = 360 -winkel ;
		91	m = -1;
		92	}
		93	else if ((winkel < 0) && (winkel > -90))
		94	{
		95	winkel = (abs(winkel));
		96	m = -1;
		97	}
		98	else if ((winkel < -90) && (winkel > -180))
		99	{
		100	winkel = 180-(abs(winkel));
		101	m = -1;
		102	}
		103	else if ((winkel < -180) && (winkel > -270))
		104	{
		105	winkel = (abs(winkel))-180;
		106	m = +1;
		107	}
		108	else if ((winkel < -270) && (winkel > -360))
		109	{
		110	winkel = 360-abs(winkel);
		111	m = +1;
		112	}
		113	else
		114	{
		115	m = +1;
		116	}
		117
		118
		119	//Aus Tabelle Werte holen
		120	switch (winkel)
		121	{
		122	case 1:
		123	winkel = 17;
		124	break;
		125	case 2:
		126	winkel = 35;
		127	break;
		128	case 3:
		129	winkel = 52;
		130	break;
		131	case 4:
		132	winkel = 70;
		133	break;
		134	case 5:
		135	winkel = 87;
		136	break;
		137	case 6:
		138	winkel = 105;
		139	break;
		140	case 7:
		141	winkel = 122;
		142	break;
		143	case 8:
		144	winkel = 139;
		145	break;
		146	case 9:
		147	winkel = 156;
		148	break;
		149	case 10:
		150	winkel = 174;
		151	break;
		152	case 11:
		153	winkel = 191;
		154	break;
		155	case 12:
		156	winkel = 208;
		157	break;
		158	case 13:
		159	winkel = 225;
		160	break;
		161	case 14:
		162	winkel = 242;
		163	break;
		164	case 15:
		165	winkel = 259;
		166	break;
		167	case 16:
		168	winkel = 276;
		169	break;
		170	case 17:
		171	winkel = 292;
		172	break;
		173	case 18:
		174	winkel = 309;
		175	break;
		176	case 19:
		177	winkel = 326;
		178	break;
		179	case 20:
		180	winkel = 342;
		181	break;
		182	case 21:
		183	winkel = 359;
		184	break;
		185	case 22:
		186	winkel = 375;
		187	break;
		188	case 23:
		189	winkel = 391;
		190	break;
		191	case 24:
		192	winkel = 407;
		193	break;
		194	case 25:
		195	winkel = 423;
		196	break;
		197	case 26:
		198	winkel = 438;
		199	break;
		200	case 27:
		201	winkel = 454;
		202	break;
		203	case 28:
		204	winkel = 469;
		205	break;
		206	case 29:
		207	winkel = 485;
		208	break;
		209	case 30:
		210	winkel = 500;
		211	break;
		212	case 31:
		213	winkel = 515;
		214	break;
		215	case 32:
		216	winkel = 530;
		217	break;
		218	case 33:
		219	winkel = 545;
		220	break;
		221	case 34:
		222	winkel = 559;
		223	break;
		224	case 35:
		225	winkel = 574;
		226	break;
		227	case 36:
		228	winkel = 588;
		229	break;
		230	case 37:
		231	winkel = 602;
		232	break;
		233	case 38:
		234	winkel = 616;
		235	break;
		236	case 39:
		237	winkel = 630;
		238	break;
		239	case 40:
		240	winkel = 643;
		241	break;
		242	case 41:
		243	winkel = 656;
		244	break;
		245	case 42:
		246	winkel = 682;
		247	break;
		248	case 43:
		249	winkel = 682;
		250	break;
		251	case 44:
		252	winkel = 695;
		253	break;
		254	case 45:
		255	winkel = 707;
		256	break;
		257	case 46:
		258	winkel = 719;
		259	break;
		260	case 47:
		261	winkel = 731;
		262	case 48:
		263	winkel = 743;
		264	break;
		265	case 49:
		266	winkel = 755;
		267	break;
		268	case 50:
		269	winkel = 766;
		270	break;
		271	case 51:
		272	winkel = 777;
		273	break;
		274	case 52:
		275	winkel = 788;
		276	break;
		277	case 53:
		278	winkel = 799;
		279	break;
		280	case 54:
		281	winkel = 809;
		282	break;
		283	case 55:
		284	winkel = 819;
		285	break;
		286	case 56:
		287	winkel = 829;
		288	break;
		289	case 57:
		290	winkel = 839;
		291	break;
		292	case 58:
		293	winkel = 848;
		294	break;
		295	case 59:
		296	winkel = 857;
		297	break;
		298	case 60:
		299	winkel = 866;
		300	break;
		301	case 61:
		302	winkel = 875;
		303	break;
		304	case 62:
		305	winkel = 883;
		306	break;
		307	case 63:
		308	winkel = 891;
		309	break;
		310	case 64:
		311	winkel = 899;
		312	break;
		313	case 65:
		314	winkel = 906;
		315	break;
		316	case 66:
		317	winkel = 914;
		318	break;
		319	case 67:
		320	winkel = 921;
		321	break;
		322	case 68:
		323	winkel = 927;
		324	break;
		325	case 69:
		326	winkel = 934;
		327	break;
		328	case 70:
		329	winkel = 940;
		330	break;
		331	case 71:
		332	winkel = 946;
		333	break;
		334	case 72:
		335	winkel = 951;
		336	break;
		337	case 73:
		338	winkel = 956;
		339	break;
		340	case 74:
		341	winkel = 961;
		342	break;
		343	case 75:
		344	winkel = 966;
		345	break;
		346	case 76:
		347	winkel = 970;
		348	break;
		349	case 77:
		350	winkel = 974;
		351	break;
		352	case 78:
		353	winkel = 978;
		354	break;
		355	case 79:
		356	winkel = 982;
		357	break;
		358	case 80:
		359	winkel = 985;
		360	break;
		361	case 81:
		362	winkel = 988;
		363	break;
		364	case 82:
		365	winkel = 990;
		366	break;
		367	case 83:
		368	winkel = 993;
		369	break;
		370	case 84:
		371	winkel = 995;
		372	break;
		373	case 85:
		374	winkel = 996;
		375	break;
		376	case 86:
		377	winkel = 998;
		378	break;
		379	case 87:
		380	winkel = 999;
		381	break;
		382	case 88:
		383	winkel = 999;
		384	break;
		385	case 89:
		386	winkel = 1000;
		387	break;
		388	case 90:
		389	winkel = 1000;
		390	break;
		391	default:
		392	winkel = 0;
		393	break;
		394
		395	}
		396	return (winkel*m);
		397
		398	}
		399
		400

Subversion Repositories FlightCtrl

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