WebSVN - FlightCtrl - Blame - Rev 613 - /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
		70	signed int diff_v;
		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;
602	salvo	309
528	salvo	310	switch (cmd)
		311	{
		312
		313	case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden.
		314	if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))
		315	{
		316	cnt++;
565	salvo	317	if (cnt > 200) // erst nach Verzoegerung
528	salvo	318	{
		319	// Erst mal initialisieren
		320	cnt = 0;
		321	gps_tick = 0;
		322	hold_fast = 0;
		323	hold_reset_int = 0; // Integrator enablen
		324	int_east = 0, int_north = 0;
		325	gps_reg_x = 0, gps_reg_y = 0;
		326	dist_east = 0, dist_north = 0;
		327	diff_east_f = 0, diff_north_f= 0;
		328	diff_east = 0, diff_north = 0;
		329	dist_flown = 0;
		330	gps_g2t_act_v = 0;
		331	gps_sub_state = GPS_CRTL_IDLE;
		332	// aktuelle positionsdaten abspeichern
		333	if (gps_rel_act_position.status > 0)
		334	{
		335	gps_rel_start_position.utm_east = gps_rel_act_position.utm_east;
		336	gps_rel_start_position.utm_north= gps_rel_act_position.utm_north;
		337	gps_rel_start_position.status = 1; // gueltige Positionsdaten
		338	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
		339	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
		340	gps_rel_hold_position.status = 1; // gueltige Positionsdaten
		341	//Richtung zur Home Position bezogen auf Nordpol bestimmen
		342	hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north);
		343	// in Winkel 0...360 Grad umrechnen
		344	if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home);
		345	else hdng_2home = (270 - hdng_2home);
		346	dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen
		347	gps_state = GPS_CRTL_HOME_ACTIVE;
		348	return (GPS_STST_OK);
		349	}
		350	else
		351	{
		352	gps_rel_start_position.status = 0; //Keine Daten verfuegbar
		353	gps_state = GPS_CRTL_IDLE;
		354	return(GPS_STST_ERR); // Keine Daten da
		355	}
		356	}
		357	else return(GPS_STST_PEND); // noch warten
		358	}
		359	break;
		360	// ******************************
		361
		362	case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
		363	if (gps_state != GPS_CRTL_HOLD_ACTIVE)
		364	{
		365	cnt++;
565	salvo	366	if (cnt > 400) // erst nach Verzoegerung
528	salvo	367	{
		368	cnt = 0;
		369	// aktuelle positionsdaten abspeichern
		370	if (gps_rel_act_position.status > 0)
		371	{
		372	hold_fast = 0;
		373	hold_reset_int = 0; // Integrator enablen
		374	int_east = 0, int_north = 0;
		375	gps_reg_x = 0, gps_reg_y = 0;
		376	dist_east = 0, dist_north = 0;
		377	diff_east_f = 0, diff_north_f= 0;
		378	diff_east = 0, diff_north = 0;
602	salvo	379	int_ovfl_cnt = 0;
528	salvo	380	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
		381	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
		382	gps_rel_hold_position.status = 1; // gueltige Positionsdaten
		383	gps_state = GPS_CRTL_HOLD_ACTIVE;
		384	return (GPS_STST_OK);
		385	}
		386	else
		387	{
		388	gps_rel_hold_position.status = 0; //Keine Daten verfuegbar
		389	gps_state = GPS_CRTL_IDLE;
		390	return(GPS_STST_ERR); // Keine Daten da
		391	}
		392	}
		393	else return(GPS_STST_PEND); // noch warten
		394	}
		395	break;
		396
		397	case GPS_CMD_STOP: // Lageregelung beenden
		398	cnt = 0;
		399	GPS_Nick = 0;
		400	GPS_Roll = 0;
		401	gps_int_x = 0;
		402	gps_int_y = 0;
		403	gps_sub_state = GPS_CRTL_IDLE;
		404	gps_state = GPS_CRTL_IDLE;
		405	return (GPS_STST_OK);
		406	break;
		407
		408	default:
		409	return (GPS_STST_ERR);
		410	break;
		411	}
		412
		413	switch (gps_state)
		414	{
		415	case GPS_CRTL_IDLE:
		416	cnt = 0;
		417	return (GPS_STST_OK);
		418	break;
		419
		420	case GPS_CRTL_HOME_ACTIVE: // Rueckflug zur Basis
		421	//Der Sollwert des Lagereglers wird der Homeposition angenaehert
		422	if (gps_rel_start_position.status >0)
		423	{
		424	if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist
		425	{
		426	gps_tick++;
		427	int d1,d2,d3;
		428	d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );
		429	d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
		430	d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel
		431
		432	if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel
		433	{
		434	if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
		435	{
		436	if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen
		437	dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
		438	gps_sub_state = GPS_HOME_FAST_IN_TOL;
		439	}
		440	else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
		441	{
		442	if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren
		443	dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen
		444	gps_sub_state = GPS_HOME_FAST_OUTOF_TOL;
		445	}
		446	hold_reset_int = 0; // Integrator einsschalten
		447	hold_fast = 1; // Regler fuer schnellen Flug
		448	dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
		449	dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
		450	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
		451	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
		452	}
		453	else if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen
		454	{
		455	if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL))
		456	{
		457	dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
		458	gps_sub_state = GPS_HOME_RMPDWN_IN_TOL;
		459	}
		460	else
		461	{
		462	dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen
		463	gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL;
		464	}
		465	hold_reset_int = 0; // Integrator ausschalten
		466	hold_fast = 1; // Wieder normal regeln
		467	dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
		468	dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
		469	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
		470	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
		471	}
		472	else //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt)
		473	{
		474	if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln
		475	{
		476	gps_sub_state = GPS_HOME_IN_TOL;
		477	hold_fast = 0; // Wieder normal regeln
		478	hold_reset_int = 0; // Integrator wieder aktivieren
		479	if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN;
		480	else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN;
		481	if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN;
		482	else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN;
		483	if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN))
		484	{
		485	gps_rel_hold_position.utm_east = 0;
		486	gps_rel_hold_position.utm_north = 0;
		487	gps_sub_state = GPS_HOME_FINISHED;
		488	}
		489	}
		490	else gps_sub_state = GPS_HOME_OUTOF_TOL;
		491	}
		492	}
		493	gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
		494	return (GPS_STST_OK);
		495	}
		496	else // Keine GPS Daten verfuegbar, deswegen Abbruch
		497	{
		498	gps_state = GPS_CRTL_IDLE;
		499	return (GPS_STST_ERR);
		500	}
		501	break;
		502
		503
		504	case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet
		505	if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen
		506	{
		507	gps_updte_flag = 0;
		508	// ab hier wird geregelt
602	salvo	509	diff_east = -dist_east; //Alten Wert fuer Differenzierer schon mal abziehen
528	salvo	510	diff_north = -dist_north;
		511	dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east;
		512	dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
		513	int_east += dist_east;
		514	int_north += dist_north;
		515	diff_east += dist_east; // Differenz zur vorhergehenden East Position
		516	diff_north += dist_north; // Differenz zur vorhergehenden North Position
		517
		518	if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen
		519	{
		520	diff_east_f = ((diff_east_f 2)/3) + (diff_east 1/3); //Differenzierer filtern
		521	diff_north_f = ((diff_north_f 2)/3) + (diff_north1/3); //Differenzierer filtern
		522	}
		523	else // schwache Filterung
		524	{
613	salvo	525	diff_east_f = ((diff_east_f * 2)/4) + ((diff_east *2)/4); //Differenzierer filtern
		526	diff_north_f = ((diff_north_f * 2)/4) + ((diff_north*2)/4); //Differenzierer filtern
528	salvo	527	}
		528
602	salvo	529	#define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow
		530	if ((abs(int_east) > GPSINT_MAX) \|\| (abs(int_north)> GPSINT_MAX))
528	salvo	531	{
602	salvo	532	if (int_ovfl_cnt < 40) int_ovfl_cnt += 20; // Zahl der Overflows zaehlen
		533	// int_east -= dist_east; auf alten Wert halten
		534	// int_north -= dist_north;
528	salvo	535	}
602	salvo	536	if (int_ovfl_cnt > 0) //bei Overflow Wert Inetgratorwert reduzieren
		537	{
		538	int_ovfl_cnt -= 1;
		539	int_east = (int_east*7)/8; // Wert reduzieren
		540	int_north = (int_north*7)/8;
		541	}
		542
528	salvo	543	if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten
		544	{
		545	int_east = 0;
		546	int_north = 0;
		547	}
		548
602	salvo	549
528	salvo	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
		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
		568	diff_v = GPS_DIFF_FAST_MAX_V ; //variable Versterkung raus 31.12.2007
528	salvo	569	}
		570
