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

Subversion Repositories FlightCtrl

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