WebSVN - FlightCtrl - Blame - Rev 1139 - /branches/thjac/V1_11/pitch

Rev	Author	Line No.	Line
1134	thjac	1	/* pitch_md.c
		2	*
		3	* Copyright 2009 Thomas Jachmann
		4	*
		5	* Die in dieser Quelldatei enthaltenen Algorithmen erm�glichen eine MD-�hnliche Pitch-Steuerung
		6	* f�r den MK.
		7	*/
		8
		9	#include "main.h"
		10	#include "parameter.h"
		11	#include "fc.h"
		12	#include "pitch.h"
		13	#include "altcon.h"
		14
		15
		16	#define STATE_INITIALIZE 0x01 // Anfangszustand nach Einschalten der Motoren
		17	#define STATE_SETUP 0x02 // Ermittlung von PARAM_PITCH_MD_HOVER
		18	#define STATE_BEGIN 0x03 // Anfangszustand f�r Flugbetrieb
		19	#define STATE_BEGIN1 0x04 // Anfangszustand f�r Flugbetrieb
		20	#define STATE_READY 0x05 // Manuelle Kontrolle
		21	#define STATE_WAIT 0x06 // Warten auf Einschalten der H�henregelung
		22	#define STATE_ACTIVATING 0x07 // Aktivierung der H�henregelung
		23	#define STATE_ACTIVE 0x08 // H�henregelung ist aktiv
		24
		25
1139	thjac	26	static int stickValue = 0; // Aktueller Stick-Wert
		27	static int lastStickValue = 0; // Vorheriger Stick-Wert
		28	static int actualPitchCount = 0; // Soll-Pitch-Wert
		29	static int targetPitchCount = 0; // Ist-Pitch-Wert
		30	static int pitchOffset; // Aktueller Grundgaswert in Neutralstellung
		31	static char state; // Zustand
		32	static int timer;
		33	static int delay = 0;
		34	static int delayCounter = 0;
		35	static int temp; // Tempor�re Werte; wird mehrfach verwendet
1134	thjac	36
1139	thjac	37	unsigned char pitchHover = 0; // Standgaswert
1134	thjac	38
1139	thjac	39
		40	void pitch_md_init( void ) {
		41	pitchHover = eeprom_read_byte( &EEPromArray[ EEPROM_ADR_PITCH_MD_HOVER ] );
		42	}
		43
		44
