Subversion Repositories FlightCtrl

/* pitch_md.c
 *
 * Copyright 2009 Thomas Jachmann
 *
 * Die in dieser Quelldatei enthaltenen Algorithmen ermöglichen eine MD-ähnliche Pitch-Steuerung
 * für den MK.
 */

#include "main.h"
#include "parameter.h"
#include "fc.h"
#include "pitch.h"
#include "altcon.h"


#define STATE_INITIALIZE                0x01                    // Anfangszustand nach Einschalten der Motoren
#define STATE_SETUP                     0x02                    // Ermittlung von PARAM_PITCH_MD_HOVER
#define STATE_BEGIN                     0x03                    // Anfangszustand für Flugbetrieb
#define STATE_BEGIN1                    0x04                    // Anfangszustand für Flugbetrieb
#define STATE_READY                     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


int     stickValue              = 0;                            // Aktueller Stick-Wert
int     lastStickValue          = 0;                            // Vorheriger Stick-Wert
int     actualPitchCount;                                       // Soll-Pitch-Wert
int     targetPitchCount;                                       // Ist-Pitch-Wert
int     pitchOffset;                                            // Aktueller Grundgaswert in Neutralstellung
char    state;                                                  // Zustand
int     timer;
int     delay                   = 0;
int     delayCounter            = 0;
int     temp;                                                   // Temporäre Werte; wird mehrfach verwendet


