WebSVN - FlightCtrl - Blame - Rev 225 - /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
224	salvo	16	Komm heim zu Papi 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
225	salvo	59	static short int hold_fast; //Flag 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	{
184	salvo	289	static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen
		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;
		295	long signed int dev,n_l;
225	salvo	296	signed int dist_frm_start_east,dist_frm_start_north;
158	salvo	297	switch (cmd)
		298	{
162	salvo	299
		300	case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden.
224	salvo	301	if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))
		302	{
		303	cnt++;
		304	if (cnt > 500) // erst nach Verzoegerung
		305	{
		306	// Erst mal initialisieren
		307	cnt = 0;
		308	gps_tick = 0;
225	salvo	309	hold_fast = 0;
224	salvo	310	int_east = 0, int_north = 0;
		311	gps_reg_x = 0, gps_reg_y = 0;
		312	dist_east = 0, dist_north = 0;
		313	diff_east_f = 0, diff_north_f= 0;
		314	diff_east = 0, diff_north = 0;
		315	dist_flown = 0;
		316	gps_sub_state = GPS_CRTL_IDLE;
		317	// aktuelle positionsdaten abspeichern
		318	if (gps_rel_act_position.status > 0)
		319	{
		320	gps_rel_start_position.utm_east = gps_rel_act_position.utm_east;
		321	gps_rel_start_position.utm_north= gps_rel_act_position.utm_north;
		322	gps_rel_start_position.status = 1; // gueltige Positionsdaten
		323	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
		324	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
		325	gps_rel_hold_position.status = 1; // gueltige Positionsdaten
		326	//Richtung zur Home Positionbezogen auf Nordpol bestimmen
		327	hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north);
		328	// in Winkel 0...360 Grad umrechnen
		329	if ((-gps_rel_start_position.utm_east >= 0)) hdng_2home = ( 90-hdng_2home);
		330	else hdng_2home = (270 - hdng_2home);
		331	dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen
		332	gps_state = GPS_CRTL_HOME_ACTIVE;
		333	return (GPS_STST_OK);
		334	}
		335	else
		336	{
		337	gps_rel_start_position.status = 0; //Keine Daten verfuegbar
		338	gps_state = GPS_CRTL_IDLE;
		339	return(GPS_STST_ERR); // Keine Daten da
		340	}
		341	}
		342	else return(GPS_STST_PEND); // noch warten
		343	}
162	salvo	344	break;
		345	// ******************************
		346
158	salvo	347	case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
		348	if (gps_state != GPS_CRTL_HOLD_ACTIVE)
		349	{
		350	cnt++;
160	salvo	351	if (cnt > 500) // erst nach Verzoegerung
158	salvo	352	{
		353	cnt = 0;
224	salvo	354	// aktuelle positionsdaten abspeichern
158	salvo	355	if (gps_rel_act_position.status > 0)
		356	{
225	salvo	357	hold_fast = 0;
224	salvo	358	int_east = 0, int_north = 0;
		359	gps_reg_x = 0, gps_reg_y = 0;
		360	dist_east = 0, dist_north = 0;
		361	diff_east_f = 0, diff_north_f= 0;
		362	diff_east = 0, diff_north = 0;
158	salvo	363	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
		364	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
		365	gps_rel_hold_position.status = 1; // gueltige Positionsdaten
		366	gps_state = GPS_CRTL_HOLD_ACTIVE;
		367	return (GPS_STST_OK);
		368	}
		369	else
		370	{
		371	gps_rel_hold_position.status = 0; //Keine Daten verfuegbar
		372	gps_state = GPS_CRTL_IDLE;
		373	return(GPS_STST_ERR); // Keine Daten da
		374	}
		375	}
		376	else return(GPS_STST_PEND); // noch warten
		377	}
		378	break;
		379
