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

Rev	Author	Line No.	Line
1134	thjac	1	/* pitch_neutral.c
		2	*
		3	* Copyright 2009 Thomas Jachmann
		4	*/
		5
		6	#include "main.h"
		7	#include "parameter.h"
		8	#include "fc.h"
		9	#include "altcon.h"
		10	#include "pitch.h"
		11	#include "pitch_neutral.h"
		12
1141	thjac	13	#define STATE_INITIALIZE 0x00 // Initialisierung nach Einschalten der Motoren
		14	#define STATE_SETUP 0x01 // Kalibrierung des Gas-Sticks
		15	#define STATE_SETUP1 0x02 //
		16	#define STATE_BEGIN 0x03 // Startphase
		17	#define STATE_BEGIN1 0x04 //
1134	thjac	18	#define STATE_INACTIVE 0x05 // Manuelle Kontrolle
		19	#define STATE_WAIT 0x06 // Warten auf Einschalten der H�henregelung
		20	#define STATE_ACTIVATING 0x07 // Aktivierung der H�henregelung
		21	#define STATE_ACTIVE 0x08 // H�henregelung ist aktiv
		22
		23
		24	static int stickValue = 0; // Aktueller Stick-Wert
		25	static int lastStickValue = 0; // Vorheriger Stick-Wert
		26	static int pitchOffset = 0; // Aktueller Grundgaswert in Neutralstellung
1141	thjac	27	static char state; // Zustand
1134	thjac	28	static int temp;
		29
1141	thjac	30	char pitchNeutralDiff = 0;
		31
		32
1134	thjac	33	/* Wird verwendet, um das Umschalten auf automatische H�henregelung
		34	* nach Erreichen der Neutralstellung zu verz�gern.
		35	*/
		36	static int pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
		37
		38
1141	thjac	39	void pitch_neutral_init( void ) {
		40	pitchNeutralDiff = eeprom_read_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ] );
		41	}
		42
		43
