WebSVN - FlightCtrl - Blame - Rev 619 - /branches/salvo_gps/Basis

Rev	Author	Line No.	Line
528	salvo	1	/*
		2	This program (files gps.c and gps.h) is free software; you can redistribute it and/or modify
		3	it under the terms of the GNU Lesser 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 and GNU Lesser General Public License for more details.
		8	You should have received a copy of GNU General Public License (License_GPL.txt) and
		9	GNU Lesser General Public License (License_LGPL.txt) along with this program.
		10	If not, see <http://www.gnu.org/licenses/>.
		11
		12	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
		13	*/
		14	/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		15	Peter Muehlenbrock
		16	Auswertung der Daten vom GPS im ublox Format
		17	Hold Modus mit PID Regler
		18	R�ckstuerz zur Basis Funktion
		19	Stand 24.10.2007
		20	Anederung: 24.10. Altitude in relativer Position jetzt auch drin
		21	++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		22	*/
		23	#include "main.h"
529	salvo	24	#include "math.h"
528	salvo	25	//#include "gps.h"
		26
		27	// Defines fuer ublox Messageformat um Auswertung zu steuern
		28	#define UBLOX_IDLE 0
		29	#define UBLOX_SYNC1 1
		30	#define UBLOX_SYNC2 2
		31	#define UBLOX_CLASS 3
		32	#define UBLOX_ID 4
		33	#define UBLOX_LEN1 5
		34	#define UBLOX_LEN2 6
		35	#define UBLOX_CKA 7
		36	#define UBLOX_CKB 8
		37	#define UBLOX_PAYLOAD 9
		38
		39	// ublox Protokoll Identifier
		40	#define UBLOX_NAV_POSUTM 0x08
		41	#define UBLOX_NAV_STATUS 0x03
		42	#define UBLOX_NAV_VELED 0x12
		43	#define UBLOX_NAV_CLASS 0x01
		44	#define UBLOX_SYNCH1_CHAR 0xB5
		45	#define UBLOX_SYNCH2_CHAR 0x62
		46
		47	signed int GPS_Nick = 0;
		48	signed int GPS_Roll = 0;
		49	short int ublox_msg_state = UBLOX_IDLE;
		50	static uint8_t chk_a =0; //Checksum
		51	static uint8_t chk_b =0;
		52	short int gps_state,gps_sub_state; //Zustaende der Statemachine
		53	short int gps_updte_flag;
		54	signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
		55	signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
		56	signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel
		57	signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;
		58	static unsigned int rx_len;
		59	static unsigned int ptr_payload_data_end;
		60	unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
		61	signed int hdng_2home,dist_2home; //Richtung und Entfernung zur home Position
		62	static signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt
		63	static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler
		64	static uint8_t *ptr_payload_data;
		65	static uint8_t *ptr_pac_status;
		66	long int dist_flown;
602	salvo	67	unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators
		68	signed int int_east,int_north; //Integrierer
