WebSVN - FlightCtrl - Blame - Rev 242 - /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.
		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/>.
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
242	salvo	19	Stand 7.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
209	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;
225	salvo	50	short int gps_state,gps_sub_state; //Zustaende der Statemachine
209	salvo	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
224	salvo	59	signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position
		60	signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt
236	salvo	61	static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler
143	salvo	62	static uint8_t *ptr_payload_data;
		63	static uint8_t *ptr_pac_status;
225	salvo	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
224	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
		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);
		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
		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;
162	salvo	392	gps_state = GPS_CRTL_IDLE;
158	salvo	393	return (GPS_STST_OK);
		394	break;
		395
		396	default:
		397	return (GPS_STST_ERR);
		398	break;
		399	}
		400
		401	switch (gps_state)
		402	{
		403	case GPS_CRTL_IDLE:
		404	cnt = 0;
		405	return (GPS_STST_OK);
		406	break;
		407
224	salvo	408	case GPS_CRTL_HOME_ACTIVE: // Rueckflug zur Basis
		409	//Der Sollwert des Lagereglers wird der Homeposition angenaehert
		410	if (gps_rel_start_position.status >0)
		411	{
		412	if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist
		413	{
		414	gps_tick++;
		415	int d1,d2,d3;
225	salvo	416	d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );
		417	d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
224	salvo	418	d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel
		419
241	salvo	420	if (d3 > GPS_G2T_DIST_MAX_STOP)
224	salvo	421	{
241	salvo	422	hold_fast = 1; // Schnell Regeln
		423	hold_reset_int = 1; // Integrator ausschalten
224	salvo	424	if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
225	salvo	425	{
236	salvo	426	if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen
		427	dist_flown += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
225	salvo	428	dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
		429	dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
		430	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
		431	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
241	salvo	432	gps_sub_state = GPS_HOME_FAST_IN_TOL;
224	salvo	433	}
236	salvo	434	else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
		435	{
241	salvo	436	/* if (gps_sub_state == GPS_HOME_FAST_IN_TOL) hold_reset_int = 1; //Integrator einmal am Beginn des normalen Regelns resetten
		437	else hold_reset_int = 0;
		438	*/ if (gps_g2t_act_v > 0) gps_g2t_act_v--;
		439	gps_sub_state = GPS_HOME_FAST_OUTOF_TOL;
236	salvo	440	}
224	salvo	441	}
		442	else //Schon ziemlich nahe am Ziel, deswegen abbremsen
		443	{
		444	if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
		445	{
241	salvo	446	gps_sub_state = GPS_HOME_RMPDWN_IN_TOL;
225	salvo	447	if (d3 > GPS_G2T_DIST_HOLD)
224	salvo	448	{
236	salvo	449	if (gps_g2t_act_v < GPS_G2T_V_RAMP_DWN) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen
		450	dist_flown += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
224	salvo	451	dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000;
		452	dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000;
225	salvo	453	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt
224	salvo	454	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt
		455	}
225	salvo	456	else //Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt)
224	salvo	457	{
241	salvo	458	hold_fast = 0; // Wieder normal regeln
		459	hold_reset_int = 0; // Integrator wieder aktivieren
		460	if (gps_rel_hold_position.utm_east > GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN;
		461	else if (gps_rel_hold_position.utm_east < -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN;
		462	if (gps_rel_hold_position.utm_north > GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN;
		463	else if (gps_rel_hold_position.utm_north < - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN;
		464	if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN))
		465	{
		466	gps_rel_hold_position.utm_east = 0;
		467	gps_rel_hold_position.utm_north = 0;
		468	gps_sub_state = GPS_HOME_FINISHED;
		469	}
224	salvo	470	}
236	salvo	471	}
241	salvo	472	else
		473	{
		474	gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL;
236	salvo	475	if (gps_g2t_act_v > 0) gps_g2t_act_v--;
		476	}
224	salvo	477	}
		478	gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
		479	return (GPS_STST_OK);
		480	}
		481	else return (GPS_STST_OK);
		482	}
		483	else
		484	{
		485	gps_state = GPS_CRTL_IDLE; //Abbruch
		486	return (GPS_STST_ERR);
		487	}
		488	break;
		489
		490
236	salvo	491	case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet
224	salvo	492	if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen
158	salvo	493	{
		494	gps_updte_flag = 0;
		495	// ab hier wird geregelt
186	salvo	496	diff_east = -dist_east; //Alten Wert fuer Differenzier schon mal abziehen
		497	diff_north = -dist_north;
190	salvo	498	dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east;
184	salvo	499	dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
162	salvo	500	int_east += dist_east;
		501	int_north += dist_north;
167	salvo	502	diff_east += dist_east; // Differenz zur vorhergehenden East Position
		503	diff_north += dist_north; // Differenz zur vorhergehenden North Position
224	salvo	504	/*
194	salvo	505	diff_east_f = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern
		506	diff_north_f = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern
224	salvo	507	*/
236	salvo	508	#define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung
162	salvo	509	if ((abs(int_east) > GPSINT_MAX) \|\| (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten
		510	{
		511	int_east -= dist_east;
		512	int_north -= dist_north;
		513	}
236	salvo	514	if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten
225	salvo	515	{
		516	int_east = 0;
		517	int_north = 0;
		518	}
		519
209	salvo	520	// Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
		521	// desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
197	salvo	522	#define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10
209	salvo	523	#define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm
236	salvo	524	signed long dist,int_east1,int_north1;
197	salvo	525	int phi;
209	salvo	526	phi = arctan_i(abs(dist_north),abs(dist_east));
224	salvo	527	dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln
		528
197	salvo	529	diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10
209	salvo	530	if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen
241	salvo	531	// if (hold_fast > 0) diff_v = 10; //im schnellen Modus schwache Wirkung des Differenzierers
161	salvo	532
236	salvo	533	//I Werte begrenzen
		534	#define INT1_MAX (20 * GPS_V)
		535	int_east1 = ((((long)int_east) * Parameter_UserParam2)/32);
		536	int_north = ((((long)int_north) * Parameter_UserParam2)/32);
		537	if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen
		538	else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX;
		539	if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen
		540	else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX;
		541
241	salvo	542	int diff_p;
		543	if (hold_fast > 0) diff_p = 2; //im schnellen Modus Proportionalanteil staerken
		544	else diff_p = 1;
		545
186	salvo	546	//PID Regler Werte aufsummieren
241	salvo	547	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
		548	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	549
186	salvo	550	//Ziel-Richtung bezogen auf Nordpol bestimmen
194	salvo	551	GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y);
159	salvo	552
186	salvo	553	// in Winkel 0...360 Grad umrechnen
161	salvo	554	if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);
162	salvo	555	else GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);
161	salvo	556
159	salvo	557	// Relative Richtung in bezug auf Nordachse des Kopters errechen
		558	n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
		559	GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n;
167	salvo	560	if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360;
159	salvo	561	else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;
		562	else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;
		563
184	salvo	564	// Regelabweichung aus x,y zu Ziel in Distanz umrechnen
160	salvo	565	if (abs(gps_reg_x) > abs(gps_reg_y) )
		566	{
197	salvo	567	dev = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen
		568	dev = abs((dev *1000) / (long) sin_i(GPS_hdng_abs_2trgt));
160	salvo	569	}
		570	else
		571	{
197	salvo	572	dev = (long)gps_reg_y;
		573	dev = abs((dev *1000) / (long) cos_i(GPS_hdng_abs_2trgt));
160	salvo	574	}
197	salvo	575	GPS_dist_2trgt = (int) dev;
185	salvo	576	// Winkel und Distanz in Nick und Rollgroessen umrechnen
197	salvo	577	GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000);
		578	GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000);
