WebSVN - FlightCtrl - Blame - /branches/salvo

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

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/GPS.c @ 259 - Rev