WebSVN - FlightCtrl - Blame - Rev 224 - /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
		17	Stand 5.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;
224	salvo	48	short int gps_state,gps_sub_state;
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
143	salvo	59
		60	static uint8_t *ptr_payload_data;
		61	static uint8_t *ptr_pac_status;
		62
224	salvo	63	long int dist_flown,dist_frm_start_east,dist_frm_start_north;
		64
156	salvo	65	short int Get_GPS_data(void);
143	salvo	66
		67	NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt
		68	NAV_STATUS_t actual_status; // Aktueller Nav Status
		69	NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten
		70
156	salvo	71	GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst
		72	GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt
		73	GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position
158	salvo	74	GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode
224	salvo	75	GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode
143	salvo	76
156	salvo	77	// Initialisierung
		78	void GPS_Neutral(void)
1	ingob	79	{
158	salvo	80	ublox_msg_state = UBLOX_IDLE;
		81	gps_state = GPS_CRTL_IDLE;
		82	actual_pos.status = 0;
		83	actual_speed.status = 0;
		84	actual_status.status = 0;
		85	gps_home_position.status = 0; // Noch keine gueltige Home Position
		86	gps_act_position.status = 0;
		87	gps_rel_act_position.status = 0;
		88	GPS_Nick = 0;
		89	GPS_Roll = 0;
		90	gps_updte_flag = 0;
159	salvo	91	gps_int_x = 0;
		92	gps_int_y = 0;
190	salvo	93	gps_alive_cnt = 0;
1	ingob	94	}
		95
156	salvo	96	// Home Position sichern falls Daten verfuegbar sind.
		97	void GPS_Save_Home(void)
1	ingob	98	{
156	salvo	99	short int n;
		100	n = Get_GPS_data();
		101	if (n == 0) // Gueltige und aktuelle Daten ?
		102	{
		103	// Neue GPS Daten liegen vor
157	salvo	104	gps_home_position.utm_east = gps_act_position.utm_east;
		105	gps_home_position.utm_north = gps_act_position.utm_north;
		106	gps_home_position.utm_alt = gps_act_position.utm_alt;
		107	gps_home_position.status = 1; // Home Position gueltig
156	salvo	108	}
		109	}
		110
		111	// Relative Position zur Home Position bestimmen
		112	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
		113	short int Get_Rel_Position(void)
		114	{
		115	short int n = 0;
		116	n = Get_GPS_data();
		117	if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
224	salvo	118	if (gps_alive_cnt < 400) gps_alive_cnt += 300; // Timeoutzaehler. Wird in Motorregler Routine ueberwacht und dekrementiert
156	salvo	119	if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
		120	{
		121	gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east);
		122	gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north);
		123	gps_rel_act_position.status = 1; // gueltige Positionsdaten
		124	n = 0;
158	salvo	125	gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen.
156	salvo	126	}
		127	else
		128	{
		129	n = 2; //keine gueltigen Daten vorhanden
		130	gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.
		131	}
		132	return (n);
		133	}
		134
		135	// Daten aus aktuellen ublox Messages extrahieren
		136	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
		137	short int Get_GPS_data(void)
		138	{
		139	short int n = 1;
		140
		141	if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
143	salvo	142	if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
		143	{
149	salvo	144	if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
143	salvo	145	{
		146	gps_act_position.utm_east = actual_pos.utm_east/10;
		147	gps_act_position.utm_north = actual_pos.utm_north/10;
		148	gps_act_position.utm_alt = actual_pos.utm_alt/10;
		149	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
		150	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
		151	gps_act_position.heading = actual_speed.heading/100000;
		152	gps_act_position.status = 1;
156	salvo	153	n = 0; //Daten gueltig
143	salvo	154	}
156	salvo	155	else
		156	{
		157	gps_act_position.status = 0; //Keine gueltigen Daten
		158	n = 2;
		159	}
		160	actual_pos.status = 0; //neue ublox Messages anfordern
		161	actual_status.status = 0;
		162	actual_speed.status = 0;
		163	}
		164	return (n);
1	ingob	165	}
		166
		167