1134	thjac	45	/*
		46	* Berechnet den aktuellen Pitch-Wert f�r die Regelung
		47	*
		48	* Nachdem die Motoren eingeschaltet sind, wird der Pitch-Stick losgelassen und damit in Mittelstellung
		49	* gebracht. Die Motoren laufen zu dieser Zeit im Leerlaufgas. Vor dem Abheben m�ssen die Motoren in
		50	* das Standgas gebracht werden. Dies geschieht durch minimales Gasgeben. Durch weiteres Gasgeben und
		51	* nehmen kann abgehoben und geflogen werden.
		52	*
		53	* Erreicht der Stick w�hrend des Fluges die Neutralstellung und verbleibt dort f�r ca. 1 Sekunde ohne
		54	* bewegt zu werden, aktiviert sich die H�henregelung und h�lt den MK auf der aktuellen H�he.
		55	*
		56	* Sobald der Stick die Neutralstellung verl��t, wird die automatische H�henregelung deaktiviert
		57	* und der vorige Pitch-Wert als Wert der Neutralstellung �bernommen. Der Pitch l��t sich nun
		58	* �ber den gesamten Stick-Bereich regeln.
		59	*
		60	* Erreicht der Stick ein weiteres Mal die Neutralstellung, wird die automatische H�henregelung
		61	* wieder aktiviert, jetzt jedoch immer mit einer zeitlichen Verz�gerung. Nur so ist ein
		62	* ungest�rtes manuelles Steuern m�glich.
		63	*
		64	* Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt.
		65	* Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb
		66	* eines Wertes zu gelangen, der die Lageregelung au�er Funktion setzt, wird ein zweiter Wert f�r
		67	* Minimalgas konfiguriert, der greift, sobald erstmalig die automatische H�henregelung aktiviert wurde.
		68	*/
		69	int pitch_md_value( void ) {
		70
		71	int register rawStickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_stickoffset();
		72
		73	// Sind die Motoren eingeschaltet?
		74	if( MotorenEin ) {
		75
		76	// Vorigen Stick-Wert merken
		77	lastStickValue = stickValue;
		78
		79	/* StickValue exponentiell angleichen, da ausgehend von der Neutralstellung
		80	* nur jeweils die halbe Aufl�sung nach oben und unten zur Verf�gung steht. Bei einer
		81	* Multiplikation mit 2 lie�e sich das Gas im Schwebebereich nicht fein genug einstellen. */
		82	temp = rawStickValue;
		83	if( temp > 0 ) {
		84	temp = temp + ( ( temp * temp ) / 150 );
		85	} else {
		86	temp = temp - ( ( temp * temp ) / 150 );
		87	}
		88
		89	stickValue = temp;
1139	thjac	90
1134	thjac	91	/* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset
		92	* zuz�glich dem Stick-Wert. */
		93	targetPitchCount = stickValue + pitchOffset;
		94
		95	switch( state ) {
		96
		97	/* Entscheidet �ber Flugbetrieb oder Setup-Betrieb. F�r den Setup-Betrieb
		98	* mu� beim Einschalten der Motoren gleichzeitig der Roll-Stick ganz
		99	* bet�tigt werden (Richtung ist egal).
		100	*/
		101	case STATE_INITIALIZE:
		102
		103	if( abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) > 70 ) {
		104	state = STATE_SETUP;
		105	} else {
		106	state = STATE_BEGIN;
		107	}
		108	break;
		109
		110	/* Erlaubt die Ermittlung des Parameters PARAM_PITCH_MD_HOVER. Hierzu wird soviel Gas
		111	* gegeben, bis der MK kurz vor dem Abheben ist, jedoch noch stabil steht. Um den
		112	* Gaswert dauerhaft zu speichern, mu� der Stick an der Position verweilen, bis
		113	* der Summer die �bernahme akustisch quittiert. Dann m�ssen die Motoren wieder
		114	* ausgeschaltet werden, da dieser Modus nicht f�r den Flug vorgesehen ist.
		115	*/
		116	case STATE_SETUP:
		117
		118	// Im Setup-Modus soll das Gas nicht tr�ge reagieren
		119	actualPitchCount = targetPitchCount;
		120
		121	if( rawStickValue < 20 \|\| ( stickValue - lastStickValue ) ) {
		122	timer = PARAM_TIMER_2S;
		123	}
		124
		125	/* Der Stick befindet sich eindeutig in der oberen H�lfte und wurde
		126	* seit dem letzten Zyklus nicht bewegt. */
		127	else {
		128	timer--;
		129
		130	/* Die Verweilzeit ist abgelaufen und der aktuelle Pitch-Wert
		131	* entspricht nicht dem bereits gespeicherten Wert. */
1139	thjac	132	if( !timer && ( pitchHover != actualPitchCount ) ) {
1134	thjac	133
		134	// Aktuellen Pitch-Wert in Konfiguration �bernehmen
1139	thjac	135	pitchHover = actualPitchCount;
		136
1134	thjac	137	// Konfiguration dauerhaft speichern
1139	thjac	138	eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_MD_HOVER ], pitchHover );
		139	/*
1134	thjac	140	WriteParameterSet(
		141	GetActiveParamSetNumber(),
		142	(unsigned char *) &EE_Parameter.Kanalbelegung[0],
		143	STRUCT_PARAM_LAENGE );
1139	thjac	144	*/
1134	thjac	145	// Signalisieren
		146	beeptime = 500;
		147	}
		148	}
		149	break;
		150
		151	/* In diesem Zustand steht der MK am Boden und die Motoren laufen auf Leerlaufgas. Ein
		152	* kurzes Auslenken des Sticks nach oben schaltet in das Standgas �ber.
		153	*/
		154	case STATE_BEGIN:
		155
		156	// Begrenzung der Pitch-Beschleunigung am Boden
		157	delay = PARAM_PITCH_MD_DELAY0;
		158
		159	if( rawStickValue > PARAM_PITCH_STICK_THRESHOLD ) {
1139	thjac	160	pitchOffset = pitchHover;
		161	} else if( pitchOffset == pitchHover ) {
1134	thjac	162	state = STATE_BEGIN1;
		163	}
		164	break;
		165
		166	// MK soll erst abheben, weil sonst die H�henregelung am Boden schon greift
		167	case STATE_BEGIN1:
		168
		169	if( abs( rawStickValue ) > 10 ) {
		170
		171	// Begrenzung der Pitch-Beschleunigung im Flug
		172	delay = PARAM_PITCH_MD_DELAY1;
		173
		174	if( rawStickValue > 0 ) {
		175	state = STATE_READY;
		176	}
		177	}
		178	break;
		179
		180	/* Die Motoren laufen jetzt mindestens mit Standgas. Der MK ist bereit zum Abheben.
		181	* Das Minimalgas kann jetzt nicht mehr unterschritten werden.
		182	*/
		183	case STATE_READY:
		184
		185	// Ist ein Restart zul�ssig?
		186	if( PARAM_PITCH_RESTART_ENABLED ) {
		187
		188	/* Wenn der Gashebel ganz unten steht, Timer f�r Restart der Pitch-Regelung
		189	* starten. Hierf�r wird die Variable pitchNeutralTimer verwendet. */
		190	if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 120 ) {
		191	timer = PITCH_MIN2_TIMER;
		192	} else {
		193	timer--;
		194
		195	/* Gashebel steht seit PITCH_MIN2_TIMER ganz unten; jetzt erfolgt die Initialisierung. */
		196	if( !timer ) {
		197	state = STATE_BEGIN;
		198	pitchOffset = 0;
		199	targetPitchCount = 0;
		200	actualPitchCount = 0;
		201
		202	// Signalisieren
		203	beeptime = 500;
		204	}
		205	}
		206	}
		207
		208	// Min2-Gas einstellen f�r Lageregelung bei Minimalgas
		209	if( targetPitchCount < PARAM_PITCH_MIN2 ) {
		210	targetPitchCount = PARAM_PITCH_MIN2;
		211	}
		212
		213	// Stick ist innerhalb der Neutralstellung
		214	if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD ) {
		215
		216	// Aktuelle H�he festhalten (aktiviert noch nicht den Regler)
		217	altcon_lock();
		218
		219	// Timer neu setzen
		220	timer = PITCH_NEUTRAL_TIMER;
		221	state = STATE_WAIT;
		222	}
		223	break;
		224
		225	/* Der Stick ist in den f�r die Neutralstellung g�ltigen Wertebereich
		226	* gelangt. Nun darf innerhalb einer bestimmten Zeit keine Stick-Bewegung
		227	* erfolgen, um die automatische H�henregelung zu aktivieren. */
		228	case STATE_WAIT:
		229
		230	/* Stick ist innerhalb der Neutralstellung und
		231	Stick-Differenzial ist < 2 */
		232	if( abs( rawStickValue ) < PARAM_PITCH_STICK_THRESHOLD &&
		233	lastStickValue == stickValue ) {
		234
		235	timer--;
		236
		237	if( !timer ) {
		238	state = STATE_ACTIVATING;
		239	}
		240
		241	// Aktivierungskriterium nicht erf�llt, zur�ck in INACTIVE
		242	} else {
		243	state = STATE_READY;
		244	}
		245	break;
		246
		247	/* Die automatische H�henregelung wird jetzt aktiviert.
		248	*/
		249	case STATE_ACTIVATING:
		250
		251	// Aktivierung des H�henreglers mit der zuvor gemerkten Sollh�he
		252	altcon_start();
		253
		254	state = STATE_ACTIVE;
		255	break;
		256
		257	/* Die automatische H�henregelung ist aktiv. */
		258	case STATE_ACTIVE:
		259
		260	// Stick ist au�erhalb der Neutralstellung
		261	if( abs( rawStickValue ) > PARAM_PITCH_STICK_THRESHOLD ) {
		262
		263	// H�henregler deaktivieren
		264	altcon_stop();
		265
		266	pitchOffset -= altcon_avgerror() / 4;
		267	targetPitchCount = stickValue + pitchOffset;
		268	state = STATE_READY;
		269	}
		270	break;
		271	}
		272
		273	// Motoren sind aus
		274	} else {
		275
		276	/* Nach dem Einschalten der Motoren wird pitchOffset auf PARAM_PITCH_OVER gesetzt.
		277	*/
		278	actualPitchCount = 0;
		279	targetPitchCount = 0;
		280	pitchOffset = 0;
		281	stickValue = 0;
		282	state = STATE_INITIALIZE;
		283	}
		284
		285	if( !delayCounter ) {
		286
		287	/* Durch die Sollwertvorgabe kann hier eine einstellbare Tr�gheit auf dem Pitch-Wert
		288	* abgebildet werden. */
		289	int pitchDelta = targetPitchCount - actualPitchCount;
		290
		291	if( pitchDelta > 3 )
		292	pitchDelta = 3;
		293	if( pitchDelta < -3 )
		294	pitchDelta = -3;
		295
		296	actualPitchCount += pitchDelta;
		297
		298	delayCounter = delay + 1;
		299	}
		300
		301	delayCounter--;
		302
1139	thjac	303	if( actualPitchCount < 0 )
1134	thjac	304	actualPitchCount = 0;
1139	thjac	305
1134	thjac	306	// pitchCount als Debug-Wert rausschreiben
		307	DebugOut.Analog[26] = stickValue;
		308	DebugOut.Analog[28] = actualPitchCount;
		309	DebugOut.Analog[29] = pitchOffset;
		310
		311	return actualPitchCount;
		312	}

Subversion Repositories FlightCtrl

(root)/branches/thjac/V1_11/pitch_md.c - Rev 1139