613	salvo	69	signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert
615	salvo	70	//signed int diff_v;
613	salvo	71	signed long dist;
528	salvo	72
		73	short int Get_GPS_data(void);
		74
		75	NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt
		76	NAV_STATUS_t actual_status; // Aktueller Nav Status
		77	NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten
		78
		79	GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst
		80	GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt
		81	GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position
		82	GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode
		83	GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode
		84
		85	// Initialisierung
		86	void GPS_Neutral(void)
		87	{
		88	ublox_msg_state = UBLOX_IDLE;
		89	gps_state = GPS_CRTL_IDLE;
		90	gps_sub_state = GPS_CRTL_IDLE;
		91	actual_pos.status = 0;
		92	actual_speed.status = 0;
		93	actual_status.status = 0;
		94	gps_home_position.status = 0; // Noch keine gueltige Home Position
		95	gps_act_position.status = 0;
		96	gps_rel_act_position.status = 0;
		97	GPS_Nick = 0;
		98	GPS_Roll = 0;
		99	gps_updte_flag = 0;
		100	gps_int_x = 0;
		101	gps_int_y = 0;
		102	gps_alive_cnt = 0;
		103	}
		104
		105	// Home Position sichern falls Daten verfuegbar sind.
		106	void GPS_Save_Home(void)
		107	{
		108	short int n;
		109	n = Get_GPS_data();
		110	if (n == 0) // Gueltige und aktuelle Daten ?
		111	{
		112	// Neue GPS Daten liegen vor
		113	gps_home_position.utm_east = gps_act_position.utm_east;
		114	gps_home_position.utm_north = gps_act_position.utm_north;
		115	gps_home_position.utm_alt = gps_act_position.utm_alt;
		116	gps_home_position.status = 1; // Home Position gueltig
		117	}
		118	}
		119
		120	// Relative Position zur Home Position bestimmen
		121	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
		122	short int Get_Rel_Position(void)
		123	{
		124	short int n = 0;
		125	n = Get_GPS_data();
		126	if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
565	salvo	127	if (gps_alive_cnt < 1000) gps_alive_cnt += 600; // Timeoutzaehler. Wird in Motorregler Routine ueberwacht und dekrementiert
528	salvo	128	if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
		129	{
		130	gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east);
		131	gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north);
		132	gps_rel_act_position.utm_alt = (int) (gps_act_position.utm_alt - gps_home_position.utm_alt);
		133	gps_rel_act_position.status = 1; // gueltige Positionsdaten
		134	n = 0;
		135	gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen.
		136	}
		137	else
		138	{
		139	n = 2; //keine gueltigen Daten vorhanden
		140	gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.
		141	}
		142	return (n);
		143	}
		144
		145	// Daten aus aktuellen ublox Messages extrahieren
		146	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
		147	short int Get_GPS_data(void)
		148	{
		149	short int n = 1;
		150
		151	if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
		152	if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
		153	{
		154	if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
		155	{
565	salvo	156	actual_status.status = 0;
528	salvo	157	gps_act_position.utm_east = actual_pos.utm_east/10;
		158	gps_act_position.utm_north = actual_pos.utm_north/10;
		159	gps_act_position.utm_alt = actual_pos.utm_alt/10;
565	salvo	160	actual_pos.status = 0; //neue ublox Messages anfordern
		161	// gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
		162	// gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
		163	// gps_act_position.heading = actual_speed.heading/100000;
		164	actual_speed.status = 0;
528	salvo	165	gps_act_position.status = 1;
		166	n = 0; //Daten gueltig
		167	}
		168	else
		169	{
565	salvo	170	gps_act_position.status = 0; //Keine gueltigen Daten
		171	actual_speed.status = 0;
		172	actual_status.status = 0;
		173	actual_pos.status = 0; //neue ublox Messages anfordern
		174	n = 2;
528	salvo	175	}
		176	}
		177	return (n);
		178	}
		179
		180	/*
		181	Daten vom GPS im ublox MSG Format auswerten
		182	Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
		183	// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
		184	*/
		185	void Get_Ublox_Msg(uint8_t rx)
		186	{
		187	switch (ublox_msg_state)
		188	{
		189
		190	case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
		191	if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;
		192	else ublox_msg_state = UBLOX_IDLE;
		193	break;
		194
		195	case UBLOX_SYNC1:
		196
		197	if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;
		198	else ublox_msg_state = UBLOX_IDLE;
		199	chk_a = 0,chk_b = 0;
		200	break;
		201
		202	case UBLOX_SYNC2:
		203	if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;
		204	else ublox_msg_state = UBLOX_IDLE;
		205	break;
		206
		207	case UBLOX_CLASS: // Nur NAV Meldungen auswerten
		208	switch (rx)
		209	{
		210	case UBLOX_NAV_POSUTM:
		211	ptr_pac_status = &actual_pos.status;
		212	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
		213	else
		214	{
		215	ptr_payload_data = &actual_pos;
		216	ptr_payload_data_end = &actual_pos.status;
		217	ublox_msg_state = UBLOX_LEN1;
		218	}
		219	break;
		220
		221	case UBLOX_NAV_STATUS:
		222	ptr_pac_status = &actual_status.status;
		223	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
		224	else
		225	{
		226	ptr_payload_data = &actual_status;
		227	ptr_payload_data_end = &actual_status.status;
		228	ublox_msg_state = UBLOX_LEN1;
		229	}
		230	break;
		231
		232	case UBLOX_NAV_VELED:
		233	ptr_pac_status = &actual_speed.status;
		234	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
		235	else
		236	{
		237	ptr_payload_data = &actual_speed;
		238	ptr_payload_data_end = &actual_speed.status;
		239	ublox_msg_state = UBLOX_LEN1;
		240	}
		241	break;
		242
		243	default:
		244	ublox_msg_state = UBLOX_IDLE;
		245	break;
		246	}
		247	chk_a = UBLOX_NAV_CLASS + rx;
		248	chk_b = UBLOX_NAV_CLASS + chk_a;
		249	break;
		250
		251	case UBLOX_LEN1: // Laenge auswerten
		252	rx_len = rx;
		253	chk_a += rx;
		254	chk_b += chk_a;
		255	ublox_msg_state = UBLOX_LEN2;
		256	break;
		257
		258
		259	case UBLOX_LEN2: // Laenge auswerten
		260	rx_len = rx_len + (rx *256); // Laenge ermitteln
		261	chk_a += rx;
		262	chk_b += chk_a;
		263	ublox_msg_state = UBLOX_PAYLOAD;
		264	break;
		265
		266	case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen
		267	if (rx_len > 0)
		268	{
		269	*ptr_payload_data = rx;
		270	chk_a += rx;
		271	chk_b += chk_a;
		272	--rx_len;
		273	if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))
		274	{
		275	ptr_payload_data++;
		276	ublox_msg_state = UBLOX_PAYLOAD;
		277	}
		278	else ublox_msg_state = UBLOX_CKA;
		279	}
		280	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
		281	break;
		282
		283	case UBLOX_CKA: // Checksum pruefen
		284	if (rx == chk_a) ublox_msg_state = UBLOX_CKB;
		285	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
		286	break;
		287
		288	case UBLOX_CKB: // Checksum pruefen
		289	if (rx == chk_b) *ptr_pac_status = 1; // Paket ok
		290	ublox_msg_state = UBLOX_IDLE;
		291	break;
		292
		293	default:
		294	ublox_msg_state = UBLOX_IDLE;
		295	break;
		296	}
		297	}
		298
		299	//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
		300	short int GPS_CRTL(short int cmd)
		301	{
		302	static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen
		303	static signed int dist_north,dist_east;
		304	static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert)
