WebSVN - FlightCtrl - Blame - /branches/salvo_gps/Basis

Rev	Author	Line No.	Line
662	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 Lesser 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 and GNU Lesser General Public License for more details.
		8	You should have received a copy of GNU General Public License (License_GPL.txt) and
		9	GNU Lesser General Public License (License_LGPL.txt) along with this program.
		10	If not, see <http://www.gnu.org/licenses/>.
		11
		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
		13	*/
		14	/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		15	von Peter Muehlenbrock alias Salvo
		16	Auswertung der Daten vom GPS im ublox Format
		17	Hold Modus mit PID Regler
		18	R�ckstuerz zur Basis Funktion
698	salvo	19	Stand 29.1.2008
662	salvo	20
		21	++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		22	*/
		23	#include "main.h"
		24	#include "math.h"
		25	//#include "gps.h"
		26
		27	// Defines fuer ublox Messageformat um Auswertung zu steuern
698	salvo	28	#define UBLOX_IDLE 0
		29	#define UBLOX_SYNC1 1
		30	#define UBLOX_SYNC2 2
		31	#define UBLOX_CLASS 3
		32	#define UBLOX_ID 4
		33	#define UBLOX_LEN1 5
		34	#define UBLOX_LEN2 6
		35	#define UBLOX_CKA 7
		36	#define UBLOX_CKB 8
662	salvo	37	#define UBLOX_PAYLOAD 9
		38
		39	// ublox Protokoll Identifier
698	salvo	40	#define UBLOX_NAV_POSUTM 0x08
		41	#define UBLOX_NAV_STATUS 0x03
		42	#define UBLOX_NAV_VELED 0x12
		43	#define UBLOX_NAV_CLASS 0x01
		44	#define UBLOX_SYNCH1_CHAR 0xB5
		45	#define UBLOX_SYNCH2_CHAR 0x62
662	salvo	46
698	salvo	47	signed int GPS_Nick = 0;
		48	signed int GPS_Roll = 0;
		49	short int ublox_msg_state = UBLOX_IDLE;
		50	static uint8_t chk_a =0; //Checksum
		51	static uint8_t chk_b =0;
		52	short int gps_state,gps_sub_state; //Zustaende der Statemachine
		53	short int gps_updte_flag;
662	salvo	54	static long signed gps_reg_x,gps_reg_y;
		55	static unsigned int rx_len;
		56	static unsigned int ptr_payload_data_end;
		57	unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
		58	static signed int hdng_2home,dist_2home; //Richtung und Entfernung zur home Position
		59	static signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt
698	salvo	60	static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler
		61	static uint8_t *ptr_payload_data;
		62	static uint8_t *ptr_pac_status;
		63	static int dist_flown;
		64	//static unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators
		65	static int gps_quiet_cnt; // zaheler fuer GPS Off Time beim Kameraausloesen
