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

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/GPS.c - Rev 266