Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 257 → Rev 258

/branches/salvo_gps/GPS.c
5,7 → 5,7
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 and GNU Lesser General Public License for more details.
You should have received a copy of GNU General Public License ((License_GPL.txt) and
You should have received a copy of GNU General Public License (License_GPL.txt) and
GNU Lesser General Public License (License_LGPL.txt) along with this program.
If not, see <http://www.gnu.org/licenses/>.
 
16,7 → 16,7
Auswertung der Daten vom GPS im ublox Format
Hold Modus mit PID Regler
Rückstuerz zur Basis Funktion
Stand 7.10.2007
Stand 8.10.2007
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
#include "main.h"
42,26 → 42,26
#define UBLOX_SYNCH1_CHAR 0xB5
#define UBLOX_SYNCH2_CHAR 0x62
 
signed int GPS_Nick = 0;
signed int GPS_Roll = 0;
short int ublox_msg_state = UBLOX_IDLE;
static uint8_t chk_a =0; //Checksum
static uint8_t chk_b =0;
short int gps_state,gps_sub_state; //Zustaende der Statemachine
short int gps_updte_flag;
signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel
signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;
signed int GPS_Nick = 0;
signed int GPS_Roll = 0;
short int ublox_msg_state = UBLOX_IDLE;
static uint8_t chk_a =0; //Checksum
static uint8_t chk_b =0;
short int gps_state,gps_sub_state; //Zustaende der Statemachine
short int gps_updte_flag;
signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel
signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;
static unsigned int rx_len;
static unsigned int ptr_payload_data_end;
unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position
signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt
static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler
static uint8_t *ptr_payload_data;
static uint8_t *ptr_pac_status;
long int dist_flown;
signed int hdng_2home,dist_2home; //Richtung und Entfernung zur home Position
static signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt
static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler
static uint8_t *ptr_payload_data;
static uint8_t *ptr_pac_status;
long int dist_flown;
 
short int Get_GPS_data(void);
 
73,7 → 73,7
GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt
GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position
GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode
GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode
GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode
 
// Initialisierung
void GPS_Neutral(void)
165,7 → 165,6
return (n);
}
 
 
/*
Daten vom GPS im ublox MSG Format auswerten
Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
333,7 → 332,7
// in Winkel 0...360 Grad umrechnen
if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home);
else hdng_2home = (270 - hdng_2home);
dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen
dist_2home = get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen
gps_state = GPS_CRTL_HOME_ACTIVE;
return (GPS_STST_OK);
}
417,72 → 416,72
d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel
if (d3 > GPS_G2T_DIST_MAX_STOP)
if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel
{
hold_fast = 1; // Schnell Regeln
hold_reset_int = 1; // Integrator ausschalten
if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
// hold_fast = 1; // Schnell Regeln
hold_reset_int = 1; // Integrator ausschalten
if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
{
if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen
dist_flown += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
gps_sub_state = GPS_HOME_FAST_IN_TOL;
gps_sub_state = GPS_HOME_FAST_IN_TOL;
}
else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
{
/* if (gps_sub_state == GPS_HOME_FAST_IN_TOL) hold_reset_int = 1; //Integrator einmal am Beginn des normalen Regelns resetten
else hold_reset_int = 0;
*/ if (gps_g2t_act_v > 0) gps_g2t_act_v--;
if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren
gps_sub_state = GPS_HOME_FAST_OUTOF_TOL;
}
}
else //Schon ziemlich nahe am Ziel, deswegen abbremsen
else if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen
{
if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
hold_reset_int = 0; // Integrator einschalten
if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
{
if (gps_g2t_act_v > GPS_G2T_V_RAMP_DWN) gps_g2t_act_v = GPS_G2T_V_RAMP_DWN; // Geschwindigkeit zu hoch
else gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen
dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
gps_sub_state = GPS_HOME_RMPDWN_IN_TOL;
if (d3 > GPS_G2T_DIST_HOLD)
}
else
{
if (gps_g2t_act_v > 1) gps_g2t_act_v--;
gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL;
}
}
else //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt)
{
if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln
{
gps_sub_state = GPS_HOME_IN_TOL;
hold_fast = 0; // Wieder normal regeln
hold_reset_int = 0; // Integrator wieder aktivieren
if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN;
else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN;
if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN;
else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN;
if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN))
{
if (gps_g2t_act_v < GPS_G2T_V_RAMP_DWN) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen
dist_flown += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000;
dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000;
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt
gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt
gps_rel_hold_position.utm_east = 0;
gps_rel_hold_position.utm_north = 0;
gps_sub_state = GPS_HOME_FINISHED;
}
else //Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt)
{
hold_fast = 0; // Wieder normal regeln
hold_reset_int = 0; // Integrator wieder aktivieren
if (gps_rel_hold_position.utm_east > GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN;
else if (gps_rel_hold_position.utm_east < -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN;
if (gps_rel_hold_position.utm_north > GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN;
else if (gps_rel_hold_position.utm_north < - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN;
if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN))
{
gps_rel_hold_position.utm_east = 0;
gps_rel_hold_position.utm_north = 0;
gps_sub_state = GPS_HOME_FINISHED;
}
}
}
else
{
gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL;
if (gps_g2t_act_v > 0) gps_g2t_act_v--;
}
}
gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
return (GPS_STST_OK);
}
}
}
else return (GPS_STST_OK);
gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
return (GPS_STST_OK);
}
else
else // Keine GPS Daten verfuegbar, deswegen Abbruch
{
gps_state = GPS_CRTL_IDLE; //Abbruch
gps_state = GPS_CRTL_IDLE;
return (GPS_STST_ERR);
}
break;
519,16 → 518,21
 
// Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
// desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
#define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10
#define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm
signed long dist,int_east1,int_north1;
int phi;
phi = arctan_i(abs(dist_north),abs(dist_east));
dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln
dist = (long) (get_dist(dist_east,dist_north,phi)); //Zunaechst Entfernung zum Ziel ermitteln
 
diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10
if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen
// if (hold_fast > 0) diff_v = 10; //im schnellen Modus schwache Wirkung des Differenzierers
if (hold_fast == 0) // je Nach Modus andere Verstaerkungskurve fuer Differenzierer
{
diff_v = (int)((dist * (GPS_DIFF_NRML_MAX_V - 10)) / GPS_DIFF_NRML_MAX_D) +10; //Verstaerkung * 10
if (diff_v > GPS_DIFF_NRML_MAX_V) diff_v = GPS_DIFF_NRML_MAX_V; //begrenzen
}
else
{
diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10
if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen
}
 
//I Werte begrenzen
#define INT1_MAX (20 * GPS_V)
540,8 → 544,8
else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX;
 
int diff_p;
if (hold_fast > 0) diff_p = 2; //im schnellen Modus Proportionalanteil staerken
else diff_p = 1;
if (hold_fast == 0) diff_p = 1;
else diff_p = 2; //im schnellen Modus Proportionalanteil staerken
 
