Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1140 → Rev 1141

/branches/thjac/V1_11/pitch_neutral.c
10,11 → 10,11
#include "pitch.h"
#include "pitch_neutral.h"
 
#define STATE_STARTUP_WAIT 0x00 // Init-Timeout beim Einschalten abwarten
#define STATE_STARTUP_INIT 0x01 // Initialisierung beim Einschalten
#define STATE_BEGIN 0x02 // Anfangszustand nach Einschalten der Motoren
#define STATE_INITIALIZING 0x03 // Initialisierungsphase
#define STATE_MANUAL 0x04 // Manuelle Kontrolle, Höhenregelung in Konfiguration deaktiviert
#define STATE_INITIALIZE 0x00 // Initialisierung nach Einschalten der Motoren
#define STATE_SETUP 0x01 // Kalibrierung des Gas-Sticks
#define STATE_SETUP1 0x02 //
#define STATE_BEGIN 0x03 // Startphase
#define STATE_BEGIN1 0x04 //
#define STATE_INACTIVE 0x05 // Manuelle Kontrolle
#define STATE_WAIT 0x06 // Warten auf Einschalten der Höhenregelung
#define STATE_ACTIVATING 0x07 // Aktivierung der Höhenregelung
24,9 → 24,12
static int stickValue = 0; // Aktueller Stick-Wert
static int lastStickValue = 0; // Vorheriger Stick-Wert
static int pitchOffset = 0; // Aktueller Grundgaswert in Neutralstellung
static char state = STATE_STARTUP_WAIT; // Zustand
static char state; // Zustand
static int temp;
 
char pitchNeutralDiff = 0;
 
 
/* Wird verwendet, um das Umschalten auf automatische Höhenregelung
* nach Erreichen der Neutralstellung zu verzögern.
*/
33,39 → 36,58
static int pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
 
 
void pitch_neutral_init( void ) {
pitchNeutralDiff = eeprom_read_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ] );
}
 
 
/*
* Berechnet den aktuellen Pitch-Wert für die Regelung
*
* Nach dem Einschalten der FC wird der aktuelle Gas-Stick-Wert gelesen und als Kalibrierungswert
* für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite
* keine Rolle.
* Funktionsweise:
*
* Nach Einschalten der Motoren geht der Stick in Neutralstellung. Diese Stick-Bewegung wird ignoriert
* und die Motoren drehen mit dem eingestellten MinGas2. Ausgehend von der Neutralstellung wird nun
* durch Bewegen des Sticks im oberen Bereich das Gas geregelt.
* 1. Vor Verwendung der Pitch-Steuerung muß ein Parameter für den Gas-Stick eingestellt werden. Hierzu
* bewegt man den Roll-Stick ganz nach außen und startet die Motoren. Man befindet sich im Setup-Modus
* und die Motoren laufen unabhängig vom Gas-Stick im Leerlauf. Im Display auf Seite 12 muß nun der
* beste Wert für "StickDiff" gefunden werden. Den Wert ändern kann man mit dem Roll-Stick. Man betätigt
* nun den Gas-Stick nach oben und läßt ihn los, so daß er in die Mittelstellung zurückspringt. Der
* optimale Wert sorgt dafür, daß das gewollte Loslassen von einer normalen Bewegung unterschieden wird.
* Jedesmal, wenn die SW das Loslassen als solches erkannt hat, ertönt ein Signal. Dieses darf jedoch
* nicht ertönen, wenn man den Stick mit normaler Geschwindigkeit in die Mittelstellung zurückbewegt.
* Wurde ein guter Wert eingestellt, verläßt man den Setup-Modus durch Ausschalten der Motoren. Der
* ermittelte Wert wird permanent im EEProm gespeichert und muß nicht neu ermittelt werden.
*
* Das erstmalige Aktivieren der automatischen Höhenregelung erfolgt durch Loslassen des Sticks im
* Schwebeflug. Der zuvor aktuelle Stick-Wert wird als Wert in Neutralstellung übernommen und die
* automatische Höhenregelung sofort aktiviert.
* 2. Nach dem Einschalten der FC wird die aktuelle Gas-Stick-Position gemerkt und als Kalibrierungswert
* für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite
* keine Rolle.
*
* Sobald der Stick die Neutralstellung verläßt, wird die automatische Höhenregelung deaktiviert
* und der vorige Pitch-Wert als Wert der Neutralstellung übernommen. Der Pitch läßt sich nun
* über den gesamten Stick-Bereich regeln.
* 3. Nach Einschalten der Motoren geht der Stick in Neutralstellung. Diese Stick-Bewegung wird ignoriert
* und die Motoren drehen mit dem eingestellten MinGas2. Ausgehend von der Neutralstellung wird nun
* durch Bewegen des Sticks im oberen Bereich das Gas geregelt.
*
* Erreicht der Stick ein weiteres Mal die Neutralstellung, wird die automatische Höhenregelung
* wieder aktiviert, jetzt jedoch immer mit einer zeitlichen Verzögerung. Nur so ist ein
* ungestörtes manuelles Steuern möglich.
* 4. Das erstmalige Aktivieren der automatischen Höhenregelung erfolgt durch Loslassen des Sticks im
* Schwebeflug. Der zuvor aktuelle Stick-Wert wird als Wert in Neutralstellung übernommen und die
* automatische Höhenregelung sofort aktiviert. Wichtig hierfür ist die unter Punkt 1 vorgenommene
* Parameterfindung. Stimmt dieser Wert nicht, führt das zur Wegnahme des Gases und somit zum Sinken.
*
* Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt.
* Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb
* eines Wertes zu gelangen, der die Lageregelung außer Funktion setzt, wird ein zweiter Wert für
* Minimalgas konfiguriert, der greift, sobald erstmalig die automatische Höhenregelung aktiviert wurde.
* 5. Sobald der Stick die Neutralstellung verläßt, wird die automatische Höhenregelung deaktiviert
* und der vorige Pitch-Wert als Wert der Neutralstellung übernommen. Der Pitch läßt sich nun
* über den gesamten Stick-Bereich regeln.
*
* 6. Erreicht der Stick ein weiteres Mal die Neutralstellung, wird die automatische Höhenregelung
* wieder aktiviert, jetzt jedoch immer mit einer zeitlichen Verzögerung. Nur so ist ein
* ungestörtes manuelles Steuern möglich.
*
* 7. Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt.
* Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb
* eines Wertes zu gelangen, der die Lageregelung außer Funktion setzt, wird ein zweiter Wert für
* Minimalgas konfiguriert, der greift, sobald erstmalig die automatische Höhenregelung aktiviert wurde.
*/
int pitch_neutral_value( void ) {
 
int register pitchCount = 0;
// Sind die Motoren eingeschaltet?
// Sind die Motoren eingeschaltet? Ach ja, im Zustand STATE_SETUP sind die Motoren aus...
if( MotorenEin ) {
 
// Vorigen Stick-Wert merken
74,7 → 96,7
/* StickValue exponentiell angleichen, da ausgehend von der Neutralstellung
* nur jeweils die halbe Auflösung nach oben und unten zur Verfügung steht. Bei einer
* Multiplikation mit 2 ließe sich das Gas im Schwebebereich nicht fein genug einstellen. */
temp = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_stickoffset();
temp = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_initialStickValue();
if( temp > 0 ) {
temp = temp + ( ( temp * temp ) / 150 );
} else {
81,29 → 103,98
temp = temp - ( ( temp * temp ) / 150 );
}
 
// Original-Stick-Wert holen und glätten
stickValue = ( temp + 2 * lastStickValue ) / 3;
 
stickValue = temp;
/* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset
* zuzüglich dem Stick-Wert. */
pitchCount = stickValue + pitchOffset;
 
switch( state ) {
 
/* Entscheidet über Flugbetrieb oder Setup-Betrieb. Für den Setup-Betrieb
* muß beim Einschalten der Motoren gleichzeitig der Roll-Stick ganz
* betätigt werden (Richtung ist egal).
*/
case STATE_INITIALIZE:
 
if( abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) > 70 ) {
state = STATE_SETUP;
} else {
state = STATE_BEGIN;
}
break;
 
case STATE_SETUP:
if( stickValue < PARAM_PITCH_STICK_THRESHOLD &&
abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) < PARAM_PITCH_STICK_THRESHOLD ) {
state = STATE_SETUP1;
}
// Im Setup immer mit Leerlaufgas
pitchCount = 0;
break;
case STATE_SETUP1:
// Roll-Stick nach links
if( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] < -20 ) {
if( pitchNeutralDiff < 20 ) {
pitchNeutralDiff++;
// Konfiguration dauerhaft speichern
eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ], pitchNeutralDiff );
// Signalisieren
beeptime = 500;
state = STATE_SETUP;
}
}
 
