Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 182 → Rev 183

/branches/salvo_gps/GPS.c
13,7 → 13,7
Peter Muehlenbrock
Auswertung der Daten vom GPS im ublox Format
Hold Modus mit PID Regler
Stand 27.9.2007
Stand 28.9.2007
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
#include "main.h"
20,15 → 20,15
//#include "gps.h"
 
// Defines fuer ublox Messageformat um Auswertung zu steuern
#define UBLOX_IDLE 0
#define UBLOX_SYNC1 1
#define UBLOX_SYNC2 2
#define UBLOX_CLASS 3
#define UBLOX_ID 4
#define UBLOX_LEN1 5
#define UBLOX_LEN2 6
#define UBLOX_CKA 7
#define UBLOX_CKB 8
#define UBLOX_IDLE 0
#define UBLOX_SYNC1 1
#define UBLOX_SYNC2 2
#define UBLOX_CLASS 3
#define UBLOX_ID 4
#define UBLOX_LEN1 5
#define UBLOX_LEN2 6
#define UBLOX_CKA 7
#define UBLOX_CKB 8
#define UBLOX_PAYLOAD 9
 
// ublox Protokoll Identifier
413,7 → 413,6
GPS_dist_2trgt = (int )dist;
 
// Winkel und Distanz in Nick und Roll groessen umrechnen
long int a,b;
GPS_Roll = (int) +((dist * sin_i(GPS_hdng_rel_2trgt))/(1000*8));
GPS_Nick = (int) -((dist * cos_i(GPS_hdng_rel_2trgt))/(1000*8));
 
/branches/salvo_gps/math.c
13,7 → 13,8
Peter Muehlenbrock
Winkelfunktionen sin, cos und arctan in
brute-force Art: Sehr Schnell, nicht sonderlich genau, aber ausreichend
Stand 20.9.2007
Sinus Funktion von Nick666 vereinfacht
Stand 28.9.2007
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
#include "main.h"
69,332 → 70,28
}
 
 
const unsigned int pgm_sinus[91] PROGMEM = {0,17,35,52,70,87,105,122,139,156,174,191,208,225,242,259,276,292,309,326,342,358,375,391,407,423,438,454,469,485,500,515,530,545,559,574,588,602,616,629,643,656,669,682,695,707,719,731,743,755,766,777,788,799,809,819,829,839,848,857,866,875,883,891,899,906,914,921,927,934,940,946,951,956,961,966,970,974,978,982,985,988,990,993,995,996,998,999,999,1000,1000};
//von Nick666, Stand 28.9.2007
// 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))
 
if (abs(winkel) >=360) winkel = winkel % 360;
if (winkel < 0)
{
winkel = 180 - winkel;
m = 1;
}
else if ((winkel >= 180 ) && (winkel <270))
{
winkel = winkel - 180;
m = -1;
winkel = abs(winkel);
}
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;
}
else m = +1;
// Quadranten auswerten
if ((winkel > 90 ) && (winkel <= 180)) winkel = winkel - 90;
else if ((winkel > 180 ) && (winkel <= 270)) winkel = winkel - 180;
else if ((winkel > 270) && (winkel <= 360)) winkel = winkel - 270;
// else //0 - 90 Grad
 
 
//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);
 
winkel = pgm_read_word(&pgm_sinus[winkel]);
return (winkel*m);
}