162	salvo	380	case GPS_CMD_STOP: // Lageregelung beenden
190	salvo	381	cnt = 0;
		382	GPS_Nick = 0;
		383	GPS_Roll = 0;
		384	gps_int_x = 0;
		385	gps_int_y = 0;
162	salvo	386	gps_state = GPS_CRTL_IDLE;
158	salvo	387	return (GPS_STST_OK);
		388	break;
		389
		390	default:
		391	return (GPS_STST_ERR);
		392	break;
		393	}
		394
		395	switch (gps_state)
		396	{
		397	case GPS_CRTL_IDLE:
		398	cnt = 0;
		399	return (GPS_STST_OK);
		400	break;
		401
224	salvo	402	case GPS_CRTL_HOME_ACTIVE: // Rueckflug zur Basis
		403	//Der Sollwert des Lagereglers wird der Homeposition angenaehert
		404	if (gps_rel_start_position.status >0)
		405	{
		406	if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist
		407	{
		408	gps_tick++;
		409	int d1,d2,d3;
225	salvo	410	d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );
		411	d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
224	salvo	412	d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel
		413
		414	if (d3 > GPS_G2T_DIST_MAX_STOP)
		415	{
		416	if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
225	salvo	417	{
		418	hold_fast = 1; // Schnell Regeln
224	salvo	419	dist_flown += (GPS_G2T_V_MAX); // Vorgabe der Strecke anhand der Geschwindigkeit
225	salvo	420	dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
		421	dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
		422	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
		423	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
224	salvo	424	}
225	salvo	425	else hold_fast = 0; // Wieder normal regeln
224	salvo	426	}
		427	else //Schon ziemlich nahe am Ziel, deswegen abbremsen
		428	{
225	salvo	429	hold_fast = 0; // Wieder normal regeln
224	salvo	430	if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
		431	{
225	salvo	432	if (d3 > GPS_G2T_DIST_HOLD)
224	salvo	433	{
		434	dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
		435	dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000;
		436	dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000;
225	salvo	437	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt
224	salvo	438	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt
		439	}
225	salvo	440	else //Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt)
224	salvo	441	{
225	salvo	442	if (gps_rel_hold_position.utm_east > 1) gps_rel_hold_position.utm_east--;
		443	else if (gps_rel_hold_position.utm_east <-1 ) gps_rel_hold_position.utm_east++;
		444	if (gps_rel_hold_position.utm_north > 1) gps_rel_hold_position.utm_north--;
		445	else if (gps_rel_hold_position.utm_north < -1 ) gps_rel_hold_position.utm_north++;
		446	if ((abs(gps_rel_hold_position.utm_east) <= 1) && (abs(gps_rel_hold_position.utm_north) <=1)) gps_sub_state = GPS_HOME_FINISHED;
224	salvo	447	}
225	salvo	448	}
224	salvo	449	}
		450	gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
		451	return (GPS_STST_OK);
		452	}
		453	else return (GPS_STST_OK);
		454	}
		455	else
		456	{
		457	gps_state = GPS_CRTL_IDLE; //Abbruch
		458	return (GPS_STST_ERR);
		459	}
		460	break;
		461
		462
