Subversion Repositories FlightCtrl

/* 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;
}