613	salvo	305	signed int n;
528	salvo	306	static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion
		307	long signed int dev,n_l;
		308	signed int dist_frm_start_east,dist_frm_start_north;
615	salvo	309	int diff_v_east,diff_v_north; //Verstaerkungsfaktoren fuer Differenzierer
602	salvo	310
528	salvo	311	switch (cmd)
		312	{
		313
		314	case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden.
		315	if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))
		316	{
		317	cnt++;
565	salvo	318	if (cnt > 200) // erst nach Verzoegerung
528	salvo	319	{
		320	// Erst mal initialisieren
		321	cnt = 0;
		322	gps_tick = 0;
		323	hold_fast = 0;
		324	hold_reset_int = 0; // Integrator enablen
		325	int_east = 0, int_north = 0;
		326	gps_reg_x = 0, gps_reg_y = 0;
		327	dist_east = 0, dist_north = 0;
		328	diff_east_f = 0, diff_north_f= 0;
		329	diff_east = 0, diff_north = 0;
		330	dist_flown = 0;
		331	gps_g2t_act_v = 0;
		332	gps_sub_state = GPS_CRTL_IDLE;
		333	// aktuelle positionsdaten abspeichern
		334	if (gps_rel_act_position.status > 0)
		335	{
		336	gps_rel_start_position.utm_east = gps_rel_act_position.utm_east;
		337	gps_rel_start_position.utm_north= gps_rel_act_position.utm_north;
		338	gps_rel_start_position.status = 1; // gueltige Positionsdaten
		339	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
		340	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
		341	gps_rel_hold_position.status = 1; // gueltige Positionsdaten
		342	//Richtung zur Home Position bezogen auf Nordpol bestimmen
		343	hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north);
		344	// in Winkel 0...360 Grad umrechnen
		345	if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home);
		346	else hdng_2home = (270 - hdng_2home);
		347	dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen
		348	gps_state = GPS_CRTL_HOME_ACTIVE;
		349	return (GPS_STST_OK);
		350	}
		351	else
		352	{
		353	gps_rel_start_position.status = 0; //Keine Daten verfuegbar
		354	gps_state = GPS_CRTL_IDLE;
		355	return(GPS_STST_ERR); // Keine Daten da
		356	}
		357	}
		358	else return(GPS_STST_PEND); // noch warten
		359	}
		360	break;
		361	// ******************************
		362
		363	case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
		364	if (gps_state != GPS_CRTL_HOLD_ACTIVE)
		365	{
		366	cnt++;
565	salvo	367	if (cnt > 400) // erst nach Verzoegerung
528	salvo	368	{
		369	cnt = 0;
		370	// aktuelle positionsdaten abspeichern
		371	if (gps_rel_act_position.status > 0)
		372	{
		373	hold_fast = 0;
		374	hold_reset_int = 0; // Integrator enablen
		375	int_east = 0, int_north = 0;
		376	gps_reg_x = 0, gps_reg_y = 0;
		377	dist_east = 0, dist_north = 0;
		378	diff_east_f = 0, diff_north_f= 0;
		379	diff_east = 0, diff_north = 0;
602	salvo	380	int_ovfl_cnt = 0;
528	salvo	381	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
		382	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
		383	gps_rel_hold_position.status = 1; // gueltige Positionsdaten
		384	gps_state = GPS_CRTL_HOLD_ACTIVE;
		385	return (GPS_STST_OK);
		386	}
		387	else
		388	{
		389	gps_rel_hold_position.status = 0; //Keine Daten verfuegbar
		390	gps_state = GPS_CRTL_IDLE;
		391	return(GPS_STST_ERR); // Keine Daten da
		392	}
		393	}
		394	else return(GPS_STST_PEND); // noch warten
		395	}
		396	break;
		397
		398	case GPS_CMD_STOP: // Lageregelung beenden
		399	cnt = 0;
		400	GPS_Nick = 0;
		401	GPS_Roll = 0;
		402	gps_int_x = 0;
		403	gps_int_y = 0;
		404	gps_sub_state = GPS_CRTL_IDLE;
		405	gps_state = GPS_CRTL_IDLE;
		406	return (GPS_STST_OK);
		407	break;
		408
		409	default:
		410	return (GPS_STST_ERR);
		411	break;
		412	}
		413
		414	switch (gps_state)
		415	{
		416	case GPS_CRTL_IDLE:
		417	cnt = 0;
		418	return (GPS_STST_OK);
		419	break;
		420
		421	case GPS_CRTL_HOME_ACTIVE: // Rueckflug zur Basis
		422	//Der Sollwert des Lagereglers wird der Homeposition angenaehert
		423	if (gps_rel_start_position.status >0)
		424	{
		425	if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist
		426	{
		427	gps_tick++;
		428	int d1,d2,d3;
		429	d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );
		430	d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
		431	d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel
		432
		433	if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel
		434	{
		435	if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
		436	{
		437	if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen
		438	dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
		439	gps_sub_state = GPS_HOME_FAST_IN_TOL;
		440	}
		441	else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
		442	{
		443	if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren
		444	dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen
		445	gps_sub_state = GPS_HOME_FAST_OUTOF_TOL;
		446	}
		447	hold_reset_int = 0; // Integrator einsschalten
		448	hold_fast = 1; // Regler fuer schnellen Flug
		449	dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
		450	dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
		451	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
		452	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
		453	}
		454	else if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen
		455	{
		456	if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL))
		457	{
		458	dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
		459	gps_sub_state = GPS_HOME_RMPDWN_IN_TOL;
		460	}
		461	else
		462	{
		463	dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen
		464	gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL;
		465	}
		466	hold_reset_int = 0; // Integrator ausschalten
		467	hold_fast = 1; // Wieder normal regeln
		468	dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
		469	dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
		470	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
		471	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
		472	}
		473	else //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt)
		474	{
		475	if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln
		476	{
		477	gps_sub_state = GPS_HOME_IN_TOL;
		478	hold_fast = 0; // Wieder normal regeln
		479	hold_reset_int = 0; // Integrator wieder aktivieren
		480	if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN;
		481	else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN;
		482	if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN;
		483	else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN;
		484	if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN))
		485	{
		486	gps_rel_hold_position.utm_east = 0;
		487	gps_rel_hold_position.utm_north = 0;
		488	gps_sub_state = GPS_HOME_FINISHED;
		489	}
		490	}
		491	else gps_sub_state = GPS_HOME_OUTOF_TOL;
		492	}
		493	}
		494	gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
		495	return (GPS_STST_OK);
		496	}
		497	else // Keine GPS Daten verfuegbar, deswegen Abbruch
		498	{
		499	gps_state = GPS_CRTL_IDLE;
		500	return (GPS_STST_ERR);
		501	}
		502	break;
		503
		504
		505	case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet
		506	if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen
		507	{
		508	gps_updte_flag = 0;
		509	// ab hier wird geregelt
602	salvo	510	diff_east = -dist_east; //Alten Wert fuer Differenzierer schon mal abziehen
528	salvo	511	diff_north = -dist_north;
		512	dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east;
		513	dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
		514	int_east += dist_east;
		515	int_north += dist_north;
		516	diff_east += dist_east; // Differenz zur vorhergehenden East Position
		517	diff_north += dist_north; // Differenz zur vorhergehenden North Position
		518
		519	if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen
		520	{
615	salvo	521	diff_east_f = (((diff_east_f 2)/3) + ((diff_east 1*10)/3)); //Differenzierer filtern
		522	diff_north_f = (((diff_north_f 2)/3) + ((diff_north1*10)/3)); //Differenzierer filtern
528	salvo	523	}
		524	else // schwache Filterung
		525	{
616	salvo	526	diff_east_f = (((diff_east_f * 3)/4) + ((diff_east 110)/4)); //Differenzierer filtern
		527	diff_north_f = (((diff_north_f * 3)/4) + ((diff_north110)/4)); //Differenzierer filtern
528	salvo	528	}
		529
602	salvo	530	#define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow
		531	if ((abs(int_east) > GPSINT_MAX) \|\| (abs(int_north)> GPSINT_MAX))
528	salvo	532	{
602	salvo	533	if (int_ovfl_cnt < 40) int_ovfl_cnt += 20; // Zahl der Overflows zaehlen
		534	// int_east -= dist_east; auf alten Wert halten
		535	// int_north -= dist_north;
528	salvo	536	}
602	salvo	537	if (int_ovfl_cnt > 0) //bei Overflow Wert Inetgratorwert reduzieren
		538	{
		539	int_ovfl_cnt -= 1;
		540	int_east = (int_east*7)/8; // Wert reduzieren
		541	int_north = (int_north*7)/8;
		542	}
		543
528	salvo	544	if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten
		545	{
		546	int_east = 0;
		547	int_north = 0;
		548	}
		549
		550	// Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
		551	// desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
613	salvo	552	signed long int_east1,int_north1;
		553
528	salvo	554	int phi;
		555	phi = arctan_i(abs(dist_north),abs(dist_east));
		556	dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln
		557
		558	if (hold_fast == 0) // je Nach Modus andere Verstaerkungskurve fuer Differenzierer
		559	{
613	salvo	560	// diff_v = (int)((dist * (GPS_DIFF_NRML_MAX_V - 10)) / GPS_DIFF_NRML_MAX_D) +10; //Verstaerkung * 10
		561	// if (diff_v > GPS_DIFF_NRML_MAX_V) diff_v = GPS_DIFF_NRML_MAX_V; //begrenzen
615	salvo	562	// diff_v = GPS_DIFF_NRML_MAX_V ; //variable Versterkung raus 31.12.2007
528	salvo	563	}
		564	else
		565	{
613	salvo	566	// diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10
		567	// if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen
615	salvo	568	// diff_v = GPS_DIFF_FAST_MAX_V ; //variable Versterkung raus 31.12.2007
528	salvo	569	}
615	salvo	570	diff_v_east = (((abs(diff_east_f)) *(DIFF_Y_MAX-1))/DIFF_X_MAX) +10;
		571	diff_v_north = (((abs(diff_north_f))*(DIFF_Y_MAX-1))/DIFF_X_MAX) +10;
		572	if (diff_v_east > (DIFF_Y_MAX 10)) diff_v_east = DIFF_Y_MAX 10; // Begrenzung
		573	if (diff_v_north > (DIFF_Y_MAX 10)) diff_v_north = DIFF_Y_MAX 10; // Begrenzung
		574	diff_v_east *= 2;
		575	diff_v_north *= 2;
528	salvo	576
616	salvo	577	// debug_gp_2 = diff_v_east; // zum Debuggen
		578	// debug_gp_3 = diff_v_north; // zum Debuggen
		579	// debug_gp_4 = diff_east_f; // zum Debuggen
		580	// debug_gp_5 = diff_north_f; // zum Debuggen
613	salvo	581
528	salvo	582	int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung
		583	if (hold_fast > 0) diff_p = GPS_PROP_FAST_V;
		584	else diff_p = GPS_PROP_NRML_V;
		585
		586	//I Werte begrenzen
602	salvo	587	#define INT1_MAX (GPS_NICKROLL_MAX * GPS_V)/2 //ab 30.12.2007 auf Halben Maximalen Nick/Rollwert begrenzen
565	salvo	588	int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT;
		589	int_north1 = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; //Fehler behoben am 17.12.2007 vorher int_north=
528	salvo	590	if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen
		591	else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX;
		592	if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen
		593	else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX;
		594
		595	//PID Regler Werte aufsummieren
615	salvo	596	gps_reg_x = ((int)int_east1 + ((dist_east * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(82))+ ((diff_east_f diff_v_east * (Parameter_UserParam3/GPS_USR_PAR_FKT))/100)); // I + P +D Anteil X Achse
		597	gps_reg_y = ((int)int_north1 + ((dist_north * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(82))+ ((diff_north_f diff_v_north * (Parameter_UserParam3/GPS_USR_PAR_FKT))/100)); // I + P +D Anteil Y Achse
		598	debug_gp_0 = gps_reg_x; // zum Debuggen
		599	debug_gp_1 = gps_reg_y; // zum Debuggen
528	salvo	600
		601	//Ziel-Richtung bezogen auf Nordpol bestimmen
		602	GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y);
		603
		604	// in Winkel 0...360 Grad umrechnen
		605	if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);
		606	else GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);
		607
		608	// Relative Richtung in bezug auf Nordachse des Kopters errechen
		609	n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
		610	GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n;
		611	if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360;
		612	else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;
		613	else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;
		614
		615	// Regelabweichung aus x,y zu Ziel in Distanz umrechnen
		616	if (abs(gps_reg_x) > abs(gps_reg_y) )
		617	{
		618	dev = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen
		619	dev = abs((dev *1000) / (long) sin_i(GPS_hdng_abs_2trgt));
		620	}
		621	else
		622	{
		623	dev = (long)gps_reg_y;
		624	dev = abs((dev *1000) / (long) cos_i(GPS_hdng_abs_2trgt));
		625	}
		626	GPS_dist_2trgt = (int) dev;
		627	// Winkel und Distanz in Nick und Rollgroessen umrechnen
		628	GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000);
		629	GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000);
		630
		631	if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V);
		632	else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V);
		633	if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);
		634	else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V);
		635
		636	//Kleine Werte verstaerken, Grosse abschwaechen
		637	n = sin_i((GPS_Roll90)/(GPS_NICKROLL_MAX GPS_V));
		638	n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000;
		639	GPS_Roll = (int) n_l;
		640	n = sin_i((GPS_Nick90)/(GPS_NICKROLL_MAX GPS_V));
		641	n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000;
		642	GPS_Nick = (int) n_l;
		643
		644	if ((abs(dist_east) > GPS_DIST_MAX) \|\| (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen
		645	{
		646	GPS_Roll = 0;
		647	GPS_Nick = 0;
		648	gps_state = GPS_CRTL_IDLE;
		649	return (GPS_STST_ERR);
		650	break;
		651	}
		652	else
		653	{
		654	if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
		655	return (GPS_STST_OK);
		656	}
		657	}
		658	else
		659	{
		660	if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
		661	return (GPS_STST_OK);
		662	}
		663	break;
		664
		665	default:
		666	gps_state = GPS_CRTL_IDLE;
		667	return (GPS_STST_ERR);
		668	break;
		669	}
		670	return (GPS_STST_ERR);
		671
		672	}
		673

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0067g/trunk/GPS.c - Rev 619