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_SETUP_HOVER               0x03                    // Konfiguration pitchHover

#define STATE_SETUP_STICK_DELTA         0x04                    // Konfiguration pitchNeutralDiff

#define STATE_SETUP_STICK_DELTA1        0x05

#define STATE_BEGIN                     0x06                    // Anfangszustand für Flugbetrieb

#define STATE_BEGIN1                    0x07                    // Anfangszustand für Flugbetrieb

#define STATE_READY0                    0x08                    // Manuelle Kontrolle

#define STATE_READY                     0x09                    // Manuelle Kontrolle

#define STATE_READY1                    0x0a

#define STATE_WAIT                      0x0b                    // Warten auf Einschalten der Höhenregelung

#define STATE_ACTIVATING                0x0c                    // Aktivierung der Höhenregelung

#define STATE_ACTIVE                    0x0d                    // Höhenregelung ist aktiv

#define STATE_RESTART                   0x0e                    // Neustart

#define STATE_AUTOSTART0                0x0f                    // Autostartsequenz 0

#define STATE_AUTOSTART1                0x10                    // Autostartsequenz 1

static int      lastStickValue          = 0;                    // Vorheriger Stick-Wert

static int      actualPitchCount        = 0;                    // Soll-Pitch-Wert

static int      targetPitchCount        = 0;                    // Ist-Pitch-Wert

static int      pitchOffset;                                    // Aktueller Grundgaswert in Neutralstellung

static char     state;                                          // Zustand

static int      timer;

static int      delay                   = 0;

static int      delayCounter            = 0;

static int      peakPitchCount;

unsigned char   pitchHover              = 0;                    // Schwebegaswert

unsigned char   pitchStand              = 0;                    // Standgaswert

unsigned char   pitchNeutralDiff        = 0;

void pitch_md_init( void ) {

    pitchHover       = eeprom_read_byte( &EEPromArray[ EEPROM_ADR_PITCH_MD_HOVER ] );

    pitchStand       = pitchHover * PARAM_PITCH_HOVER_PERCENT / 100;

    pitchNeutralDiff = eeprom_read_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ] );

}

/*

 * Berechnet den aktuellen Pitch-Wert für die Regelung

 *

 * Funktionsweise:

 *

 * 1. Einmalig muß man das Schwebegas einstellen. Schwebegas ist ein neuer Parameter.

 *    Dies tut man durch Betätigen des Roll-Sticks nach links (Vollausschlag) und

 *    Starten der Motoren. Nun gibt man soviel Gas (in der oberen Hälfte des Stick-Bereichs), bis der MK

 *    kurz über dem Boden schwebt (im Bodeneffekt). Man hält diese Stick-Position solange, bis ein

 *    Signal ertönt (ca. 1 Sekunde). Das Signal ist die Bestätigung für die Übernahme des aktuellen Gaswertes

 *    als Schwebegas. Der Wert wird dauerhaft gespeichert und kann im LCD-Display des MK-Tool im letzten Bild

 *    überprüft werden. Diese Kalibrierung muß mit vollem Akku und nach jeder Gewichtsänderung des MK einmal

 *    durchgeführt werden.

 *

 * 2. Für den Flug startet man die Motoren und läßt den Stick los. Die Motoren laufen im Leerlaufgas.

 *

 * 3. Man kann nun entweder mit oder ohne Unterstützung der Höhenregelung fliegen.

 *

 * Mit Unterstützung geht man wie folgt vor:

 *

 *    Die Startsequenz wird durch Betätigung des Gasknüppels nach oben eingeleitet, bis ein akustisches Signal ertönt.

 *    Hierbei drehen die Motoren noch im Leerlauf. Erst nach Rückstellung des Sticks in die Mittelposition beschleunigen

 *    die Motoren langsam bis zum unter Punkt 1 eingestellten Schwebegas. Nun schaltet automatisch die Höhenregelung zu

 *    und der MK steigt bis auf eine im UserParameter eingestellte Höhe (normalerweise 1 Meter). In Mittelstellung ist

 *    nun die Höhenregelung aktiv, im oberen und unteren Stick-Bereich steigt oder sinkt der MK.

 *

 * Ohne Unterstützung geht man wie folgt vor:

 *

 *    Man schaltet die Motoren durch Betätigung des Gasknüppels nach oben in das Standgas. Hier reicht ein kurzes Antippen

 *    des Sticks. Als Standgas wird 70% des unter Punkt 1 eingestellten Schwebegaswertes angenommen. Nun kann man

 *    manuell gesteuert durch Ausnutzung nur der oberen Hälfte des Stick-Bereichs fliegen. Dies entspricht der herkömmlichen

 *    Steuerung. Da nur der halbe Stick-Weg zur Verfügung steht, ist der Stick-Weg um die Mittelstellung herum höher aufgelöst.

 *    So kann das Gas trotzdem einigermaßen fein gesteuert werden.

 *  

 * 4. Nach der Landung drückt man den Stick ganz nach unten, bis ein Signal ertönt. Jetzt laufen die Motoren

 *    wieder im Leerlaufgas. Man kann nun neu Starten (ab Punkt 3) oder die Motoren ausschalten.

 */