// Roll-Stick nach rechts
if( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] > 20 ) {
if( pitchNeutralDiff > 0 ) {
pitchNeutralDiff--;
 
// Konfiguration dauerhaft speichern
eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ], pitchNeutralDiff );
 
// Signalisieren
beeptime = 500;
state = STATE_SETUP;
}
}
 
if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&
( lastStickValue - stickValue >= pitchNeutralDiff ) ) {
state = STATE_SETUP;
// Signalisieren
beeptime = 500;
}
// Im Setup immer mit Leerlaufgas
pitchCount = 0;
break;
 
case STATE_BEGIN:
 
// Schnelles Bewegen aus dem oberen Bereich des Sticks in Neutralstellung
if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&
( lastStickValue - stickValue >= PARAM_PITCH_NEUTRAL_DIFF ) ) {
( lastStickValue - stickValue >= pitchNeutralDiff ) ) {
 
pitchOffset = lastStickValue;
state = STATE_INITIALIZING;
state = STATE_BEGIN1;
pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
}
break;
 
case STATE_INITIALIZING:
case STATE_BEGIN1:
 
// Während der Initialisierung das Gas konstant halten
pitchCount = pitchOffset;
120,26 → 211,10
 
// Ist die Neutralstellung erreicht?
if( abs( stickValue ) <= PARAM_PITCH_STICK_THRESHOLD ) {
 
// Ist die Höhenregelung aktiviert?
if( EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG ) {
state = STATE_ACTIVATING;
} else {
state = STATE_MANUAL;
}
state = STATE_ACTIVATING;
}
break;
 
/* Wenn die Höhenregelung per Konfiguration deaktiviert ist, verbleibt
* die Funktion in diesem Zustand. */
case STATE_MANUAL:
// Min2-Gas einstellen für Lageregelung bei Minimalgas
if( pitchCount < PARAM_PITCH_MIN2 ) {
pitchCount = PARAM_PITCH_MIN2;
}
break;
 
/* Die Höhenregelung ist per Konfiguration aktiviert, jedoch befindet
* sich der Stick außerhalb des als Neutralstellung anerkannten
* Wertebereiches. Es wird manuell geregelt. */
239,7 → 314,7
pitchCount = 0;
pitchOffset = 0;
stickValue = 0;
state = STATE_BEGIN;
state = STATE_INITIALIZE;
}
 
if( pitchOffset < 0 )