Subversion Repositories FlightCtrl

/*

This program (files gps.c and gps.h) is free software; you can redistribute it and/or modify

it under the terms of the GNU Lesser 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 and GNU Lesser General Public License for more details.

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/>.

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

Auswertung der Daten vom GPS im ublox Format

Hold Modus mit PID Regler

Rückstuerz zur Basis Funktion

Stand 24.10.2007

Anederung: 24.10. Altitude in relativer Position jetzt auch drin

++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

*/

#include "main.h"

#include "math.h"

//#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_PAYLOAD	9

// ublox Protokoll Identifier

#define			UBLOX_NAV_POSUTM		0x08

#define			UBLOX_NAV_STATUS		0x03

#define			UBLOX_NAV_VELED			0x12

#define			UBLOX_NAV_CLASS			0x01

#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;

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 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);

NAV_POSUTM_t actual_pos;    // Aktuelle Nav Daten werden hier im ublox Format abgelegt

NAV_STATUS_t actual_status; // Aktueller Nav Status

NAV_VELNED_t actual_speed;  // Aktueller Geschwindigkeits und Richtungsdaten

GPS_ABS_POSITION_t 		gps_act_position;  		// Alle wichtigen Daten zusammengefasst

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

// Initialisierung

void GPS_Neutral(void)

{

	ublox_msg_state 			= 	UBLOX_IDLE;

	gps_state					=	GPS_CRTL_IDLE;

	gps_sub_state				= 	GPS_CRTL_IDLE;

	actual_pos.status			= 	0;

	actual_speed.status			= 	0;

	actual_status.status		= 	0;

	gps_home_position.status	= 	0; // Noch keine gueltige Home Position

	gps_act_position.status		= 	0;

	gps_rel_act_position.status	= 	0;

	GPS_Nick					=	0;

	GPS_Roll					=	0;

	gps_updte_flag				=	0;

	gps_int_x					=	0;

	gps_int_y					=	0;

	gps_alive_cnt				=	0;

}

// Home Position sichern falls Daten verfuegbar sind.

void GPS_Save_Home(void)

{

	short int n;

	n = Get_GPS_data();

	if (n == 0)   // Gueltige  und aktuelle Daten ?

	{

		// Neue GPS Daten liegen vor

		gps_home_position.utm_east	= gps_act_position.utm_east;

		gps_home_position.utm_north	= gps_act_position.utm_north;

		gps_home_position.utm_alt	= gps_act_position.utm_alt;

		gps_home_position.status	= 1; // Home Position gueltig

	}

}

// Relative Position zur Home Position bestimmen

// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig

short int Get_Rel_Position(void)

{

	short int n = 0;

	n = Get_GPS_data();

	if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind

	if (gps_alive_cnt < 1000) gps_alive_cnt += 600; // Timeoutzaehler. Wird in Motorregler Routine ueberwacht und dekrementiert

	if  (gps_home_position.status > 0) //Nur wenn Home Position vorliegt

	{

		gps_rel_act_position.utm_east	= (int)  (gps_act_position.utm_east - gps_home_position.utm_east);

		gps_rel_act_position.utm_north	= (int)  (gps_act_position.utm_north - gps_home_position.utm_north);

		gps_rel_act_position.utm_alt	= (int)  (gps_act_position.utm_alt - gps_home_position.utm_alt);

		gps_rel_act_position.status 	= 1; // gueltige Positionsdaten

		n = 0;

		gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen.

	}

	else

	{

		n = 2; //keine gueltigen Daten vorhanden

		gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.

	}

	return (n);

}

// Daten aus aktuellen ublox Messages extrahieren

// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig

short int Get_GPS_data(void)

{

	short int n = 1;

	if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist

	if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))

	{

		if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind

		{

			actual_status.status 		= 0;

			gps_act_position.utm_east	= actual_pos.utm_east/10;

			gps_act_position.utm_north	= actual_pos.utm_north/10;

			gps_act_position.utm_alt	= actual_pos.utm_alt/10;

			actual_pos.status	 		= 0; //neue ublox Messages anfordern

//			gps_act_position.speed_gnd	= actual_speed.speed_gnd/10;

//			gps_act_position.speed_gnd	= actual_speed.speed_gnd/10;

//			gps_act_position.heading	= actual_speed.heading/100000;

			actual_speed.status 		= 0;

			gps_act_position.status		= 1;

			n							= 0; //Daten gueltig

		}

		else

		{

			gps_act_position.status 	= 0; //Keine gueltigen Daten

			actual_speed.status 		= 0;

			actual_status.status 		= 0;

			actual_pos.status	 		= 0; //neue ublox Messages anfordern

			n							= 2;

		}

	}

	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

// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein

*/

void Get_Ublox_Msg(uint8_t rx)