662	salvo	66
		67
		68	short int Get_GPS_data(void);
		69
		70	NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt
		71	NAV_STATUS_t actual_status; // Aktueller Nav Status
		72	NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten
		73
		74	GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst
		75	GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt
		76	GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position
		77	GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode
		78	GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode
		79
		80	// Initialisierung
		81	void GPS_Neutral(void)
		82	{
698	salvo	83	ublox_msg_state = UBLOX_IDLE;
		84	gps_state = GPS_CRTL_IDLE;
		85	gps_sub_state = GPS_CRTL_IDLE;
		86	actual_pos.status = 0;
		87	actual_speed.status = 0;
662	salvo	88	actual_status.status = 0;
		89	gps_home_position.status = 0; // Noch keine gueltige Home Position
		90	gps_act_position.status = 0;
		91	gps_rel_act_position.status = 0;
698	salvo	92	GPS_Nick = 0;
		93	GPS_Roll = 0;
		94	gps_updte_flag = 0;
		95	gps_alive_cnt = 0;
662	salvo	96	}
		97
		98	// Home Position sichern falls Daten verfuegbar sind.
		99	void GPS_Save_Home(void)
		100	{
		101	short int n;
		102	n = Get_GPS_data();
		103	if (n == 0) // Gueltige und aktuelle Daten ?
		104	{
		105	// Neue GPS Daten liegen vor
		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
		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
		120	if (gps_alive_cnt < 1000) gps_alive_cnt += 600; // Timeoutzaehler. Wird in Motorregler Routine ueberwacht und dekrementiert
		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.utm_alt = (int) (gps_act_position.utm_alt - gps_home_position.utm_alt);
		126	gps_rel_act_position.status = 1; // gueltige Positionsdaten
		127	n = 0;
		128	gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen.
		129	}
		130	else
		131	{
		132	n = 2; //keine gueltigen Daten vorhanden
		133	gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.
		134	}
		135	return (n);
		136	}
		137
		138	// Daten aus aktuellen ublox Messages extrahieren
		139	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
		140	short int Get_GPS_data(void)
		141	{
		142	short int n = 1;
		143
		144	if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
		145	if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
		146	{
		147	if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
		148	{
		149	actual_status.status = 0;
		150	gps_act_position.utm_east = actual_pos.utm_east/10;
		151	gps_act_position.utm_north = actual_pos.utm_north/10;
		152	gps_act_position.utm_alt = actual_pos.utm_alt/10;
698	salvo	153	actual_pos.status = 0; //neue ublox Messages anfordern
662	salvo	154	gps_act_position.speed_gnd = actual_speed.speed_gnd;
		155	gps_act_position.speed_gnd = actual_speed.speed_gnd;
		156	gps_act_position.heading = actual_speed.heading/100000;
		157	actual_speed.status = 0;
		158	gps_act_position.status = 1;
		159	n = 0; //Daten gueltig
		160	}
		161	else
		162	{
698	salvo	163	gps_act_position.status = 0; //Keine gueltigen Daten
		164	actual_speed.status = 0;
		165	actual_status.status = 0;
		166	actual_pos.status = 0; //neue ublox Messages anfordern
		167	n = 2;
662	salvo	168	}
		169	}
		170	return (n);
		171	}
		172
		173	/*
		174	Daten vom GPS im ublox MSG Format auswerten
		175	Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
		176	// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
		177	*/
		178	void Get_Ublox_Msg(uint8_t rx)
		179	{
		180	switch (ublox_msg_state)
		181	{
		182
		183	case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
		184	if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;
		185	else ublox_msg_state = UBLOX_IDLE;
		186	break;
		187
		188	case UBLOX_SYNC1:
		189
		190	if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;
		191	else ublox_msg_state = UBLOX_IDLE;
		192	chk_a = 0,chk_b = 0;
		193	break;
		194
		195	case UBLOX_SYNC2:
		196	if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;
		197	else ublox_msg_state = UBLOX_IDLE;
		198	break;
		199
		200	case UBLOX_CLASS: // Nur NAV Meldungen auswerten
		201	switch (rx)
		202	{
		203	case UBLOX_NAV_POSUTM:
		204	ptr_pac_status = &actual_pos.status;
		205	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
		206	else
		207	{
		208	ptr_payload_data = &actual_pos;
		209	ptr_payload_data_end = &actual_pos.status;
		210	ublox_msg_state = UBLOX_LEN1;
		211	}
		212	break;
		213
		214	case UBLOX_NAV_STATUS:
		215	ptr_pac_status = &actual_status.status;
		216	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
		217	else
		218	{
698	salvo	219	ptr_payload_data = &actual_status;
662	salvo	220	ptr_payload_data_end = &actual_status.status;
698	salvo	221	ublox_msg_state = UBLOX_LEN1;
662	salvo	222	}
		223	break;
		224
		225	case UBLOX_NAV_VELED:
		226	ptr_pac_status = &actual_speed.status;
		227	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
		228	else
		229	{
698	salvo	230	ptr_payload_data = &actual_speed;
662	salvo	231	ptr_payload_data_end = &actual_speed.status;
698	salvo	232	ublox_msg_state = UBLOX_LEN1;
662	salvo	233	}
		234	break;
		235
		236	default:
		237	ublox_msg_state = UBLOX_IDLE;
		238	break;
		239	}
		240	chk_a = UBLOX_NAV_CLASS + rx;
		241	chk_b = UBLOX_NAV_CLASS + chk_a;
		242	break;
		243
		244	case UBLOX_LEN1: // Laenge auswerten
		245	rx_len = rx;
		246	chk_a += rx;
		247	chk_b += chk_a;
		248	ublox_msg_state = UBLOX_LEN2;
		249	break;
		250
		251
		252	case UBLOX_LEN2: // Laenge auswerten
		253	rx_len = rx_len + (rx *256); // Laenge ermitteln
		254	chk_a += rx;
		255	chk_b += chk_a;
		256	ublox_msg_state = UBLOX_PAYLOAD;
		257	break;
		258
		259	case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen
		260	if (rx_len > 0)
		261	{
		262	*ptr_payload_data = rx;
		263	chk_a += rx;
		264	chk_b += chk_a;
		265	--rx_len;
		266	if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))
		267	{
		268	ptr_payload_data++;
		269	ublox_msg_state = UBLOX_PAYLOAD;
		270	}
		271	else ublox_msg_state = UBLOX_CKA;
		272	}
		273	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
		274	break;
		275
		276	case UBLOX_CKA: // Checksum pruefen
		277	if (rx == chk_a) ublox_msg_state = UBLOX_CKB;
		278	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
		279	break;
		280
		281	case UBLOX_CKB: // Checksum pruefen
		282	if (rx == chk_b) *ptr_pac_status = 1; // Paket ok
		283	ublox_msg_state = UBLOX_IDLE;
		284	break;
		285
		286	default:
		287	ublox_msg_state = UBLOX_IDLE;
		288	break;
		289	}
		290	}
		291
		292	//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
		293	short int GPS_CRTL(short int cmd)
		294	{
		295	static unsigned int cnt; // Zaehler fuer diverse Verzoegerungen
		296	static long int delta_north,delta_east; // Mass fuer Distanz zur Sollposition
698	salvo	297	signed int n;
662	salvo	298	static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion
698	salvo	299	signed int dist_frm_start_east,dist_frm_start_north;
		300	int amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil
662	salvo	301	static signed int int_east,int_north; //Integrierer
698	salvo	302	long int speed_east,speed_north; //Aktuelle Geschwindigkeit
662	salvo	303	signed long int_east1,int_north1;
698	salvo	304	int dist_east,dist_north;
		305	int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung
		306	long ni,ro; // Nick und Roll Zwischenwerte
662	salvo	307
		308
		309	switch (cmd)
		310	{
		311
		312	case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden.
		313	if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))
		314	{
		315	cnt++;
698	salvo	316	if (cnt > 100) // erst nach Verzoegerung
662	salvo	317	{
		318	// Erst mal initialisieren
		319	cnt = 0;
698	salvo	320	gps_tick = 0;
		321	hold_fast = 0;
662	salvo	322	hold_reset_int = 0; // Integrator enablen
698	salvo	323	int_east = 0, int_north = 0;
		324	gps_reg_x = 0, gps_reg_y = 0;
		325	delta_east = 0, delta_north = 0;
		326	dist_flown = 0;
662	salvo	327	gps_g2t_act_v = 0;
		328	gps_sub_state = GPS_CRTL_IDLE;
		329	// aktuelle positionsdaten abspeichern
		330	if (gps_rel_act_position.status > 0)
		331	{
		332	gps_rel_start_position.utm_east = gps_rel_act_position.utm_east;
		333	gps_rel_start_position.utm_north= gps_rel_act_position.utm_north;
		334	gps_rel_start_position.status = 1; // gueltige Positionsdaten
		335	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
		336	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
		337	gps_rel_hold_position.status = 1; // gueltige Positionsdaten
		338	//Richtung zur Home Position bezogen auf Nordpol bestimmen
		339	hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north);
		340	// in Winkel 0...360 Grad umrechnen
		341	if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home);
		342	else hdng_2home = (270 - hdng_2home);
		343	dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen
		344	gps_state = GPS_CRTL_HOME_ACTIVE;
		345	return (GPS_STST_OK);
		346	}
		347	else
		348	{
		349	gps_rel_start_position.status = 0; //Keine Daten verfuegbar
		350	gps_state = GPS_CRTL_IDLE;
		351	return(GPS_STST_ERR); // Keine Daten da
		352	}
		353	}
		354	else return(GPS_STST_PEND); // noch warten
		355	}
		356	break;
		357	// ******************************
		358
		359	case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
		360	if (gps_state != GPS_CRTL_HOLD_ACTIVE)
		361	{
		362	cnt++;
		363	if (cnt > 600) // erst nach Verzoegerung
		364	{
		365	cnt = 0;
		366	// aktuelle positionsdaten abspeichern
		367	if (gps_rel_act_position.status > 0)
		368	{
		369	hold_fast = 0;
698	salvo	370	hold_reset_int = 0; // Integrator enablen
662	salvo	371	int_east = 0, int_north = 0;
		372	gps_reg_x = 0, gps_reg_y = 0;
		373	delta_east = 0, delta_north = 0;
		374	speed_east = 0; speed_north= 0;
698	salvo	375	// int_ovfl_cnt = 0;
		376	gps_quiet_cnt = 0;
662	salvo	377	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
		378	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
		379	gps_rel_hold_position.status = 1; // gueltige Positionsdaten
		380	gps_state = GPS_CRTL_HOLD_ACTIVE;
		381	return (GPS_STST_OK);
		382	}
		383	else
		384	{
		385	gps_rel_hold_position.status = 0; //Keine Daten verfuegbar
		386	gps_state = GPS_CRTL_IDLE;
		387	return(GPS_STST_ERR); // Keine Daten da
		388	}
		389	}
		390	else return(GPS_STST_PEND); // noch warten
		391	}
		392	break;
		393
		394	case GPS_CMD_STOP: // Lageregelung beenden