//PID Regler Werte aufsummieren
gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1 * diff_p)/8)+ ((diff_east * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse
/branches/salvo_gps/README_Gps.txt
1,6 → 1,6
*********************************************************************
GPS Implementierung von Peter Muehlenbrock ("Salvo") für Mikrokopter/FlightCrtl
Stand 7.10.2007
Stand 9.10.2007
Verwendung der SW ohne Gewaehr. Siehe auch die Lizenzbedingungen in File Licensce_LPGL.txt und Licensce_GPL.txt
*********************************************************************
Hardware-Voraussetzungen:
25,12 → 25,11
Parametrierung:
Der GPS Hold Regler ist ein PID Regler, der ueber die UserParameter1(P), 2(I) und D(3) gesteuert wird.
UserParameter1 beschreibt den P-Anteil, UserParameter2 den I-Anteil und UserParameter3 den D-Anteil.
Hier kann und muss gespielt werden.
Hier kann und muss gespielt werden.Alle Parameter koennen direkt im Mikrokoptertool in den Settings eingestellt werden.
Standardwerte bei kaum Wind sind 8 für den P-Anteil, 1 für den I-Anteil und 12 für den D-Anteil.
Standardwerte bei leichtem verhältnissen sind 12 für den P-Anteil, 2 für den I-Anteil und 18 für den D-Anteil.
Standardwerte bei rauheren verhältnissen sind 16 für den P-Anteil, 2 für den I-Anteil und 28 für den D-Anteil.
Standardwerte bei rauheren verhältnissen sind 16 für den P-Anteil, 2 für den I-Anteil und 24 für den D-Anteil.
Je größer die Werte des "ruckeliger" reagiert die Regelung aber desto schneller und stärker greift sie auch.
 
Wenn alle 0 sind, ist der Regler deaktiviert.
 
Voraussetzungen für GPS_Hold:
62,10 → 61,11
 
 
Bekannte Schwächen:
Leichte Schwingneigung ca. +-3m, bei längerer Neigung weicht der Ersatzkompass ab.
Bei längerer Neigung weicht der Ersatzkompass ab, was zu Lageregelungsfehlern bis zum Ausbrechen führen kann.
 
 
 
 
 
 
/branches/salvo_gps/fc.c
854,9 → 854,9
/*
DebugOut.Analog[7] = GasMischanteil;
*/
DebugOut.Analog[7] = dist_flown;
DebugOut.Analog[8] = KompassValue;
 
DebugOut.Analog[7] = dist_flown;
// DebugOut.Analog[8] = dist_frm_start_north;
 
/* DebugOut.Analog[0] = GPS_hdng_rel_2trgt;
/branches/salvo_gps/gps.h
1,7 → 1,7
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Peter Muehlenbrock ("Salvo")
// Definitionen fuer Modul GPS
// Stand 7.10.007
// Stand 9.10.007
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
extern signed int GPS_Nick;
extern signed int GPS_Roll;
67,21 → 67,21
} GPS_REL_POSITION_t;
 
 
extern GPS_ABS_POSITION_t gps_act_position;
extern GPS_ABS_POSITION_t gps_home_position;
extern GPS_REL_POSITION_t gps_rel_act_position;
extern GPS_REL_POSITION_t gps_rel_hold_position;
extern short int gps_state,gps_sub_state;
extern signed int GPS_hdng_abs_2trgt;
extern signed int GPS_hdng_rel_2trgt;
extern signed int GPS_dist_2trgt;
extern signed int gps_reg_x,gps_reg_y;
extern signed int GPS_dist_2trgt;
extern GPS_ABS_POSITION_t gps_act_position;
extern GPS_ABS_POSITION_t gps_home_position;
extern GPS_REL_POSITION_t gps_rel_act_position;
extern GPS_REL_POSITION_t gps_rel_hold_position;
extern short int gps_state,gps_sub_state;
extern signed int GPS_hdng_abs_2trgt;
extern signed int GPS_hdng_rel_2trgt;
extern signed int GPS_dist_2trgt;
extern signed int gps_reg_x,gps_reg_y;
extern signed int GPS_dist_2trgt;
extern unsigned int gps_alive_cnt;
 
//nur provisorisch fuer Debugausgaben
extern signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position
extern long int dist_flown;
extern signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position
extern long int dist_flown;
 
// Zustaende der zentralen GPS statemachine
#define GPS_CRTL_IDLE 0 //
91,7 → 91,9
#define GPS_HOME_FAST_IN_TOL 4 // Rueckflug: Aktuelle Position innerhalb der Toleranz
#define GPS_HOME_FAST_OUTOF_TOL 5 // Rueckflug: Aktuelle Position ausserhalb der Toleranz
#define GPS_HOME_RMPDWN_IN_TOL 6 // Rueckflug: beim Abbremsen Position innerhalb der Toleranz
#define GPS_HOME_RMPDWN_OUTOF_TOL 7 // Rueckflug: beim Abbremsen Position innerhalb der Toleranz
#define GPS_HOME_RMPDWN_OUTOF_TOL 7 // Rueckflug: beim Abbremsen Position ausserhalb der Toleranz
#define GPS_HOME_IN_TOL 8 // Rueckflug: Nahe am Ziel innerhalb der Toleranz
#define GPS_HOME_OUTOF_TOL 9 // Rueckflug: Nahe am Ziel ausserhalb der Toleranz
 
// Kommandokonstanten fuer die zentrale GPS Statemachine
#define GPS_CMD_REQ_INIT 0 // Initialisierung
109,13 → 111,20
#define GPS_DIST_MAX 400 // Maximal zulaessige Distanz bevor Regelung gestoppt wird (in 10cm)
#define GPS_STICK_HOLDOFF 25 // Wenn der Nick oder Roll Stickwerte groesser sind, wird GPS_HOLD deaktiviert
#define GPS_V 8 // Teilerfaktor Regelabweichung zu Ausgabewert
// Differenzierer Kennwerte fuer von Distanz abhaengige Verstaerkung.
#define GPS_DIFF_NRML_MAX_V 20 //maximale Verstaerkung * 10
#define GPS_DIFF_NRML_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm
#define GPS_DIFF_FAST_MAX_V 20 //maximale Verstaerkung * 10 im Fast mode
#define GPS_DIFF_FAST_MAX_D 40 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm im Fast mode
 
 
// GPS G2T /Go to Target Regler
#define GPS_G2T_DIST_MAX_STOP 60 // Ab dieser Entfernung vom Zielpunkt soll die Geschwindigkeit runtergeregelt werden( in 10 cm)
#define GPS_G2T_DIST_HOLD 30 // Ab dieser Entfernung vom Zielpunkt wird mit Minimaler geschwindigkeit eingeregelt
#define GPS_G2T_V_MAX 8 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird.
#define GPS_G2T_V_RAMP_DWN 4 // Geschwindigkeit (in 10cm/0.25ekunden) in der Nahe der Home Position um abzubremsen
#define GPS_G2T_V_MAX 10 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird.
#define GPS_G2T_V_RAMP_DWN 5 // Geschwindigkeit (in 10cm/0.25ekunden) in der Nahe der Home Position um abzubremsen
#define GPS_G2T_V_MIN 2 // Minimale (in 10cm/0.25 Sekunden) ganz nahe an Homeposition.
#define GPS_G2T_TOL 50 // Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht
#define GPS_G2T_FAST_TOL 80 // Bei grosser Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht
#define GPS_G2T_NRML_TOL 40 // Bei kleiner Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht
 
 
/branches/salvo_gps/math.c
5,7 → 5,7
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 and GNU Lesser General Public License for more details.
You should have received a copy of GNU General Public License ((License_GPL.txt) and
You should have received a copy of GNU General Public License (License_GPL.txt) and
GNU Lesser General Public License (License_LGPL.txt) along with this program.
If not, see <http://www.gnu.org/licenses/>.
 
15,7 → 15,7
Peter Muehlenbrock
Winkelfunktionen sin, cos und arctan in
brute-force Art: Sehr Schnell, nicht sonderlich genau, aber ausreichend
Sinus Funktion von Nick666 vereinfacht
get_dist Funktion fuer Entfernungsermittlung
Stand 1.10.2007
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
42,7 → 42,7
 
// Wert durch lineare Interpolation ermitteln
if ((y == 0) && (x == 0)) wert =1; // Division durch 0 nicht erlaubt
else wert= abs((x*1000)/y);
else wert= abs(((long) x*1000)/((long)y));
 
if (wert <=268) //0...0.0,268 entsprechend 0..15 Grad
{
108,7 → 108,7
}
 
// Aus x,y und Winkel Distanz ermitteln
long get_dist(signed int x, signed int y, signed int phi)
int get_dist(signed int x, signed int y, signed int phi)
{
long dist;
if (abs(x) > abs(y) )
121,6 → 121,6
dist = (long) y;
dist = abs((dist *1000) / (long) cos_i(phi));
}
return dist;
return (int)dist;
}
 
/branches/salvo_gps/math.h
8,4 → 8,4
extern signed int sin_i(signed int winkel);
extern signed int cos_i(signed int winkel);
extern signed int arctan_i(signed int x, signed int y);
extern long get_dist(signed int x, signed int y, signed int phi);
extern int get_dist(signed int x, signed int y, signed int phi);