int pitch_md_value( void ) {

        int register rawStickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_initialStickValue();

        int register stickValue;

        // Sind die Motoren eingeschaltet?

        if( MotorenEin ) {

                /* 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. */

                if( rawStickValue > 0 ) {

                        stickValue = rawStickValue + ( ( (long) rawStickValue * (long) rawStickValue ) / 150L );

                } else {

                        stickValue = rawStickValue - ( ( (long) rawStickValue * (long) rawStickValue ) / 150L );

                }

                /* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset

                 * zuzüglich dem Stick-Wert. Die folgenden Bedingungen erzeugen einen toten Bereich

                 * um die Mittelstellung herum, in dem sich targetPitchCount nicht ändert. So wird

                 * ein sanfterer Übergang vom höhengeregelten in den manuell gesteuerten Flug erreicht.

                 */

                if( stickValue > PARAM_PITCH_STICK_THRESHOLD )

                        targetPitchCount = stickValue - PARAM_PITCH_STICK_THRESHOLD + pitchOffset;

                else if( stickValue < -PARAM_PITCH_STICK_THRESHOLD )

                        targetPitchCount = stickValue + PARAM_PITCH_STICK_THRESHOLD + pitchOffset;

                else

                        targetPitchCount = 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. Es gilt:

                         *

                         * Roll-Stick links:  Einstellen des Gas-Stick-Deltas

                         * Roll-Stick rechts: Einstellen des Schwebegases

                         */

                        case STATE_INITIALIZE:

                                if( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] > 70 ) {

                                        state = STATE_SETUP_HOVER;

                                        // Signalisieren

                                        beeptime = 500;

                                } else if( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] < -70 ) {

                                        state = STATE_SETUP_STICK_DELTA;

                                        // Signalisieren

                                        beeptime = 500;

                                } else {

                                        state = STATE_RESTART;

                                }

                                break;

                        /* Erlaubt die Ermittlung des Parameters pitchHover. 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_HOVER:

                                // Im Setup-Modus soll das Gas spontan reagieren

                                actualPitchCount = targetPitchCount;

                                if( rawStickValue < 10 || abs( stickValue - lastStickValue ) > 1 ) {

                                        timer = PARAM_TIMER_1S;

                                }

                                /* 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 && ( pitchHover != actualPitchCount ) ) {

                                                // Aktuellen Pitch-Wert in Konfiguration übernehmen

                                                pitchHover = actualPitchCount;

                                                // Konfiguration dauerhaft speichern

                                                eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_MD_HOVER ], pitchHover );

                                                // Signalisieren

                                                beeptime = 500;

                                        }

                                }

                                break;

                        /* Der Stick-Deltawert kann hier konfiguriert werden. Ein Betätigen des Roll-Sticks nach links

                         * erhöht den Deltawert um 1, ein Betätigen nach rechts veringert den Wert um 1. Der eingestellte

                         * Wert kann mit dem Gas-Stick getestet werden, indem dieser in den oberen Bereich bewegt und dann

                         * losgelassen wird. Ertönt dabei ein Signal, wurde die Bewegung erkannt. Es ist dann der richtige

                         * Wert gefunden, wenn das Signal beim Loslassen des Gas-Sticks ertönt, beim gesteuerten Bewegen nach

                         * unten jedoch nicht. Durch Ausschalten der Motoren (diese laufen während der Konfiguration nicht

                         * wirklich) wird der Konfigurationsmodus beendet.

                         */

                        case STATE_SETUP_STICK_DELTA:

                                if( stickValue < PARAM_PITCH_STICK_THRESHOLD &&

                                    abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) < PARAM_PITCH_STICK_THRESHOLD ) {

                                        state = STATE_SETUP_STICK_DELTA1;

                                }

                                // Im Setup immer mit Leerlaufgas

                                targetPitchCount = 0;

                                break;

                        case STATE_SETUP_STICK_DELTA1:

                                // Roll-Stick nach links erhöht den Deltawert

                                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_STICK_DELTA;

                                        }

                                }

                                // Roll-Stick nach rechts verringert den Deltawert

                                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_STICK_DELTA;

                                        }

                                }

                                if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&

                                    ( lastStickValue - stickValue >= pitchNeutralDiff ) ) {

                                        state    = STATE_SETUP_STICK_DELTA;

                                        // Signalisieren

                                        beeptime = 500;

                                }

                                // Im Setup immer mit Leerlaufgas

                                targetPitchCount = 0;

                                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:

                                // Stick ist oberhalb der Neutralstellung

                                if( rawStickValue > PARAM_PITCH_STICK_THRESHOLD ) {

                                        pitchOffset = pitchStand;

                                        // Vollausschlag bedeutet: Einleitung der Autostartsequenz

                                        if( rawStickValue > 80 ) {

                                                state = STATE_AUTOSTART0;

                                                // Signalisieren

                                                beeptime = 500;

                                        }

                                        /* In diesem Zustand darf Standgas nicht überschritten werden, damit

                                         * der MK nicht aus diesem Status heraus abhebt.

                                         */

                                        if( targetPitchCount > pitchStand ) {

                                                targetPitchCount = pitchStand;

                                        }

                                // Erst im Status weitergehen, wenn Stick nicht mehr oberhalb der Neutralstellung

                                } else if( pitchOffset == pitchStand ) {

                                        state = STATE_BEGIN1;

                                }

                                break;

                        // MK soll erst abheben, weil sonst die Höhenregelung am Boden schon greift

                        case STATE_BEGIN1:

                                // Jetzt kann abgehoben werden

                                if( rawStickValue > PARAM_PITCH_STICK_THRESHOLD ) {

                                        // Begrenzung der Pitch-Beschleunigung nach dem Start

                                        delay = PARAM_PITCH_MD_DELAY1;

                                        state = STATE_READY0;

                                // Knüppel unterhalb der Neutralstellung bewirkt einen sofortigen Neustart

                                } else if( rawStickValue < -PARAM_PITCH_STICK_THRESHOLD ) {

                                        state = STATE_RESTART;

                                }

                                break;

                        /* Automatischer Start: Warten, bis Standgas erreicht ist, dann auf Schwebegas gehen und

                         * die Sollhöhe setzen.

                         */

                        case STATE_AUTOSTART0:

                                // Standgas darf in diesem Zustand nicht überschritten werden

                                if( targetPitchCount > pitchStand ) {

                                        targetPitchCount = pitchStand;

                                }

                                if( rawStickValue < PARAM_PITCH_STICK_THRESHOLD ) {

                                        // Knüppel unterhalb der Neutralstellung bewirkt einen sofortigen Neustart

                                        if( rawStickValue < -PARAM_PITCH_STICK_THRESHOLD ) {

                                                state = STATE_RESTART;

                                        }

                                        // Standgas muß erreicht sein, bevor die Startsequenz weiterläuft

                                        if( targetPitchCount >= pitchStand ) {

                                                // Jetzt auf Schwebegas übergehen (etwas mehr als eingestellt)

                                                pitchOffset = pitchHover;

                                                delay = PARAM_PITCH_MD_DELAY_AUTOSTART;

                                                // Einstellen der zu erreichenden Flughöhe über Grund

                                                altcon_lock();

                                                altcon_inc( PARAM_PITCH_AUTOSTART_ALT );

                                                altcon_start();

                                                state = STATE_AUTOSTART1;

                                        }

                                }

                                // Begrenzung der Pitch-Beschleunigung beim Autostart

                                delay = PARAM_PITCH_MD_DELAY_AUTOSTART;

                                break;

                        /* Wenn das Schwebegas erreicht wurde, kann davon ausgegangen werden, daß der MK

                         * nicht mehr am Boden steht.

                         */

                        case STATE_AUTOSTART1:

                                if( targetPitchCount >= pitchHover ) {

                                        delay = PARAM_PITCH_MD_DELAY2;

                                        state = STATE_ACTIVATING;

                                        // Signalisieren

                                        beeptime = 500;

                                }

                                // Eine Bewegung des Gasknüppels bricht die Autostartsequenz ab

                                if( abs( rawStickValue ) > PARAM_PITCH_STICK_THRESHOLD ) {

                                        // Höhenregler deaktivieren

                                        altcon_stop();

                                        pitchOffset     -= altcon_avgerror() / 4;

                                        targetPitchCount = pitchOffset;

                                        state            = STATE_READY;

                                }

                                break;

                        /* Die Motoren laufen jetzt mindestens mit Standgas. Der MK ist bereit zum Abheben.

                         */

                        case STATE_READY0:

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

                                        }

                                }

                                /* Übergang in den höhengeregelten Flug durch:

                                 * 1. Schnelles Bewegen des Sticks in Richtung Mittelstellung

                                 *    oder

                                 * 2. langsames Bewegen des Sticks in Mittelstellung */

                                if( ( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&

                                      ( lastStickValue - stickValue >= pitchNeutralDiff ) ) ||

                                    ( ( lastStickValue < PARAM_PITCH_STICK_THRESHOLD ) &&

                                        PARAM_PITCH_SOFT_ACTIVATING ) ) {

                                        // Aktuelle Höhe festhalten (aktiviert noch nicht den Regler)

                                        altcon_lock();

                                        // Höhe vorübergehend merken und beibehalten

                                        peakPitchCount = lastStickValue;

                                        state          = STATE_READY1;

                                        timer          = PITCH_NEUTRAL_TIMER;

                                        delay          = PARAM_PITCH_MD_DELAY2;

                                }

                                break;

                        case STATE_READY:

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

                                        }

                                }

                                // Stick ist innerhalb der Neutralstellung

                                if( abs( rawStickValue ) < 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;

                        /* Hier wird die Zeit gemessen, in der der Stick nach dem Loslassen

                         * bis zur Neutralstellung benötigt. Bei Überschreiten der Zeit

                         * wird die Höhenregelung nicht aktiviert.

                         */

                        case STATE_READY1:

                                // Gas wird konstant gehalten

                                targetPitchCount = peakPitchCount + pitchOffset;

                                timer--;

                                /* Läuft der Timer ab, bevor der Stick die Neutralstellung erreicht,

                                 * wird die Aktion nicht als "schnelles Bewegen in Neutralstellung"

                                 * gedeutet. */

                                if( !timer ) {

                                        state = STATE_READY0;

                                }

                                // Ist die Neutralstellung erreicht?

                                if( rawStickValue < PARAM_PITCH_STICK_THRESHOLD ) {

                                        pitchOffset = targetPitchCount;

                                        // Aktivierung des Höhenreglers mit der zuvor gemerkten Sollhöhe

                                        altcon_start();

                                        // Signalisieren

                                        beeptime = 500;

                                        state = STATE_ACTIVE;

                                }

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

                                    abs( stickValue - lastStickValue ) < 2 ) {

                                        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:

                                /* Nach Aktivierung der Höhenregelung soll sich der Gaswert nicht mehr

                                 * ändern. */

                                actualPitchCount = targetPitchCount;

                                pitchOffset      = targetPitchCount;

                                // Aktivierung des Höhenreglers mit der zuvor gemerkten Sollhöhe

                                altcon_start();

                                // Einschalten der Höhenregelung signalisieren

                                beeptime = 500;

                                state = STATE_ACTIVE;

                                break;

                        /* Die automatische Höhenregelung ist aktiv. */                

                        case STATE_ACTIVE:

                                targetPitchCount = pitchOffset;

                                // Stick ist außerhalb der Neutralstellung

                                if( abs( rawStickValue ) > PARAM_PITCH_STICK_THRESHOLD ) {

                                        // Höhenregler deaktivieren

                                        altcon_stop();

                                        // Ausschalten der Höhenregelung signalisieren

                                        beeptime = 500;

                                        pitchOffset     -= altcon_avgerror() / 4;

                                        targetPitchCount = pitchOffset;

                                        state            = STATE_READY;

                                }

                                break;

                        /* Neustart nach dem Flug oder vor Start. */

                        case STATE_RESTART:

                                // Begrenzung der Pitch-Beschleunigung am Boden

                                delay = PARAM_PITCH_MD_DELAY0;

                                pitchOffset      = 0;

                                targetPitchCount = 0;

                                // Folgezustand

                                state = STATE_BEGIN;

                                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;

        }

        /* Durch die Sollwertvorgabe kann hier eine einstellbare Trägheit auf dem Pitch-Wert

         * abgebildet werden. */

        int pitchDelta = targetPitchCount - actualPitchCount;

        if( pitchDelta > delay )

                pitchDelta = delay;

        if( pitchDelta < -delay )

                pitchDelta = -delay;

        if( !delayCounter ) {

                actualPitchCount += pitchDelta;

                delayCounter      = 5;

        }

        delayCounter--;

        if( actualPitchCount < 0 )

                actualPitchCount = 0;

        // Vorigen Stick-Wert merken

        lastStickValue = stickValue;

        DebugOut.Analog[25] = pitchOffset;

        DebugOut.Analog[26] = stickValue;

        return actualPitchCount;

}