613	salvo	571
528	salvo	572	int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung
		573	if (hold_fast > 0) diff_p = GPS_PROP_FAST_V;
		574	else diff_p = GPS_PROP_NRML_V;
		575
		576	//I Werte begrenzen
602	salvo	577	#define INT1_MAX (GPS_NICKROLL_MAX * GPS_V)/2 //ab 30.12.2007 auf Halben Maximalen Nick/Rollwert begrenzen
565	salvo	578	int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT;
		579	int_north1 = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; //Fehler behoben am 17.12.2007 vorher int_north=
528	salvo	580	if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen
		581	else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX;
		582	if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen
		583	else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX;
		584
		585	//PID Regler Werte aufsummieren
542	salvo	586	gps_reg_x = ((int)int_east1 + ((dist_east * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(82))+ ((diff_east_f diff_v * (Parameter_UserParam3/GPS_USR_PAR_FKT))/10)); // I + P +D Anteil X Achse
		587	gps_reg_y = ((int)int_north1 + ((dist_north * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(82))+ ((diff_north_f diff_v * (Parameter_UserParam3/GPS_USR_PAR_FKT))/10)); // I + P +D Anteil Y Achse
528	salvo	588
		589	//Ziel-Richtung bezogen auf Nordpol bestimmen
		590	GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y);
		591
		592	// in Winkel 0...360 Grad umrechnen
		593	if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);
		594	else GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);
		595
		596	// Relative Richtung in bezug auf Nordachse des Kopters errechen
		597	n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
		598	GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n;
		599	if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360;
		600	else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;
		601	else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;
		602
		603	// Regelabweichung aus x,y zu Ziel in Distanz umrechnen
		604	if (abs(gps_reg_x) > abs(gps_reg_y) )
		605	{
		606	dev = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen
		607	dev = abs((dev *1000) / (long) sin_i(GPS_hdng_abs_2trgt));
		608	}
		609	else
		610	{
		611	dev = (long)gps_reg_y;
		612	dev = abs((dev *1000) / (long) cos_i(GPS_hdng_abs_2trgt));
		613	}
		614	GPS_dist_2trgt = (int) dev;
		615	// Winkel und Distanz in Nick und Rollgroessen umrechnen
		616	GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000);
		617	GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000);
		618
		619	if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V);
		620	else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V);
		621	if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);
		622	else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V);
		623
		624	//Kleine Werte verstaerken, Grosse abschwaechen
		625	n = sin_i((GPS_Roll90)/(GPS_NICKROLL_MAX GPS_V));
		626	n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000;
		627	GPS_Roll = (int) n_l;
		628	n = sin_i((GPS_Nick90)/(GPS_NICKROLL_MAX GPS_V));
		629	n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000;
		630	GPS_Nick = (int) n_l;
		631
		632	if ((abs(dist_east) > GPS_DIST_MAX) \|\| (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen
		633	{
		634	GPS_Roll = 0;
		635	GPS_Nick = 0;
		636	gps_state = GPS_CRTL_IDLE;
		637	return (GPS_STST_ERR);
		638	break;
		639	}
		640	else
		641	{
		642	if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
		643	return (GPS_STST_OK);
		644	}
		645	}
		646	else
		647	{
		648	if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
		649	return (GPS_STST_OK);
		650	}
		651	break;
		652
		653	default:
		654	gps_state = GPS_CRTL_IDLE;
		655	return (GPS_STST_ERR);
		656	break;
		657	}
		658	return (GPS_STST_ERR);
		659
		660	}
		661

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0067g/trunk/GPS.c @ 633 - Rev 613