698	salvo	395	cnt = 0;
662	salvo	396	GPS_Nick = 0;
		397	GPS_Roll = 0;
698	salvo	398	gps_sub_state = GPS_CRTL_IDLE;
662	salvo	399	gps_state = GPS_CRTL_IDLE;
		400	return (GPS_STST_OK);
		401	break;
		402
		403	default:
		404	return (GPS_STST_ERR);
		405	break;
		406	}
		407
		408	switch (gps_state)
		409	{
		410	case GPS_CRTL_IDLE:
		411	cnt = 0;
		412	return (GPS_STST_OK);
		413	break;
		414
		415	case GPS_CRTL_HOME_ACTIVE: // Rueckflug zur Basis
		416	//Der Sollwert des Lagereglers wird der Homeposition angenaehert
		417	if (gps_rel_start_position.status >0)
		418	{
		419	if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist
		420	{
		421	gps_tick++;
		422	int d1,d2,d3;
		423	d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );
		424	d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
		425	d3 = (dist_2home - dist_flown); // Restdistanz zum Ziel
		426
		427	if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel
		428	{
698	salvo	429	if ((d1 < (GPS_G2T_FAST_TOL/2)) && (d2 < (GPS_G2T_FAST_TOL/2))) //voll Stoff weiter wenn Lage gut innerhalb der Toleranz
662	salvo	430	{
		431	if (gps_g2t_act_v < GPS_G2T_V_MAX-3) gps_g2t_act_v += 4; //Geschwindigkeit erhoehen
		432	dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
		433	gps_sub_state = GPS_HOME_FAST_IN_TOL;
		434	}
676	salvo	435	else if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
		436	{
698	salvo	437	if (gps_g2t_act_v > (GPS_G2T_V_MAX/2)) gps_g2t_act_v -= 1; //Geschwindigkeit auf Haelfte runter oder rauffahren
		438	else if (gps_g2t_act_v < (GPS_G2T_V_MAX/2)) gps_g2t_act_v += 1;
676	salvo	439	dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
		440	gps_sub_state = GPS_HOME_FAST_IN_TOL;
		441	}
662	salvo	442	else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
		443	{
		444	if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren
		445	// dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen
		446	gps_sub_state = GPS_HOME_FAST_OUTOF_TOL;
		447	}
698	salvo	448	hold_reset_int = 0; // Integrator einsschalten
662	salvo	449	hold_fast = 1; // Regler fuer schnellen Flug
		450	dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000);
		451	dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000);
		452	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
		453	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
		454	}
		455	else if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen
		456	{
		457	if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL))
		458	{
		459	dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
		460	gps_sub_state = GPS_HOME_RMPDWN_IN_TOL;
		461	}
		462	else
		463	{
		464	dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen
		465	gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL;
		466	}
		467	hold_reset_int = 0; // Integrator einsschalten
		468	hold_fast = 1; // Regler fuer schnellen Flug
		469	dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000);
		470	dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000);
		471	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
		472	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
		473	}
		474	else //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt)
		475	{
		476	if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln
		477	{
		478	gps_sub_state = GPS_HOME_IN_TOL;
		479	hold_fast = 0; // Wieder normal regeln
		480	hold_reset_int = 0; // Integrator einsschalten
		481	if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN;
		482	else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN;
		483	if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN;
		484	else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN;
		485	if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN))
		486	{
		487	gps_rel_hold_position.utm_east = 0;
		488	gps_rel_hold_position.utm_north = 0;
698	salvo	489	gps_sub_state = GPS_HOME_FINISHED;
662	salvo	490	}
		491	}
		492	else gps_sub_state = GPS_HOME_OUTOF_TOL;
		493	}
		494	}
		495	gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
		496	return (GPS_STST_OK);
		497	}
		498	else // Keine GPS Daten verfuegbar, deswegen Abbruch
		499	{
		500	gps_state = GPS_CRTL_IDLE;
		501	return (GPS_STST_ERR);
		502	}
		503	break;
		504
		505
		506	case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet
		507	if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen
		508	{
		509	gps_quiet_cnt++;
		510	// ab hier wird geregelt
		511	delta_east = (long) (gps_rel_act_position.utm_east - gps_rel_hold_position.utm_east);
		512	delta_north = (long) (gps_rel_act_position.utm_north - gps_rel_hold_position.utm_north);
		513	int_east += (int)delta_east;
		514	int_north += (int)delta_north;
		515	speed_east = actual_speed.speed_e;
		516	speed_north = actual_speed.speed_n;
		517	gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden
		518	dist_east = (int)delta_east; //merken
		519	dist_north = (int)delta_north;
		520
		521
677	salvo	522	// #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow
		523	long int gpsintmax;
		524	if (Parameter_UserParam2 > 0)
662	salvo	525	{
698	salvo	526	gpsintmax = (GPS_NICKROLL_MAX * GPS_V * GPS_USR_PAR_FKT * ((32*3)/10))/(long)Parameter_UserParam2; //auf ungefeahren Maximalwert begrenzen
677	salvo	527	if ((abs(int_east) > (int)gpsintmax) \|\| (abs(int_north)> (int)gpsintmax))
		528	{
698	salvo	529	// // = 1; // Zahl der Overflows zaehlen
		530	// int_ovfl_cnt -= 1;
		531	int_east = (int_east * 6)/8; // Wert reduzieren
		532	int_north = (int_north* 6)/8;
677	salvo	533	}
		534
		535	if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten
		536	{
698	salvo	537	int_east = 0;
		538	int_north = 0;
677	salvo	539	}
662	salvo	540	}
677	salvo	541	else // Integrator deaktiviert
662	salvo	542	{
677	salvo	543	int_east = 0;
		544	int_north = 0;
662	salvo	545	}
		546
698	salvo	547	debug_gp_4 = (int)int_east; // zum Debuggen
677	salvo	548	debug_gp_5 = (int)int_north; // zum Debuggen
662	salvo	549
		550	//I Werte begrenzen
736	salvo	551	#define INT1_MAX (GPS_NICKROLL_MAX * GPS_V*3)/10// auf 30 Prozent des maximalen Nick/Rollwert begrenzen
662	salvo	552	int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT;
		553	int_north1 = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT;
		554	if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen
		555	else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX;
		556	if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen
		557	else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX;
		558
		559	if (hold_fast > 0) //schneller Coming Home Modus
		560	{
		561	amplfy_speed_east = DIFF_Y_F_MAX;
		562	amplfy_speed_north = DIFF_Y_F_MAX;
		563	amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT);
		564	amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT);