1134	thjac	44	/*
		45	* Berechnet den aktuellen Pitch-Wert f�r die Regelung
		46	*
1141	thjac	47	* Funktionsweise:
1134	thjac	48	*
1141	thjac	49	* 1. Vor Verwendung der Pitch-Steuerung mu� ein Parameter f�r den Gas-Stick eingestellt werden. Hierzu
		50	* bewegt man den Roll-Stick ganz nach au�en und startet die Motoren. Man befindet sich im Setup-Modus
		51	* und die Motoren laufen unabh�ngig vom Gas-Stick im Leerlauf. Im Display auf Seite 12 mu� nun der
		52	* beste Wert f�r "StickDiff" gefunden werden. Den Wert �ndern kann man mit dem Roll-Stick. Man bet�tigt
		53	* nun den Gas-Stick nach oben und l��t ihn los, so da� er in die Mittelstellung zur�ckspringt. Der
		54	* optimale Wert sorgt daf�r, da� das gewollte Loslassen von einer normalen Bewegung unterschieden wird.
		55	* Jedesmal, wenn die SW das Loslassen als solches erkannt hat, ert�nt ein Signal. Dieses darf jedoch
		56	* nicht ert�nen, wenn man den Stick mit normaler Geschwindigkeit in die Mittelstellung zur�ckbewegt.
		57	* Wurde ein guter Wert eingestellt, verl��t man den Setup-Modus durch Ausschalten der Motoren. Der
		58	* ermittelte Wert wird permanent im EEProm gespeichert und mu� nicht neu ermittelt werden.
1134	thjac	59	*
1141	thjac	60	* 2. Nach dem Einschalten der FC wird die aktuelle Gas-Stick-Position gemerkt und als Kalibrierungswert
		61	* f�r die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite
		62	* keine Rolle.
1134	thjac	63	*
1141	thjac	64	* 3. Nach Einschalten der Motoren geht der Stick in Neutralstellung. Diese Stick-Bewegung wird ignoriert
		65	* und die Motoren drehen mit dem eingestellten MinGas2. Ausgehend von der Neutralstellung wird nun
		66	* durch Bewegen des Sticks im oberen Bereich das Gas geregelt.
1134	thjac	67	*
1141	thjac	68	* 4. Das erstmalige Aktivieren der automatischen H�henregelung erfolgt durch Loslassen des Sticks im
		69	* Schwebeflug. Der zuvor aktuelle Stick-Wert wird als Wert in Neutralstellung �bernommen und die
		70	* automatische H�henregelung sofort aktiviert. Wichtig hierf�r ist die unter Punkt 1 vorgenommene
		71	* Parameterfindung. Stimmt dieser Wert nicht, f�hrt das zur Wegnahme des Gases und somit zum Sinken.
1134	thjac	72	*
1141	thjac	73	* 5. Sobald der Stick die Neutralstellung verl��t, wird die automatische H�henregelung deaktiviert
		74	* und der vorige Pitch-Wert als Wert der Neutralstellung �bernommen. Der Pitch l��t sich nun
		75	* �ber den gesamten Stick-Bereich regeln.
		76	*
		77	* 6. Erreicht der Stick ein weiteres Mal die Neutralstellung, wird die automatische H�henregelung
		78	* wieder aktiviert, jetzt jedoch immer mit einer zeitlichen Verz�gerung. Nur so ist ein
		79	* ungest�rtes manuelles Steuern m�glich.
		80	*
		81	* 7. Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt.
		82	* Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb
		83	* eines Wertes zu gelangen, der die Lageregelung au�er Funktion setzt, wird ein zweiter Wert f�r
		84	* Minimalgas konfiguriert, der greift, sobald erstmalig die automatische H�henregelung aktiviert wurde.
1134	thjac	85	*/
		86	int pitch_neutral_value( void ) {
		87
		88	int register pitchCount = 0;
		89
1141	thjac	90	// Sind die Motoren eingeschaltet? Ach ja, im Zustand STATE_SETUP sind die Motoren aus...
1134	thjac	91	if( MotorenEin ) {
		92
		93	// Vorigen Stick-Wert merken
		94	lastStickValue = stickValue;
		95
		96	/* StickValue exponentiell angleichen, da ausgehend von der Neutralstellung
		97	* nur jeweils die halbe Aufl�sung nach oben und unten zur Verf�gung steht. Bei einer
		98	* Multiplikation mit 2 lie�e sich das Gas im Schwebebereich nicht fein genug einstellen. */
1141	thjac	99	temp = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_initialStickValue();
1134	thjac	100	if( temp > 0 ) {
		101	temp = temp + ( ( temp * temp ) / 150 );
		102	} else {
		103	temp = temp - ( ( temp * temp ) / 150 );
		104	}
		105
1141	thjac	106	stickValue = temp;
		107
1134	thjac	108	/* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset
		109	* zuz�glich dem Stick-Wert. */
		110	pitchCount = stickValue + pitchOffset;
		111
		112	switch( state ) {
1141	thjac	113
		114	/* Entscheidet �ber Flugbetrieb oder Setup-Betrieb. F�r den Setup-Betrieb
		115	* mu� beim Einschalten der Motoren gleichzeitig der Roll-Stick ganz
		116	* bet�tigt werden (Richtung ist egal).
		117	*/
		118	case STATE_INITIALIZE:
		119
		120	if( abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) > 70 ) {
		121	state = STATE_SETUP;
		122	} else {
		123	state = STATE_BEGIN;
		124	}
		125	break;
		126
		127	case STATE_SETUP:
		128	if( stickValue < PARAM_PITCH_STICK_THRESHOLD &&
		129	abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) < PARAM_PITCH_STICK_THRESHOLD ) {
		130	state = STATE_SETUP1;
		131	}
		132
		133	// Im Setup immer mit Leerlaufgas
		134	pitchCount = 0;
		135
		136	break;
		137
		138	case STATE_SETUP1:
		139
		140	// Roll-Stick nach links
		141	if( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] < -20 ) {
		142	if( pitchNeutralDiff < 20 ) {
		143	pitchNeutralDiff++;
		144
		145	// Konfiguration dauerhaft speichern
		146	eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ], pitchNeutralDiff );
		147
		148	// Signalisieren
		149	beeptime = 500;
		150
		151	state = STATE_SETUP;
		152	}
		153	}
		154
		155	// Roll-Stick nach rechts
		156	if( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] > 20 ) {
		157	if( pitchNeutralDiff > 0 ) {
		158	pitchNeutralDiff--;
		159
		160	// Konfiguration dauerhaft speichern
		161	eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ], pitchNeutralDiff );
		162
		163	// Signalisieren
		164	beeptime = 500;
		165
		166	state = STATE_SETUP;
		167	}
		168	}
		169
		170	if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&
		171	( lastStickValue - stickValue >= pitchNeutralDiff ) ) {
		172
		173	state = STATE_SETUP;
		174
		175	// Signalisieren
		176	beeptime = 500;
		177	}
		178
		179	// Im Setup immer mit Leerlaufgas
		180	pitchCount = 0;
		181
		182	break;
		183
1134	thjac	184	case STATE_BEGIN:
		185
		186	// Schnelles Bewegen aus dem oberen Bereich des Sticks in Neutralstellung
		187	if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&
1141	thjac	188	( lastStickValue - stickValue >= pitchNeutralDiff ) ) {
1134	thjac	189
		190	pitchOffset = lastStickValue;
		191
1141	thjac	192	state = STATE_BEGIN1;
1134	thjac	193	pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
		194	}
		195	break;
		196