143	salvo	168	/*
		169	Daten vom GPS im ublox MSG Format auswerten
156	salvo	170	Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
143	salvo	171	// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
		172	*/
		173	void Get_Ublox_Msg(uint8_t rx)
		174	{
		175	switch (ublox_msg_state)
		176	{
1	ingob	177
143	salvo	178	case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
		179	if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;
		180	else ublox_msg_state = UBLOX_IDLE;
		181	break;
		182
		183	case UBLOX_SYNC1:
		184
		185	if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;
		186	else ublox_msg_state = UBLOX_IDLE;
		187	chk_a = 0,chk_b = 0;
		188	break;
		189
		190	case UBLOX_SYNC2:
		191	if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;
		192	else ublox_msg_state = UBLOX_IDLE;
		193	break;
		194
		195	case UBLOX_CLASS: // Nur NAV Meldungen auswerten
		196	switch (rx)
		197	{
		198	case UBLOX_NAV_POSUTM:
		199	ptr_pac_status = &actual_pos.status;
		200	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
		201	else
		202	{
		203	ptr_payload_data = &actual_pos;
		204	ptr_payload_data_end = &actual_pos.status;
		205	ublox_msg_state = UBLOX_LEN1;
		206	}
		207	break;
		208
		209	case UBLOX_NAV_STATUS:
		210	ptr_pac_status = &actual_status.status;
		211	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
		212	else
		213	{
		214	ptr_payload_data = &actual_status;
		215	ptr_payload_data_end = &actual_status.status;
		216	ublox_msg_state = UBLOX_LEN1;
		217	}
		218	break;
		219
		220	case UBLOX_NAV_VELED:
156	salvo	221	ptr_pac_status = &actual_speed.status;
143	salvo	222	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
		223	else
		224	{
		225	ptr_payload_data = &actual_speed;
		226	ptr_payload_data_end = &actual_speed.status;
		227	ublox_msg_state = UBLOX_LEN1;
		228	}
		229	break;
		230
		231	default:
		232	ublox_msg_state = UBLOX_IDLE;
		233	break;
		234	}
		235	chk_a = UBLOX_NAV_CLASS + rx;
		236	chk_b = UBLOX_NAV_CLASS + chk_a;
		237	break;
		238
		239	case UBLOX_LEN1: // Laenge auswerten
		240	rx_len = rx;
		241	chk_a += rx;
		242	chk_b += chk_a;
		243	ublox_msg_state = UBLOX_LEN2;
		244	break;
		245
		246
		247	case UBLOX_LEN2: // Laenge auswerten
		248	rx_len = rx_len + (rx *256); // Laenge ermitteln
		249	chk_a += rx;
		250	chk_b += chk_a;
		251	ublox_msg_state = UBLOX_PAYLOAD;
		252	break;
		253
		254	case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen
		255	if (rx_len > 0)
		256	{
		257	*ptr_payload_data = rx;
		258	chk_a += rx;
		259	chk_b += chk_a;
		260	--rx_len;
		261	if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))
		262	{
		263	ptr_payload_data++;
156	salvo	264	ublox_msg_state = UBLOX_PAYLOAD;
143	salvo	265	}
		266	else ublox_msg_state = UBLOX_CKA;
		267	}
		268	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
		269	break;
		270
		271	case UBLOX_CKA: // Checksum pruefen
		272	if (rx == chk_a) ublox_msg_state = UBLOX_CKB;
		273	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
		274	break;
		275
		276	case UBLOX_CKB: // Checksum pruefen
		277	if (rx == chk_b) *ptr_pac_status = 1; // Paket ok
		278	ublox_msg_state = UBLOX_IDLE;
		279	break;
		280
		281	default:
		282	ublox_msg_state = UBLOX_IDLE;
		283	break;
