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

static int      stickValue              = 0;                    // Aktueller Stick-Wert

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;                    // Standgaswert

unsigned char   pitchNeutralDiffMD      = 0;                    // Suffix MD später entfernen

void pitch_md_init( void ) {

    pitchHover         = eeprom_read_byte( &EEPromArray[ EEPROM_ADR_PITCH_MD_HOVER ] );

    pitchNeutralDiffMD = eeprom_read_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ] );

}

/*

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

 *

 * Funktionsweise:

 *

 * 1. Einmalig muß man das Standgas kalibrieren. Standgas 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) wie

 *    der MK gerade noch stabil auf dem Boden steht. Man hält diese Stick-Position solange, bis ein

 *    Signal ertönt. Damit wurde der Gaswert als Standgasparameter übernommen und im EEProm dauerhaft gespeichert.

 *    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. Nun schaltet man auf Standgas durch kurzes Antippen des Sticks nach oben. Die Motoren beschleunigen langsam

 *    bis zum Erreichen des zuvor kalibriertes Standgaswertes.

 *

 * 4. Nun hebt man wie gewohnt ab. Hierzu hat man den oberen Stick-Bereich zur Verfügung. Da der Grundgaswert

 *    bei Mittelstellung jetzt knapp unterhalb des Schwebegaswertes liegt, kann man im Flug einfach die Höhe halten,

 *    indem man den Stick in Mittelstellung bewegt. Nach kurzer Zeit (quittiert mit einem Piep) wird die automatische

 *    Höhenregelung aktiviert und solange gehalten, bis man den Stick wieder bewegt.

 *

 * 5. 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();

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

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

                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. 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 nicht träge reagieren

                                actualPitchCount = targetPitchCount;

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

                                        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 && ( 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( pitchNeutralDiffMD < 20 ) {

                                                pitchNeutralDiffMD++;

                                                // Konfiguration dauerhaft speichern

                                                eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ], pitchNeutralDiffMD );

                                                // 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( pitchNeutralDiffMD > 0 ) {

                                                pitchNeutralDiffMD--;

                                                // Konfiguration dauerhaft speichern

                                                eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ], pitchNeutralDiffMD );

                                                // Signalisieren

                                                beeptime = 500;

                                                state    = STATE_SETUP_STICK_DELTA;

                                        }

                                }

                                if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&

                                    ( lastStickValue - stickValue >= pitchNeutralDiffMD ) ) {

                                        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 = pitchHover;

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

                                         * der MK nicht aus diesem Status heraus abhebt.

                                         */

                                        if( targetPitchCount > pitchHover ) {

                                                targetPitchCount = pitchHover;

                                        }

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

                                } else if( pitchOffset == pitchHover ) {

                                        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;

                        /* 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 >= pitchNeutralDiffMD ) ) ||

                                    ( ( 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();

                                        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();

                                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();

                                        pitchOffset     -= altcon_avgerror() / 4;

                                        targetPitchCount = stickValue + 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;

        DebugOut.Analog[25] = pitchOffset;

        DebugOut.Analog[26] = stickValue;

        return actualPitchCount;

}