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
293	salvo	19	Stand 13.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
291	salvo	331	//Richtung zur Home Position bezogen auf Nordpol bestimmen
224	salvo	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);
291	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
291	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
291	salvo	506
		507	if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen
		508	{
293	salvo	509	diff_east_f = ((diff_east_f 2)/3) + (diff_east 1/3); //Differenzierer filtern
		510	diff_north_f = ((diff_north_f 2)/3) + (diff_north1/3); //Differenzierer filtern
291	salvo	511	}
		512	else // schwache Filterung
		513	{
293	salvo	514	diff_east_f = ((diff_east_f * 1)/4) + ((diff_east *3)/4); //Differenzierer filtern
		515	diff_north_f = ((diff_north_f * 1)/4) + ((diff_north*3)/4); //Differenzierer filtern
291	salvo	516	}
		517
236	salvo	518	#define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung
162	salvo	519	if ((abs(int_east) > GPSINT_MAX) \|\| (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten
		520	{
		521	int_east -= dist_east;
		522	int_north -= dist_north;
		523	}
236	salvo	524	if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten
225	salvo	525	{
		526	int_east = 0;
		527	int_north = 0;
		528	}
		529
209	salvo	530	// Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
		531	// desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
236	salvo	532	signed long dist,int_east1,int_north1;
197	salvo	533	int phi;
266	salvo	534	phi = arctan_i(abs(dist_north),abs(dist_east));
		535	dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln
224	salvo	536
258	salvo	537	if (hold_fast == 0) // je Nach Modus andere Verstaerkungskurve fuer Differenzierer
		538	{
		539	diff_v = (int)((dist * (GPS_DIFF_NRML_MAX_V - 10)) / GPS_DIFF_NRML_MAX_D) +10; //Verstaerkung * 10
		540	if (diff_v > GPS_DIFF_NRML_MAX_V) diff_v = GPS_DIFF_NRML_MAX_V; //begrenzen
		541	}
		542	else
		543	{
		544	diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10
		545	if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen
		546	}
161	salvo	547
266	salvo	548	int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung
		549	if (hold_fast > 0) diff_p = GPS_PROP_FAST_V;
		550	else diff_p = GPS_PROP_NRML_V;
		551
236	salvo	552	//I Werte begrenzen
		553	#define INT1_MAX (20 * GPS_V)
		554	int_east1 = ((((long)int_east) * Parameter_UserParam2)/32);
		555	int_north = ((((long)int_north) * Parameter_UserParam2)/32);
		556	if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen
		557	else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX;
		558	if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen
		559	else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX;
		560
186	salvo	561	//PID Regler Werte aufsummieren
291	salvo	562	gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1 * diff_p)/(82))+ ((diff_east_f diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse
		563	gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/(82))+ ((diff_north_f diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil Y Achse
159	salvo	564
186	salvo	565	//Ziel-Richtung bezogen auf Nordpol bestimmen
194	salvo	566	GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y);
159	salvo	567
186	salvo	568	// in Winkel 0...360 Grad umrechnen
161	salvo	569	if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);
162	salvo	570	else GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);
161	salvo	571
159	salvo	572	// Relative Richtung in bezug auf Nordachse des Kopters errechen
		573	n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
		574	GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n;
167	salvo	575	if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360;
159	salvo	576	else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;
		577	else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;
		578
184	salvo	579	// Regelabweichung aus x,y zu Ziel in Distanz umrechnen
160	salvo	580	if (abs(gps_reg_x) > abs(gps_reg_y) )
		581	{
197	salvo	582	dev = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen
		583	dev = abs((dev *1000) / (long) sin_i(GPS_hdng_abs_2trgt));
160	salvo	584	}
		585	else
		586	{
197	salvo	587	dev = (long)gps_reg_y;
		588	dev = abs((dev *1000) / (long) cos_i(GPS_hdng_abs_2trgt));
160	salvo	589	}
197	salvo	590	GPS_dist_2trgt = (int) dev;
185	salvo	591	// Winkel und Distanz in Nick und Rollgroessen umrechnen
197	salvo	592	GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000);
		593	GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000);
190	salvo	594
184	salvo	595	if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V);
		596	else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V);
		597	if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);
		598	else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V);
		599
		600	//Kleine Werte verstaerken, Grosse abschwaechen
186	salvo	601	n = sin_i((GPS_Roll90)/(GPS_NICKROLL_MAX GPS_V));
		602	n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000;
		603	GPS_Roll = (int) n_l;
185	salvo	604	n = sin_i((GPS_Nick90)/(GPS_NICKROLL_MAX GPS_V));
186	salvo	605	n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000;
		606	GPS_Nick = (int) n_l;
184	salvo	607
		608	if ((abs(dist_east) > GPS_DIST_MAX) \|\| (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen
		609	{
		610	GPS_Roll = 0;
		611	GPS_Nick = 0;
		612	gps_state = GPS_CRTL_IDLE;
186	salvo	613	return (GPS_STST_ERR);
		614	break;
184	salvo	615	}
224	salvo	616	else
		617	{
		618	if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
		619	return (GPS_STST_OK);
		620	}
158	salvo	621	}
224	salvo	622	else
		623	{
		624	if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
		625	return (GPS_STST_OK);
		626	}
158	salvo	627	break;
		628
		629	default:
224	salvo	630	gps_state = GPS_CRTL_IDLE;
158	salvo	631	return (GPS_STST_ERR);
		632	break;
		633	}
		634	return (GPS_STST_ERR);
		635
		636	}
		637

Subversion Repositories FlightCtrl

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