158	salvo	284	}
143	salvo	285	}
158	salvo	286
		287	//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
		288	short int GPS_CRTL(short int cmd)
		289	{
184	salvo	290	static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen
		291	static signed int int_east,int_north; //Integrierer
		292	static signed int dist_north,dist_east;
186	salvo	293	static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert)
		294	static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert
197	salvo	295	signed int n,diff_v;
		296	long signed int dev,n_l;
224	salvo	297
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;
		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	{
224	salvo	357	int_east = 0, int_north = 0;
		358	gps_reg_x = 0, gps_reg_y = 0;
		359	dist_east = 0, dist_north = 0;
		360	diff_east_f = 0, diff_north_f= 0;
		361	diff_east = 0, diff_north = 0;
158	salvo	362	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
		363	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
		364	gps_rel_hold_position.status = 1; // gueltige Positionsdaten
		365	gps_state = GPS_CRTL_HOLD_ACTIVE;
		366	return (GPS_STST_OK);
		367	}
		368	else
		369	{
		370	gps_rel_hold_position.status = 0; //Keine Daten verfuegbar
		371	gps_state = GPS_CRTL_IDLE;
		372	return(GPS_STST_ERR); // Keine Daten da
		373	}
		374	}
		375	else return(GPS_STST_PEND); // noch warten
		376	}
		377	break;
		378
162	salvo	379	case GPS_CMD_STOP: // Lageregelung beenden
190	salvo	380	cnt = 0;
		381	GPS_Nick = 0;
		382	GPS_Roll = 0;
		383	gps_int_x = 0;
		384	gps_int_y = 0;
162	salvo	385	gps_state = GPS_CRTL_IDLE;
158	salvo	386	return (GPS_STST_OK);
		387	break;
		388
		389	default:
		390	return (GPS_STST_ERR);
		391	break;
		392	}
		393
		394	switch (gps_state)
		395	{
		396	case GPS_CRTL_IDLE:
		397	cnt = 0;
		398	return (GPS_STST_OK);
		399	break;
		400
224	salvo	401	case GPS_CRTL_HOME_ACTIVE: // Rueckflug zur Basis
		402	//Der Sollwert des Lagereglers wird der Homeposition angenaehert
		403	if (gps_rel_start_position.status >0)
		404	{
		405	if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist
		406	{
		407	gps_tick++;
		408	int d1,d2,d3;
		409	d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel
		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 );
		412
		413	if (d3 > GPS_G2T_DIST_MAX_STOP)
		414	{
		415	if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
		416	{
		417	dist_flown += (GPS_G2T_V_MAX); // Vorgabe der Strecke anhand der Geschwindigkeit
		418	dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000;
		419	dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000;
		420	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt
		421	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt
		422	}
		423	}
		424	else //Schon ziemlich nahe am Ziel, deswegen abbremsen
		425	{
		426	if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
		427	{
		428	if (d3 > 0)
		429	{
		430	dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
		431	dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000;
		432	dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000;
		433	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt
		434	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt
		435	}
		436	else //Ziel erreicht, Regelung beenden
		437	{
		438	gps_rel_hold_position.utm_east = 0;
		439	gps_rel_hold_position.utm_north = 0;
		440	gps_sub_state = GPS_HOME_FINISHED;
		441	}
		442	}
		443
		444	}
		445	gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
		446	return (GPS_STST_OK);
		447	}
		448	else return (GPS_STST_OK);
		449	}
		450	else
		451	{
		452	gps_state = GPS_CRTL_IDLE; //Abbruch
		453	return (GPS_STST_ERR);
		454	}
		455	break;
		456
		457