190	salvo	579
184	salvo	580	if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V);
		581	else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V);
		582	if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);
		583	else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V);
		584
		585	//Kleine Werte verstaerken, Grosse abschwaechen
186	salvo	586	n = sin_i((GPS_Roll90)/(GPS_NICKROLL_MAX GPS_V));
		587	n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000;
		588	GPS_Roll = (int) n_l;
185	salvo	589	n = sin_i((GPS_Nick90)/(GPS_NICKROLL_MAX GPS_V));
186	salvo	590	n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000;
		591	GPS_Nick = (int) n_l;
184	salvo	592
		593	if ((abs(dist_east) > GPS_DIST_MAX) \|\| (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen
		594	{
		595	GPS_Roll = 0;
		596	GPS_Nick = 0;
		597	gps_state = GPS_CRTL_IDLE;
186	salvo	598	return (GPS_STST_ERR);
		599	break;
184	salvo	600	}
224	salvo	601	else
		602	{
		603	if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
		604	return (GPS_STST_OK);
		605	}
158	salvo	606	}
224	salvo	607	else
		608	{
		609	if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
		610	return (GPS_STST_OK);
		611	}
158	salvo	612	break;
		613
		614	default:
224	salvo	615	gps_state = GPS_CRTL_IDLE;
158	salvo	616	return (GPS_STST_ERR);
		617	break;
		618	}
		619	return (GPS_STST_ERR);
		620
		621	}
		622

Subversion Repositories FlightCtrl

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