Subversion Repositories FlightCtrl

/* pitch.c

 *

 * Pitch-Steuerung

 */

#include "main.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     lastAirPressure         = 0;

int     lastN                   = 0;                            // Zuletzt errechneter Fehlerwert

long    altIntegral             = 0;

int     temp;                                                   // Temporäre Werte; wird mehrfach verwendet

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;

        // 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 ) &&

                                ( lastStickValue - stickValue >= PITCH_NEUTRAL_DIFF ) ) {

                                        pitchOffset += lastStickValue;

                                        state        = STATE_INITIALIZING;

                                }

                                break;

                        case STATE_INITIALIZING:

                                if( abs( stickValue ) > PITCH_STICK_THRESHOLD ) {

                                        pitchCount = pitchOffset;

                                } else {

                                        // 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 < PITCH_MIN2 ) {

                                        pitchCount = 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:

                                // Min2-Gas einstellen für Lageregelung bei Minimalgas

                                if( pitchCount < PITCH_MIN2 ) {

                                        pitchCount = 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 &&

                                    abs( lastStickValue - stickValue ) < 2 ) {

                                        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;

                                lastAirPressure = 0;

                                lastN        = 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 -= lastN / 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 ] ];

                                // 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[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 / 1000;                // 4 * Reglerfrequenz = 4 * 500Hz = 1000

                // Differenzialanteil

                n           += ( PARAM_ALT_D * ( airPressure - lastAirPressure ) ) / 2;

                lastAirPressure = airPressure;

                // 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_SHIFT ) )

                        temp = 70 << STICK_GAIN_SHIFT;

                else if( temp < -( 70 << STICK_GAIN_SHIFT ) )

                        temp = -( 70 << STICK_GAIN_SHIFT );

                n += temp;

                // Verstärkung des Fehlerwertes zur Anpassung des Gewichtes

                n = n * PARAM_ALT_GAIN / 10;

                int altMax = PARAM_ALT_MAX << STICK_GAIN_SHIFT;

                // Limitierung des Korrekturwertes

                if( n > altMax )

                        n = altMax;

                else if( n < -altMax )

                        n = -altMax;

                // Korrekturwert glätten

                n     = ( n + 15 * lastN ) / 16;

                lastN = n;

        }

//      DebugOut.Analog[28] = altIntegral;

//      DebugOut.Analog[27] = n;

        return n;

}