1141	thjac	197	case STATE_BEGIN1:
1134	thjac	198
		199	// W�hrend der Initialisierung das Gas konstant halten
		200	pitchCount = pitchOffset;
		201
		202	pitchNeutralTimer--;
		203
		204	/* L�uft der Timer ab, bevor der Stick die Neutralstellung erreicht,
		205	* wird die Aktion nicht als "schnelles Bewegen in Neutralstellung"
		206	* gedeutet. */
		207	if( !pitchNeutralTimer ) {
		208	pitchOffset = 0;
		209	state = STATE_BEGIN;
		210	}
		211
		212	// Ist die Neutralstellung erreicht?
		213	if( abs( stickValue ) <= PARAM_PITCH_STICK_THRESHOLD ) {
1141	thjac	214	state = STATE_ACTIVATING;
1134	thjac	215	}
		216	break;
		217
		218	/* Die H�henregelung ist per Konfiguration aktiviert, jedoch befindet
		219	* sich der Stick au�erhalb des als Neutralstellung anerkannten
		220	* Wertebereiches. Es wird manuell geregelt. */
		221	case STATE_INACTIVE:
		222
		223	// Ist ein Restart zul�ssig?
		224	if( PARAM_PITCH_RESTART_ENABLED ) {
		225
		226	/* Wenn der Gashebel ganz unten steht, Timer f�r Reduzierung des Minimalgaswertes
		227	* starten. Hierf�r wird die Variable pitchNeutralTimer verwendet. */
		228	if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 120 ) {
		229	pitchNeutralTimer = PITCH_MIN2_TIMER;
		230	} else {
		231	pitchNeutralTimer--;
		232
		233	/* Gashebel steht seit PITCH_MIN2_TIMER ganz unten; jetzt erfolgt die Initialisierung. */
		234	if( !pitchNeutralTimer ) {
		235	state = STATE_BEGIN;
		236	pitchOffset = 0;
		237
		238	// Signalisieren
		239	beeptime = 500;
		240	}
		241	}
		242	}
		243
		244	// Min2-Gas einstellen f�r Lageregelung bei Minimalgas
		245	if( pitchCount < PARAM_PITCH_MIN2 ) {
		246	pitchCount = PARAM_PITCH_MIN2;
		247	}
		248
		249	// Stick ist innerhalb der Neutralstellung
		250	if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD ) {
		251
		252	// Timer neu setzen
		253	pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
		254	state = STATE_WAIT;
		255	}
		256	break;
		257
		258	/* Der Stick ist in den f�r die Neutralstellung g�ltigen Wertebereich
		259	* gelangt. Nun darf innerhalb einer bestimmten Zeit keine Stick-Bewegung
		260	* erfolgen, um die automatische H�henregelung zu aktivieren. */
		261	case STATE_WAIT:
		262
		263	/* Stick ist innerhalb der Neutralstellung und
		264	Stick-Differenzial ist < 2 */
		265	if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD &&
		266	lastStickValue == stickValue ) {
		267
		268	pitchNeutralTimer--;
		269
		270	if( !pitchNeutralTimer ) {
		271	state = STATE_ACTIVATING;
		272	}
		273
		274	// Aktivierungskriterium nicht erf�llt, zur�ck in INACTIVE
		275	} else {
		276	state = STATE_INACTIVE;
		277	}
		278	break;
		279
		280	/* Die automatische H�henregelung wird jetzt aktiviert. Der aktuelle
		281	* Luftdruck wird gespeichert und notwendige Werte f�r den Regler
		282	* werden initialisiert. */
		283	case STATE_ACTIVATING:
		284
		285	// H�henregler starten
		286	altcon_start();
		287
		288	state = STATE_ACTIVE;
		289
		290	break;
		291
		292	/* Die automatische H�henregelung ist aktiv. */
		293	case STATE_ACTIVE:
		294
		295	// Stick ist au�erhalb der Neutralstellung
		296	if( abs( stickValue ) > PARAM_PITCH_STICK_THRESHOLD ) {
		297	pitchOffset -= altcon_avgerror() / 4;
		298	pitchCount = stickValue + pitchOffset;
		299
		300	// H�henregler abschalten
		301	altcon_stop();
		302
		303	state = STATE_INACTIVE;
		304	}
		305	break;
		306	}
		307
		308	// Motoren sind aus
		309	} else {
		310
		311	/* Nach dem Einschalten der Motoren darf pitchOffset keinen hohen Wert haben,
		312	* da der Kopter sonst sofort hochschie�en w�rde.
		313	*/
		314	pitchCount = 0;
		315	pitchOffset = 0;
		316	stickValue = 0;
1141	thjac	317	state = STATE_INITIALIZE;
1134	thjac	318	}
		319
		320	if( pitchOffset < 0 )
		321	pitchOffset = 0;
		322
		323	// Pitch-Wert darf nicht < 0 sein
		324	if( pitchCount < 0 ) {
		325	pitchCount = 0;
		326	}
		327
		328	// pitchCount als Debug-Wert rausschreiben
		329	DebugOut.Analog[26] = stickValue;
		330	DebugOut.Analog[28] = pitchCount;
		331	DebugOut.Analog[29] = pitchOffset;
		332
		333	return pitchCount;
		334	}

Subversion Repositories FlightCtrl

(root)/branches/thjac/V1_11/pitch_neutral.c - Rev 1141