186	salvo	463	case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet
224	salvo	464	if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen
158	salvo	465	{
		466	gps_updte_flag = 0;
		467	// ab hier wird geregelt
186	salvo	468	diff_east = -dist_east; //Alten Wert fuer Differenzier schon mal abziehen
		469	diff_north = -dist_north;
190	salvo	470	dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east;
184	salvo	471	dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
162	salvo	472	int_east += dist_east;
		473	int_north += dist_north;
167	salvo	474	diff_east += dist_east; // Differenz zur vorhergehenden East Position
		475	diff_north += dist_north; // Differenz zur vorhergehenden North Position
224	salvo	476	/*
194	salvo	477	diff_east_f = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern
		478	diff_north_f = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern
224	salvo	479	*/
194	salvo	480	#define GPSINT_MAX 2048 //neuer Wert ab 1.10.2007 Begrenzung
162	salvo	481	if ((abs(int_east) > GPSINT_MAX) \|\| (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten
		482	{
		483	int_east -= dist_east;
		484	int_north -= dist_north;
		485	}
225	salvo	486	if (hold_fast > 0) //Im Schnellen Mode Integrator abschalten
		487	{
		488	int_east = 0;
		489	int_north = 0;
		490	}
		491
209	salvo	492	// Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
		493	// desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
197	salvo	494	#define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10
209	salvo	495	#define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm
197	salvo	496	signed long dist;
		497	int phi;
209	salvo	498	phi = arctan_i(abs(dist_north),abs(dist_east));
224	salvo	499	dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln
		500
197	salvo	501	diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10
209	salvo	502	if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen
225	salvo	503	if (hold_fast > 0) diff_v = 10; // im schnellen Modus keine staerkere Wirkung des Differenzierers
161	salvo	504
186	salvo	505	//PID Regler Werte aufsummieren
224	salvo	506	gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil X Achse
		507	gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil Y Achse
159	salvo	508
186	salvo	509	//Ziel-Richtung bezogen auf Nordpol bestimmen
194	salvo	510	GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y);
159	salvo	511
186	salvo	512	// in Winkel 0...360 Grad umrechnen
161	salvo	513	if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);
162	salvo	514	else GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);
161	salvo	515
159	salvo	516	// Relative Richtung in bezug auf Nordachse des Kopters errechen
		517	n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
		518	GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n;
167	salvo	519	if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360;
159	salvo	520	else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;
		521	else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;
		522
184	salvo	523	// Regelabweichung aus x,y zu Ziel in Distanz umrechnen
160	salvo	524	if (abs(gps_reg_x) > abs(gps_reg_y) )
		525	{
197	salvo	526	dev = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen
		527	dev = abs((dev *1000) / (long) sin_i(GPS_hdng_abs_2trgt));
160	salvo	528	}
		529	else
		530	{
197	salvo	531	dev = (long)gps_reg_y;
		532	dev = abs((dev *1000) / (long) cos_i(GPS_hdng_abs_2trgt));
160	salvo	533	}
197	salvo	534	GPS_dist_2trgt = (int) dev;
185	salvo	535	// Winkel und Distanz in Nick und Rollgroessen umrechnen
197	salvo	536	GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000);
		537	GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000);
190	salvo	538
194	salvo	539	#define GPS_V 8 // auf Maximalwert normieren. Teilerfaktor ist 8
184	salvo	540	if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V);
		541	else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V);
		542	if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);
		543	else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V);
		544
		545	//Kleine Werte verstaerken, Grosse abschwaechen
186	salvo	546	n = sin_i((GPS_Roll90)/(GPS_NICKROLL_MAX GPS_V));
		547	n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000;
		548	GPS_Roll = (int) n_l;
185	salvo	549	n = sin_i((GPS_Nick90)/(GPS_NICKROLL_MAX GPS_V));
186	salvo	550	n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000;
		551	GPS_Nick = (int) n_l;
184	salvo	552
		553	if ((abs(dist_east) > GPS_DIST_MAX) \|\| (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen
		554	{
		555	GPS_Roll = 0;
		556	GPS_Nick = 0;
		557	gps_state = GPS_CRTL_IDLE;
186	salvo	558	return (GPS_STST_ERR);
		559	break;
184	salvo	560	}
224	salvo	561	else
		562	{
		563	if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
		564	return (GPS_STST_OK);
		565	}
158	salvo	566	}
224	salvo	567	else
		568	{
		569	if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
		570	return (GPS_STST_OK);
		571	}
158	salvo	572	break;
		573
		574	default:
224	salvo	575	gps_state = GPS_CRTL_IDLE;
158	salvo	576	return (GPS_STST_ERR);
		577	break;
		578	}
		579	return (GPS_STST_ERR);
		580
		581	}
		582

Subversion Repositories FlightCtrl

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