677	salvo	565	speed_east = (speed_east * (long)amplfy_speed_east) /50;
		566	speed_north = (speed_north * (long)amplfy_speed_north)/50;
662	salvo	567	// D Werte begrenzen
670	salvo	568	#define D_F_MAX (GPS_NICKROLL_MAX * GPS_V*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
662	salvo	569	if (speed_east > D_F_MAX) speed_east = D_F_MAX;
		570	else if (speed_east < -D_F_MAX) speed_east = -D_F_MAX;
		571	if (speed_north > D_F_MAX) speed_north = D_F_MAX;
		572	else if (speed_north < -D_F_MAX) speed_north = -D_F_MAX;
		573
		574	diff_p = (Parameter_UserParam1 * GPS_PROP_FAST_V)/GPS_USR_PAR_FKT; //Verstaerkung fuer P-Anteil
		575	}
		576	else //langsamer Holdmodus
		577	{
		578	amplfy_speed_east = DIFF_Y_N_MAX;
		579	amplfy_speed_north = DIFF_Y_N_MAX;
		580	amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT);
		581	amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT);
670	salvo	582	speed_east = (speed_east * (long)amplfy_speed_east) /25;
		583	speed_north = (speed_north * (long)amplfy_speed_north)/25;
662	salvo	584	// D Werte begrenzen
670	salvo	585	#define D_N_MAX (GPS_NICKROLL_MAX * GPS_V*8)/10 // auf 80 Prozent des Maximalen Nick/Rollwert begrenzen
662	salvo	586	if (speed_east > D_N_MAX) speed_east = D_N_MAX;
		587	else if (speed_east < -D_N_MAX) speed_east = -D_N_MAX;
		588	if (speed_north > D_N_MAX) speed_north = D_N_MAX;
		589	else if (speed_north < -D_N_MAX) speed_north = -D_N_MAX;
		590
		591	diff_p = (Parameter_UserParam1 * GPS_PROP_NRML_V)/GPS_USR_PAR_FKT; //Verstaerkung fuer P-Anteil
		592	}
		593