{

	switch (ublox_msg_state)

	{

		case UBLOX_IDLE: // Zuerst Synchcharacters pruefen

			if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;

			else ublox_msg_state = UBLOX_IDLE;

			break;

		case UBLOX_SYNC1:

			if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;

			else ublox_msg_state = UBLOX_IDLE;

			chk_a = 0,chk_b = 0;

 			break;

		case UBLOX_SYNC2:

			if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;

			else ublox_msg_state = UBLOX_IDLE;

			break;

		case UBLOX_CLASS: // Nur NAV Meldungen auswerten

			switch (rx)

			{

				case UBLOX_NAV_POSUTM:

					ptr_pac_status	=	&actual_pos.status;

					if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden

					else

					{

						ptr_payload_data		= &actual_pos;

						ptr_payload_data_end	= &actual_pos.status;

						ublox_msg_state 		= UBLOX_LEN1;

					}

					break;

				case UBLOX_NAV_STATUS:

					ptr_pac_status	=	&actual_status.status;

					if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;

					else

					{

						ptr_payload_data		= &actual_status;

						ptr_payload_data_end	= &actual_status.status;

						ublox_msg_state 		= UBLOX_LEN1;

					}

					break;

				case UBLOX_NAV_VELED:

					ptr_pac_status		=	&actual_speed.status;

					if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;

					else

					{

						ptr_payload_data		= &actual_speed;

						ptr_payload_data_end	= &actual_speed.status;

						ublox_msg_state 		= UBLOX_LEN1;

					}

					break;

				default:

					ublox_msg_state = UBLOX_IDLE;

					break;

			}

			chk_a	= UBLOX_NAV_CLASS + rx;

			chk_b	= UBLOX_NAV_CLASS + chk_a;

			break;

		case UBLOX_LEN1: // Laenge auswerten

			rx_len 	= rx;

			chk_a	+= rx;

			chk_b	+= chk_a;

			ublox_msg_state = UBLOX_LEN2;

			break;

		case UBLOX_LEN2: // Laenge auswerten

			rx_len = rx_len + (rx *256); // Laenge ermitteln

			chk_a	+= rx;

			chk_b	+= chk_a;

			ublox_msg_state = UBLOX_PAYLOAD;

			break;

		case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen

			if (rx_len > 0)

			{

				*ptr_payload_data = rx;

				chk_a	+= rx;

				chk_b	+= chk_a;

				--rx_len;

				if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))

				{

					ptr_payload_data++;

					ublox_msg_state	= UBLOX_PAYLOAD;

			    }

				else ublox_msg_state = UBLOX_CKA;

			}

			else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler

			break;

		case UBLOX_CKA: // Checksum pruefen

			if (rx == chk_a) ublox_msg_state = UBLOX_CKB;

			else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler

			break;

		case UBLOX_CKB: // Checksum pruefen

			if (rx == chk_b) *ptr_pac_status = 1; // Paket ok

			ublox_msg_state    = UBLOX_IDLE;

			break;

		default:

			ublox_msg_state = UBLOX_IDLE;

			break;

	}

}

//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe

short int GPS_CRTL(short int cmd)

{

	static unsigned int cnt;					//Zaehler fuer diverse Verzoegerungen

	static signed int int_east,int_north;	//Integrierer

	static signed int dist_north,dist_east;

	static signed int diff_east,diff_north;		// Differenzierer  (Differenz zum  vorhergehenden x bzw. y  Wert)

	static signed int diff_east_f,diff_north_f; // Differenzierer,  gefiltert

	signed int n,diff_v;

	static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion

	long signed int dev,n_l;

	signed int dist_frm_start_east,dist_frm_start_north;

	switch (cmd)

	{

		case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden.

			if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))

			{

				cnt++;

				if (cnt > 200) // erst nach Verzoegerung

				{

					// Erst mal initialisieren

					cnt				= 0;

					gps_tick		= 0;

					hold_fast		= 0;

					hold_reset_int	= 0; // Integrator enablen

					int_east		= 0, int_north	= 0;

					gps_reg_x		= 0, gps_reg_y	= 0;

					dist_east		= 0, dist_north	= 0;

					diff_east_f		= 0, diff_north_f= 0;

					diff_east		= 0, diff_north	 = 0;

					dist_flown		= 0;

					gps_g2t_act_v	= 0;

					gps_sub_state	= GPS_CRTL_IDLE;

					// aktuelle positionsdaten abspeichern

					if (gps_rel_act_position.status > 0)

					{

						gps_rel_start_position.utm_east	= gps_rel_act_position.utm_east;

						gps_rel_start_position.utm_north= gps_rel_act_position.utm_north;

						gps_rel_start_position.status 	= 1; // gueltige Positionsdaten

						gps_rel_hold_position.utm_east	= gps_rel_act_position.utm_east;

						gps_rel_hold_position.utm_north	= gps_rel_act_position.utm_north;

						gps_rel_hold_position.status 	= 1; // gueltige Positionsdaten

						//Richtung zur Home Position bezogen auf Nordpol bestimmen

						hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north);

						// 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

						gps_state	= GPS_CRTL_HOME_ACTIVE;

						return (GPS_STST_OK);

					}

					else

					{

						gps_rel_start_position.status 	=	0;  //Keine Daten verfuegbar

						gps_state						= GPS_CRTL_IDLE;

						return(GPS_STST_ERR); // Keine Daten da

					}

				}

				else return(GPS_STST_PEND); // noch warten

			}

		   break;