/*
 * Berechnet den aktuellen Pitch-Wert für die Regelung
 *
 * Nachdem die Motoren eingeschaltet sind, wird der Pitch-Stick losgelassen und damit in Mittelstellung
 * gebracht. Die Motoren laufen zu dieser Zeit im Leerlaufgas. Vor dem Abheben müssen die Motoren in
 * das Standgas gebracht werden. Dies geschieht durch minimales Gasgeben. Durch weiteres Gasgeben und
 * nehmen kann abgehoben und geflogen werden.
 *
 * Erreicht der Stick während des Fluges die Neutralstellung und verbleibt dort für ca. 1 Sekunde ohne
 * bewegt zu werden, aktiviert sich die Höhenregelung und hält den MK auf der aktuellen Höhe.
 *
 * 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_md_value( void ) {

        int register rawStickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_stickoffset();
       
        // 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 = rawStickValue;
                if( temp > 0 ) {
                        temp = temp + ( ( temp * temp ) / 150 );
                } else {
                        temp = temp - ( ( temp * temp ) / 150 );
                }

                stickValue = temp;
               
                /* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset
                 * zuzüglich dem Stick-Wert. */
                targetPitchCount = 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;

                        /* Erlaubt die Ermittlung des Parameters PARAM_PITCH_MD_HOVER. Hierzu wird soviel Gas
                         * gegeben, bis der MK kurz vor dem Abheben ist, jedoch noch stabil steht. Um den
                         * Gaswert dauerhaft zu speichern, muß der Stick an der Position verweilen, bis
                         * der Summer die Übernahme akustisch quittiert. Dann müssen die Motoren wieder
                         * ausgeschaltet werden, da dieser Modus nicht für den Flug vorgesehen ist.
                         */            
                        case STATE_SETUP:

                                // Im Setup-Modus soll das Gas nicht träge reagieren
                                actualPitchCount = targetPitchCount;

                                if( rawStickValue < 20 || ( stickValue - lastStickValue ) ) {
                                        timer = PARAM_TIMER_2S;
                                }

                                /* Der Stick befindet sich eindeutig in der oberen Hälfte und wurde
                                 * seit dem letzten Zyklus nicht bewegt. */
                                else {
                                        timer--;

                                        /* Die Verweilzeit ist abgelaufen und der aktuelle Pitch-Wert
                                         * entspricht nicht dem bereits gespeicherten Wert. */
                                        if( !timer && ( PARAM_PITCH_MD_HOVER != actualPitchCount ) ) {

                                                // Aktuellen Pitch-Wert in Konfiguration übernehmen
                                                PARAM_PITCH_MD_HOVER = actualPitchCount;

                                                // Konfiguration dauerhaft speichern
                                                WriteParameterSet(
                                                        GetActiveParamSetNumber(),
                                                        (unsigned char *) &EE_Parameter.Kanalbelegung[0],
                                                        STRUCT_PARAM_LAENGE );

                                                // Signalisieren
                                                beeptime = 500;
                                        }
                                }
                                break;

                        /* In diesem Zustand steht der MK am Boden und die Motoren laufen auf Leerlaufgas. Ein
                         * kurzes Auslenken des Sticks nach oben schaltet in das Standgas über.
                         */
                        case STATE_BEGIN:

                                // Begrenzung der Pitch-Beschleunigung am Boden
                                delay = PARAM_PITCH_MD_DELAY0;

                                if( rawStickValue > PARAM_PITCH_STICK_THRESHOLD ) {
                                        pitchOffset = PARAM_PITCH_MD_HOVER;
                                } else if( pitchOffset == PARAM_PITCH_MD_HOVER ) {
                                        state = STATE_BEGIN1;
                                }
                                break;

                        // MK soll erst abheben, weil sonst die Höhenregelung am Boden schon greift
                        case STATE_BEGIN1:
                       
                                if( abs( rawStickValue ) > 10 ) {

                                        // Begrenzung der Pitch-Beschleunigung im Flug
                                        delay = PARAM_PITCH_MD_DELAY1;
                                               
                                        if( rawStickValue > 0 ) {
                                                state = STATE_READY;
                                        }
                                }
                                break;

                        /* Die Motoren laufen jetzt mindestens mit Standgas. Der MK ist bereit zum Abheben.
                         * Das Minimalgas kann jetzt nicht mehr unterschritten werden.
                         */
                        case STATE_READY:
                       
                                // Ist ein Restart zulässig?
                                if( PARAM_PITCH_RESTART_ENABLED ) {
                               
                                        /* Wenn der Gashebel ganz unten steht, Timer für Restart der Pitch-Regelung
                                         * starten. Hierfür wird die Variable pitchNeutralTimer verwendet. */
                                        if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 120 ) {
                                                timer = PITCH_MIN2_TIMER;
                                        } else {
                                                timer--;
                                       
                                                /* Gashebel steht seit PITCH_MIN2_TIMER ganz unten; jetzt erfolgt die Initialisierung. */
                                                if( !timer ) {
                                                        state            = STATE_BEGIN;
                                                        pitchOffset      = 0;
                                                        targetPitchCount = 0;
                                                        actualPitchCount = 0;

                                                        // Signalisieren
                                                        beeptime    = 500;
                                                }
                                        }
                                }
                               
                                // Min2-Gas einstellen für Lageregelung bei Minimalgas
                                if( targetPitchCount < PARAM_PITCH_MIN2 ) {
                                        targetPitchCount = PARAM_PITCH_MIN2;
                                }
                               
                                // Stick ist innerhalb der Neutralstellung
                                if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD ) {
                               
                                        // Aktuelle Höhe festhalten (aktiviert noch nicht den Regler)
                                        altcon_lock();

                                        // Timer neu setzen
                                        timer = 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( rawStickValue ) < PARAM_PITCH_STICK_THRESHOLD &&
                                    lastStickValue == stickValue ) {
                               
                                        timer--;
                                       
                                        if( !timer ) {
                                                state = STATE_ACTIVATING;
                                        }

                                // Aktivierungskriterium nicht erfüllt, zurück in INACTIVE
                                } else {
                                        state = STATE_READY;
                                }
                                break;
                       
                        /* Die automatische Höhenregelung wird jetzt aktiviert.
                         */
                        case STATE_ACTIVATING:

                                // Aktivierung des Höhenreglers mit der zuvor gemerkten Sollhöhe
                                altcon_start();

                                state = STATE_ACTIVE;
                                break;

                        /* Die automatische Höhenregelung ist aktiv. */                
                        case STATE_ACTIVE:

                                // Stick ist außerhalb der Neutralstellung
                                if( abs( rawStickValue ) > PARAM_PITCH_STICK_THRESHOLD ) {

                                        // Höhenregler deaktivieren
                                        altcon_stop();

                                        pitchOffset     -= altcon_avgerror() / 4;
                                        targetPitchCount = stickValue + pitchOffset;
                                        state            = STATE_READY;
                                }
                                break;
                }

        // Motoren sind aus
        } else {
       
                /* Nach dem Einschalten der Motoren wird pitchOffset auf PARAM_PITCH_OVER gesetzt.
                 */
                actualPitchCount = 0;
                targetPitchCount = 0;
                pitchOffset      = 0;
                stickValue       = 0;
                state            = STATE_INITIALIZE;
        }

        if( pitchOffset < 0 )
                pitchOffset = 0;

        if( !delayCounter ) {
       
                /* Durch die Sollwertvorgabe kann hier eine einstellbare Trägheit auf dem Pitch-Wert
                 * abgebildet werden. */
                int pitchDelta = targetPitchCount - actualPitchCount;

                if( pitchDelta > 3 )
                        pitchDelta = 3;
                if( pitchDelta < -3 )
                        pitchDelta = -3;

                actualPitchCount += pitchDelta;
               
                delayCounter = delay + 1;
        }
       
        delayCounter--;

        // Pitch-Wert darf nicht < 0 sein
        if( actualPitchCount < 0 ) {
                actualPitchCount = 0;
        }

        // pitchCount als Debug-Wert rausschreiben
        DebugOut.Analog[26] = stickValue;
        DebugOut.Analog[28] = actualPitchCount;
        DebugOut.Analog[29] = pitchOffset;

        return actualPitchCount;
}