0,0 → 1,375 |
/* pitch.c |
* |
* Pitch-Steuerung |
*/ |
|
#include "main.h" |
#include "parameter.h" |
#include "fc.h" |
#include "pitch.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_INACTIVE 0x05 // Manuelle Kontrolle |
#define STATE_WAIT 0x06 // Warten auf Einschalten der Höhenregelung |
#define STATE_ACTIVATING 0x07 // Aktivierung der Höhenregelung |
#define STATE_ACTIVE 0x08 // Höhenregelung ist aktiv |
|
#define PARAM_INIT_TIMEOUT 25 |
|
|
char initTimer = PARAM_INIT_TIMEOUT; |
int stickValue = 0; // Aktueller Stick-Wert |
int lastStickValue = 0; // Vorheriger Stick-Wert |
int zeroStickOffset = 0; // Offset für Stick-Kalibrierung |
int pitchOffset = 0; // Aktueller Grundgaswert in Neutralstellung |
char state = STATE_STARTUP_WAIT; // Zustand |
|
/* Wird verwendet, um das Umschalten auf automatische Höhenregelung |
* nach Erreichen der Neutralstellung zu verzögern. |
*/ |
int pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
|
// Variable zur Höhenregelung |
int pressureOffset = 0; |
int accZOffset = 0; |
int lastError = 0; |
int lastN = 0; // Zuletzt errechneter Fehlerwert |
int averageN = 0; |
long altIntegral = 0; |
int temp; // Temporäre Werte; wird mehrfach verwendet |
|
char pitchNeutralStartup = 1; // 1=Gas-Stick beim Einschalten in Mittelstellung |
|
|
extern unsigned char Notlandung; // aus fc.c |
|
|
/* |
* 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. |
* |
* 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. |
* |
* 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. |
* |
* 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. |
* |
* 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. |
* |
* 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( void ) { |
|
int register pitchCount = 0; |
|
// Sind die Motoren eingeschaltet? |
if( MotorenEin ) { |
|
// Vorigen Stick-Wert merken |
lastStickValue = stickValue; |
|
/* 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 ] ] - zeroStickOffset; |
if( temp > 0 ) { |
temp = temp + ( ( temp * temp ) / 150 ); |
} else { |
temp = temp - ( ( temp * temp ) / 150 ); |
} |
|
// Original-Stick-Wert holen und glätten |
stickValue = ( temp + 2 * lastStickValue ) / 3; |
|
/* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset |
* zuzüglich dem Stick-Wert. */ |
pitchCount = stickValue + pitchOffset; |
|
switch( state ) { |
|
case STATE_BEGIN: |
|
// Schnelles Bewegen aus dem oberen Bereich des Sticks in Neutralstellung |
if( ( lastStickValue > PITCH_STICK_THRESHOLD ) && |
( lastStickValue - stickValue >= PITCH_NEUTRAL_DIFF ) ) { |
|
pitchOffset = lastStickValue; |
|
state = STATE_INITIALIZING; |
pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
} |
break; |
|
case STATE_INITIALIZING: |
|
// Während der Initialisierung das Gas konstant halten |
pitchCount = pitchOffset; |
|
pitchNeutralTimer--; |
|
/* Läuft der Timer ab, bevor der Stick die Neutralstellung erreicht, |
* wird die Aktion nicht als "schnelles Bewegen in Neutralstellung" |
* gedeutet. */ |
if( !pitchNeutralTimer ) { |
pitchOffset = 0; |
state = STATE_BEGIN; |
} |
|
// Ist die Neutralstellung erreicht? |
if( abs( stickValue ) <= PITCH_STICK_THRESHOLD ) { |
|
// Ist die Höhenregelung aktiviert? |
if( EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG ) { |
state = STATE_ACTIVATING; |
} else { |
state = STATE_MANUAL; |
} |
} |
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. */ |
case STATE_INACTIVE: |
|
// Ist ein Restart zulässig? |
if( PARAM_PITCH_RESTART_ENABLED ) { |
|
/* Wenn der Gashebel ganz unten steht, Timer für Reduzierung des Minimalgaswertes |
* starten. Hierfür wird die Variable pitchNeutralTimer verwendet. */ |
if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 120 ) { |
pitchNeutralTimer = PITCH_MIN2_TIMER; |
} else { |
pitchNeutralTimer--; |
|
/* Gashebel steht seit PITCH_MIN2_TIMER ganz unten; jetzt erfolgt die Initialisierung. */ |
if( !pitchNeutralTimer ) { |
state = STATE_BEGIN; |
pitchOffset = 0; |
|
// Signalisieren |
beeptime = 500; |
} |
} |
} |
|
// Min2-Gas einstellen für Lageregelung bei Minimalgas |
if( pitchCount < PARAM_PITCH_MIN2 ) { |
pitchCount = PARAM_PITCH_MIN2; |
} |
|
// Stick ist innerhalb der Neutralstellung |
if( abs( stickValue ) < PITCH_STICK_THRESHOLD ) { |
|
// Timer neu setzen |
pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
state = STATE_WAIT; |
} |
break; |
|
/* Der Stick ist in den für die Neutralstellung gültigen Wertebereich |
* gelangt. Nun darf innerhalb einer bestimmten Zeit keine Stick-Bewegung |
* erfolgen, um die automatische Höhenregelung zu aktivieren. */ |
case STATE_WAIT: |
|
/* Stick ist innerhalb der Neutralstellung und |
Stick-Differenzial ist < 2 */ |
if( abs( stickValue ) < PITCH_STICK_THRESHOLD && |
lastStickValue == stickValue ) { |
|
pitchNeutralTimer--; |
|
if( !pitchNeutralTimer ) { |
state = STATE_ACTIVATING; |
} |
|
// Aktivierungskriterium nicht erfüllt, zurück in INACTIVE |
} else { |
state = STATE_INACTIVE; |
} |
break; |
|
/* Die automatische Höhenregelung wird jetzt aktiviert. Der aktuelle |
* Luftdruck wird gespeichert und notwendige Werte für den Regler |
* werden initialisiert. */ |
case STATE_ACTIVATING: |
|
// Die Referenzhöhe soll zu Beginn der Neutralstellung genommen werden |
pressureOffset = airPressure; |
accZOffset = Mess_Integral_Hoch / 128; |
|
lastError = 0; |
lastN = 0; |
averageN = 0; |
altIntegral = 0L; |
state = STATE_ACTIVE; |
|
// Einschalten der Höhenregelung signalisieren |
beeptime = 500; |
|
break; |
|
/* Die automatische Höhenregelung ist aktiv. */ |
case STATE_ACTIVE: |
|
// Stick ist außerhalb der Neutralstellung |
if( abs( stickValue ) > PITCH_STICK_THRESHOLD ) { |
pitchOffset -= averageN / 4; |
pitchCount = stickValue + pitchOffset; |
lastN = 0; |
state = STATE_INACTIVE; |
|
// Abschaltung der Höhenregelung signalisieren |
beeptime = 500; |
} |
break; |
} |
|
// Motoren sind aus |
} else { |
|
switch( state ) { |
|
case STATE_STARTUP_WAIT: |
|
if( !initTimer-- ) { |
state = STATE_STARTUP_INIT; |
} |
break; |
|
case STATE_STARTUP_INIT: |
|
/* Lädt den beim Einschalten der FC anliegenden Stickwert als |
* Offset für die Kalibrierung des Gas-Sticks. */ |
zeroStickOffset = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ]; |
|
if( zeroStickOffset < -75 ) |
pitchNeutralStartup = 0; |
|
// Die Einschaltphase ist jetzt beendet |
state = STATE_BEGIN; |
|
break; |
|
default: |
|
/* Nach dem Einschalten der Motoren darf pitchOffset keinen hohen Wert haben, |
* da der Kopter sonst sofort hochschießen würde. |
*/ |
pitchCount = 0; |
pitchOffset = 0; |
stickValue = 0; |
state = STATE_BEGIN; |
} |
} |
|
if( pitchOffset < 0 ) |
pitchOffset = 0; |
|
// Pitch-Wert darf nicht < 0 sein |
if( pitchCount < 0 ) { |
pitchCount = 0; |
} |
|
// pitchCount als Debug-Wert rausschreiben |
DebugOut.Analog[26] = stickValue; |
DebugOut.Analog[28] = pitchCount; |
DebugOut.Analog[29] = pitchOffset; |
|
return pitchCount; |
} |
|
|
/* |
* Berechnet den Korrekturwert für die Höhenregelung |
*/ |
int altitudeController( void ) { |
|
int register n = 0; |
int register error; |
|
if( ( state == STATE_ACTIVE ) && !Notlandung ) { |
|
// Fehlerwert für Regler ermitteln |
error = airPressure - pressureOffset; |
|
// Proportionalanteil |
n = ( PARAM_ALT_P * error ) / 4; // dividiert durch ( 16 / STICK_GAIN ) = 16 / 4 = 4 |
|
// Integralanteil |
altIntegral += ( PARAM_ALT_I * error ) / 4; |
|
// Integral begrenzen |
if( altIntegral > PARAM_ALT_INT_MAX ) |
altIntegral = PARAM_ALT_INT_MAX; |
else if( altIntegral < -PARAM_ALT_INT_MAX ) |
altIntegral = -PARAM_ALT_INT_MAX; |
|
n += altIntegral / 4000; |
|
// Differenzialanteil |
n += ( PARAM_ALT_D * ( error - lastError ) ) / 2; |
|
// ACC-Z-Integral zur Dämpfung einbeziehen |
temp = ( ( ( Mess_Integral_Hoch / 128 ) - accZOffset ) * (signed long) PARAM_ALT_ACC ) / 32; |
|
// Dämpfung limitieren |
if( temp > ( 70 * STICK_GAIN ) ) |
temp = 70 * STICK_GAIN; |
else if( temp < -( 70 * STICK_GAIN ) ) |
temp = -( 70 * STICK_GAIN ); |
|
n += temp; |
|
// Verstärkung des Fehlerwertes zur Anpassung des Gewichtes |
n = n * PARAM_ALT_GAIN / 10; |
|
int altMax = PARAM_ALT_MAX * STICK_GAIN; |
|
// Limitierung des Korrekturwertes |
if( n > altMax ) |
n = altMax; |
else if( n < -altMax ) |
n = -altMax; |
|
lastN = n; |
lastError = error; |
|
/* Berechnung einer exponentiellen Glättung für den neuen Gaswert bei Verlassen der |
* Höhenregelung. Dies soll ein zu heftiges Reagieren mindern. */ |
averageN = averageN + PARAM_EXP_SMOOTHING_FACTOR * ( n - averageN ) / 100; |
} |
|
DebugOut.Analog[30] = altIntegral / 4000; |
DebugOut.Analog[27] = n; |
|
return n; |
} |