// ******************************

		case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.

			if (gps_state != GPS_CRTL_HOLD_ACTIVE)

			{

				cnt++;

				if (cnt > 400) // erst nach Verzoegerung

				{

					cnt	=	0;

					// aktuelle positionsdaten abspeichern

					if (gps_rel_act_position.status > 0)

					{

						hold_fast		= 0;

						hold_reset_int	= 0; // Integrator enablen

						int_east	= 0, int_north	= 0;

						gps_reg_x	= 0, gps_reg_y	= 0;

						dist_east	= 0, dist_north	= 0;

						diff_east_f	= 0, diff_north_f= 0;

						diff_east	= 0, diff_north	 = 0;

						gps_rel_hold_position.utm_east	= gps_rel_act_position.utm_east;

						gps_rel_hold_position.utm_north	= gps_rel_act_position.utm_north;

						gps_rel_hold_position.status 	= 1; // gueltige Positionsdaten

						gps_state						= GPS_CRTL_HOLD_ACTIVE;

						return (GPS_STST_OK);

					}

					else

					{

						gps_rel_hold_position.status 	=	0;  //Keine Daten verfuegbar

						gps_state						= GPS_CRTL_IDLE;

						return(GPS_STST_ERR); // Keine Daten da

					}

				}

				else return(GPS_STST_PEND); // noch warten

			}

			break;

		case GPS_CMD_STOP: // Lageregelung beenden

			cnt				=	0;

			GPS_Nick		=	0;

			GPS_Roll		=	0;

			gps_int_x		=	0;

			gps_int_y		=	0;

			gps_sub_state	= 	GPS_CRTL_IDLE;

			gps_state		=	GPS_CRTL_IDLE;

			return (GPS_STST_OK);

			break;

		default:

			return (GPS_STST_ERR);

			break;

	}

	switch (gps_state)

	{

		case GPS_CRTL_IDLE:

			cnt		=	0;

			return (GPS_STST_OK);

			break;

		case GPS_CRTL_HOME_ACTIVE: // Rueckflug zur Basis

		//Der Sollwert des Lagereglers wird der Homeposition angenaehert

			if (gps_rel_start_position.status >0)

			{

				if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist

				{

					gps_tick++;

					int d1,d2,d3;

					d1 	= abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );

					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) // Schneller Rueckflug, noch weit weg vom Ziel

					{

						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

							gps_sub_state	= GPS_HOME_FAST_IN_TOL;

						}

						else	//Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz

						{

							if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren

							dist_flown++; 							//Auch ausserhalb der Toleranz langsam erhoehen

							gps_sub_state	= GPS_HOME_FAST_OUTOF_TOL;

						}

						hold_reset_int 					= 0; // Integrator einsschalten

						hold_fast 						= 1; // Regler fuer schnellen Flug

						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

					}

					else if (d3 > GPS_G2T_DIST_HOLD)   //Das Ziel naehert sich, deswegen abbremsen

					{

						if ((d1 < GPS_G2T_NRML_TOL)  && (d2 < GPS_G2T_NRML_TOL))

						{

							dist_flown 		+=	GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit

							gps_sub_state	= 	GPS_HOME_RMPDWN_IN_TOL;

						}

						else

						{

							dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen

							gps_sub_state	= GPS_HOME_RMPDWN_OUTOF_TOL;

						}

						hold_reset_int 					= 0; // Integrator ausschalten

						hold_fast 						= 1; // Wieder normal regeln

						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

					}

					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))

							{

								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_OUTOF_TOL;

					}

				}

				gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung

				return (GPS_STST_OK);

			}

			else  // Keine GPS Daten verfuegbar, deswegen Abbruch

			{

				gps_state	=	GPS_CRTL_IDLE;

				return (GPS_STST_ERR);

			}

			break;

		case GPS_CRTL_HOLD_ACTIVE:  // Hier werden die Daten fuer Nick und Roll errechnet

			if (gps_updte_flag >0)  // nur wenn neue GPS Daten vorliegen

			{

				gps_updte_flag = 0;

				// ab hier wird geregelt

				diff_east 	= -dist_east;	  //Alten Wert fuer Differenzier schon mal abziehen

				diff_north	= -dist_north;

				dist_east	= gps_rel_hold_position.utm_east  - gps_rel_act_position.utm_east;

				dist_north	= gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;

				int_east	+= dist_east;

				int_north   += dist_north;

				diff_east	+= dist_east;	// Differenz zur vorhergehenden East Position

				diff_north	+= dist_north;	// Differenz zur vorhergehenden North Position

				if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen

				{

					diff_east_f		= ((diff_east_f	 *2)/3) + (diff_east *1/3); //Differenzierer filtern

					diff_north_f	= ((diff_north_f *2)/3) + (diff_north*1/3); //Differenzierer filtern

				}

				else // schwache Filterung

				{

					diff_east_f		= ((diff_east_f	 * 1)/4) + ((diff_east *3)/4); //Differenzierer filtern

					diff_north_f	= ((diff_north_f * 1)/4) + ((diff_north*3)/4); //Differenzierer filtern

				}

				#define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung

				if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten

				{

					int_east	-= dist_east;

					int_north   -= dist_north;

				}

				if (hold_reset_int > 0)  //Im Schnellen Mode Integrator abschalten

				{

					int_east 	= 0;

					int_north 	= 0;

				}

				// 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

				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

				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

				}

				int diff_p;  //Vom Modus abhaengige zusaetzliche Verstaerkung

				if (hold_fast > 0) diff_p = GPS_PROP_FAST_V;

				else diff_p = GPS_PROP_NRML_V;

				//I Werte begrenzen

				#define INT1_MAX (20 * GPS_V)

				int_east1  =  ((((long)int_east)   * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT;

				int_north1 =  ((((long)int_north)  * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT;  //Fehler behoben am 17.12.2007 vorher int_north=

				if (int_east1 > INT1_MAX) int_east1 =  INT1_MAX; //begrenzen

				else if (int_east1 < -INT1_MAX) int_east1 =  -INT1_MAX;

				if (int_north1 > INT1_MAX) int_north1 =  INT1_MAX; //begrenzen

				else if (int_north1 < -INT1_MAX) int_north1 =  -INT1_MAX;

				//PID Regler Werte aufsummieren

				gps_reg_x = ((int)int_east1  + ((dist_east  * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((diff_east_f  * diff_v * (Parameter_UserParam3/GPS_USR_PAR_FKT))/10));  // I + P +D  Anteil X Achse

				gps_reg_y = ((int)int_north1 + ((dist_north * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((diff_north_f * diff_v * (Parameter_UserParam3/GPS_USR_PAR_FKT))/10));  // I + P +D  Anteil Y Achse

				//Ziel-Richtung bezogen auf Nordpol bestimmen

				GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y);

				// in Winkel 0...360 Grad umrechnen

				if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);

				else  GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);

				// Relative Richtung in bezug auf Nordachse des Kopters errechen

				n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;

				GPS_hdng_rel_2trgt	=	GPS_hdng_abs_2trgt - n;

				if 	((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360;

				else if (GPS_hdng_rel_2trgt >180)  GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;

				else if (GPS_hdng_rel_2trgt <-180)  GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;

				// Regelabweichung aus x,y zu Ziel in Distanz umrechnen

				if (abs(gps_reg_x) > abs(gps_reg_y) )

				{

					dev = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen

					dev = abs((dev *1000) / (long) sin_i(GPS_hdng_abs_2trgt));

				}

				else

				{

					dev = (long)gps_reg_y;

					dev = abs((dev *1000) / (long) cos_i(GPS_hdng_abs_2trgt));

				}

				GPS_dist_2trgt	= (int) dev;

				// Winkel und Distanz in Nick und Rollgroessen umrechnen

				GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000);

				GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000);

				if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V);

				else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V);

				if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);

				else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V);

				//Kleine Werte verstaerken, Grosse abschwaechen

				n 			= sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V));

				n_l			= ((long) GPS_NICKROLL_MAX  * (long) n)/1000;

				GPS_Roll	= (int) n_l;

				n 			= sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V));

				n_l			= ((long) GPS_NICKROLL_MAX  * (long) n)/1000;

				GPS_Nick	= (int) n_l;

				if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX))  // bei zu grossem Abstand abbrechen

				{

					GPS_Roll 	= 0;

					GPS_Nick 	= 0;

					gps_state	= GPS_CRTL_IDLE;

					return (GPS_STST_ERR);

					break;

				}

				else

				{

					if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen

					return (GPS_STST_OK);

				}

			}

			else

			{

				if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen

				return (GPS_STST_OK);

			}

			break;

		default:

			gps_state = GPS_CRTL_IDLE;

			return (GPS_STST_ERR);

			break;

	}

	return (GPS_STST_ERR);

}