668	salvo	594	// debug_gp_4 = (int)speed_east; // zum Debuggen
		595	// debug_gp_5 = (int)speed_north; // zum Debuggen
662	salvo	596
668	salvo	597	//P-Werte verstaerken
698	salvo	598	delta_east = (delta_east * (long)diff_p)/(40);
		599	delta_north = (delta_north * (long)diff_p)/(40);
662	salvo	600
		601	if (hold_fast > 0) //schneller Coming Home Modus
		602	{
		603	// P Werte begrenzen
736	salvo	604	#define P1_F_MAX (GPS_NICKROLL_MAX * GPS_V*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
662	salvo	605	if (delta_east > P1_F_MAX) delta_east = P1_F_MAX;
		606	else if (delta_east < -P1_F_MAX) delta_east = -P1_F_MAX;
		607	if (delta_north > P1_F_MAX) delta_north = P1_F_MAX;
		608	else if (delta_north < -P1_F_MAX) delta_north = -P1_F_MAX;
		609	}
		610	else // Hold modus
		611	{
		612	// P Werte begrenzen
670	salvo	613	#define P1_N_MAX (GPS_NICKROLL_MAX * GPS_V*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
662	salvo	614	if (delta_east > P1_N_MAX) delta_east = P1_N_MAX;
		615	else if (delta_east < -P1_N_MAX) delta_east = -P1_N_MAX;
		616	if (delta_north > P1_N_MAX) delta_north = P1_N_MAX;
		617	else if (delta_north < -P1_N_MAX) delta_north = -P1_N_MAX;
		618	}
		619
		620	debug_gp_2 = (int)delta_east; // zum Debuggen
		621	debug_gp_3 = (int)delta_north; // zum Debuggen
		622
		623
		624	//PID Regler Werte aufsummieren
		625	gps_reg_x = -(int_east1 + delta_east + speed_east); // I + P +D Anteil X Achse
		626	gps_reg_y = -(int_north1 + delta_north + speed_north); // I + P +D Anteil Y Achse
		627	debug_gp_0 = (int)gps_reg_x; // zum Debuggen
		628	debug_gp_1 = (int)gps_reg_y; // zum Debuggen
		629
		630	// Werte fuer Nick und Roll direkt aus gps_reg_x und gps_reg_y bestimmen
		631	n = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; //Ausrichtung Kopter
		632	ni = -((gps_reg_y * (long)cos_i(n)) + (gps_reg_x * (long)sin_i(n)))/(1000*GPS_V);
		633	ro = ((gps_reg_x * (long)cos_i(n)) - (gps_reg_y * (long)sin_i(n)))/(1000*GPS_V);
		634
		635	if (ni > (GPS_NICKROLL_MAX )) ni = (GPS_NICKROLL_MAX);
		636	else if (ni < -(GPS_NICKROLL_MAX )) ni = -(GPS_NICKROLL_MAX );
		637	if (ro > (GPS_NICKROLL_MAX )) ro = (GPS_NICKROLL_MAX );
		638	else if (ro < -(GPS_NICKROLL_MAX)) ro = -(GPS_NICKROLL_MAX );
		639
		640	if ((abs(dist_east) > GPS_DIST_MAX) \|\| (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen
		641	{
		642	GPS_Roll = 0;
		643	GPS_Nick = 0;
		644	gps_state = GPS_CRTL_IDLE;
		645	return (GPS_STST_ERR);
		646	break;
		647	}
		648	else if ((PPM_in[7] > 100) && (CAM_GPS_QUIET > 0) && (gps_quiet_cnt <=4) ) // Wenn Fotoausloeser gedruckt wird, GPS Stellwerte kurzzeitig auf 0 setzen
		649	{
		650	gps_quiet_cnt++;
		651	GPS_Roll = 0;
		652	GPS_Nick = 0;
		653	if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
		654	return (GPS_STST_OK);
		655	}
		656	else if ((PPM_in[7] < 50) && (CAM_GPS_QUIET > 0) && (gps_quiet_cnt >= 4))
		657	{
		658	gps_quiet_cnt = 0;
		659	if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
		660	return (GPS_STST_OK);
		661	}
		662	else
		663	{
		664	GPS_Roll = (int)ro;
		665	GPS_Nick = (int)ni;
		666	if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
		667	return (GPS_STST_OK);
		668	}
		669	}
		670	else
		671	{
		672	if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
		673	return (GPS_STST_OK);
		674	}
		675	break;
		676
		677	default:
		678	gps_state = GPS_CRTL_IDLE;
		679	return (GPS_STST_ERR);
		680	break;
		681	}
		682	return (GPS_STST_ERR);
		683
		684	}
		685

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0067g/trunk/GPS.c @ 847 - Rev