186	salvo	458	case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet
224	salvo	459	if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen
158	salvo	460	{
		461	gps_updte_flag = 0;
		462	// ab hier wird geregelt
186	salvo	463	diff_east = -dist_east; //Alten Wert fuer Differenzier schon mal abziehen
		464	diff_north = -dist_north;
190	salvo	465	dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east;
184	salvo	466	dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
162	salvo	467	int_east += dist_east;
		468	int_north += dist_north;
167	salvo	469	diff_east += dist_east; // Differenz zur vorhergehenden East Position
		470	diff_north += dist_north; // Differenz zur vorhergehenden North Position
224	salvo	471	/*
194	salvo	472	diff_east_f = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern
		473	diff_north_f = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern
224	salvo	474	*/
194	salvo	475	#define GPSINT_MAX 2048 //neuer Wert ab 1.10.2007 Begrenzung
162	salvo	476	if ((abs(int_east) > GPSINT_MAX) \|\| (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten
		477	{
		478	int_east -= dist_east;
		479	int_north -= dist_north;
		480	}
209	salvo	481	// Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
		482	// desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
197	salvo	483	#define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10
209	salvo	484	#define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm
197	salvo	485	signed long dist;
		486	int phi;
209	salvo	487	phi = arctan_i(abs(dist_north),abs(dist_east));
224	salvo	488	dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln
		489
197	salvo	490	diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10
209	salvo	491	if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen
161	salvo	492
186	salvo	493	//PID Regler Werte aufsummieren
224	salvo	494	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
		495	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	496
186	salvo	497	//Ziel-Richtung bezogen auf Nordpol bestimmen
194	salvo	498	GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y);
159	salvo	499
186	salvo	500	// in Winkel 0...360 Grad umrechnen
161	salvo	501	if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);
162	salvo	502	else GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);
161	salvo	503
159	salvo	504	// Relative Richtung in bezug auf Nordachse des Kopters errechen
		505	n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
		506	GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n;
167	salvo	507	if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360;
159	salvo	508	else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;
		509	else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;
		510
184	salvo	511	// Regelabweichung aus x,y zu Ziel in Distanz umrechnen
160	salvo	512	if (abs(gps_reg_x) > abs(gps_reg_y) )
		513	{
197	salvo	514	dev = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen
		515	dev = abs((dev *1000) / (long) sin_i(GPS_hdng_abs_2trgt));
160	salvo	516	}
		517	else
		518	{
197	salvo	519	dev = (long)gps_reg_y;
		520	dev = abs((dev *1000) / (long) cos_i(GPS_hdng_abs_2trgt));
160	salvo	521	}
197	salvo	522	GPS_dist_2trgt = (int) dev;
185	salvo	523	// Winkel und Distanz in Nick und Rollgroessen umrechnen
197	salvo	524	GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000);
		525	GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000);
190	salvo	526
194	salvo	527	#define GPS_V 8 // auf Maximalwert normieren. Teilerfaktor ist 8
184	salvo	528	if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V);
		529	else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V);
		530	if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);
		531	else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V);
		532
		533	//Kleine Werte verstaerken, Grosse abschwaechen
186	salvo	534	n = sin_i((GPS_Roll90)/(GPS_NICKROLL_MAX GPS_V));
		535	n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000;
		536	GPS_Roll = (int) n_l;
185	salvo	537	n = sin_i((GPS_Nick90)/(GPS_NICKROLL_MAX GPS_V));
186	salvo	538	n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000;
		539	GPS_Nick = (int) n_l;
184	salvo	540
		541	if ((abs(dist_east) > GPS_DIST_MAX) \|\| (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen
		542	{
		543	GPS_Roll = 0;
		544	GPS_Nick = 0;
		545	gps_state = GPS_CRTL_IDLE;
186	salvo	546	return (GPS_STST_ERR);
		547	break;
184	salvo	548	}
224	salvo	549	else
		550	{
		551	if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
		552	return (GPS_STST_OK);
		553	}
158	salvo	554	}
224	salvo	555	else
		556	{
		557	if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
		558	return (GPS_STST_OK);
		559	}
158	salvo	560	break;
		561
		562	default:
224	salvo	563	gps_state = GPS_CRTL_IDLE;
158	salvo	564	return (GPS_STST_ERR);
		565	break;
		566	}
		567	return (GPS_STST_ERR);
		568
		569	}
		570

Subversion Repositories FlightCtrl

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