Subversion Repositories FlightCtrl

/* pitch_neutral.c

 *

 * Copyright 2009 Thomas Jachmann

 */

#include "main.h"

#include "parameter.h"

#include "fc.h"

#include "altcon.h"

#include "pitch.h"

#include "pitch_neutral.h"

#define STATE_INITIALIZE                0x00                    // Initialisierung nach Einschalten der Motoren

#define STATE_SETUP                     0x01                    // Kalibrierung des Gas-Sticks

#define STATE_SETUP1                    0x02                    //

#define STATE_BEGIN                     0x03                    // Startphase

#define STATE_BEGIN1                    0x04                    //

#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

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

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

static int      pitchOffset             = 0;                            // Aktueller Grundgaswert in Neutralstellung

static char     state;                                                  // Zustand

static int      temp;

char            pitchNeutralDiff        = 0;

/* Wird verwendet, um das Umschalten auf automatische Höhenregelung

 * nach Erreichen der Neutralstellung zu verzögern.

 */

static int      pitchNeutralTimer       = PITCH_NEUTRAL_TIMER;

/*

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

 *

 * Funktionsweise:

 *

 * 1. Vor Verwendung der Pitch-Steuerung muß ein Parameter für den Gas-Stick eingestellt werden. Hierzu

 *    bewegt man den Roll-Stick ganz nach außen und startet die Motoren. Man befindet sich im Setup-Modus

 *    und die Motoren laufen unabhängig vom Gas-Stick im Leerlauf. Im Display auf Seite 12 muß nun der

 *    beste Wert für "StickDiff" gefunden werden. Den Wert ändern kann man mit dem Roll-Stick. Man betätigt

 *    nun den Gas-Stick nach oben und läßt ihn los, so daß er in die Mittelstellung zurückspringt. Der

 *    optimale Wert sorgt dafür, daß das gewollte Loslassen von einer normalen Bewegung unterschieden wird.

 *    Jedesmal, wenn die SW das Loslassen als solches erkannt hat, ertönt ein Signal. Dieses darf jedoch

 *    nicht ertönen, wenn man den Stick mit normaler Geschwindigkeit in die Mittelstellung zurückbewegt.

 *    Wurde ein guter Wert eingestellt, verläßt man den Setup-Modus durch Ausschalten der Motoren. Der

 *    ermittelte Wert wird permanent im EEProm gespeichert und muß nicht neu ermittelt werden.

 *

 * 2. Nach dem Einschalten der FC wird die aktuelle Gas-Stick-Position gemerkt und als Kalibrierungswert

 *    für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite

 *    keine Rolle.

 *

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

 *

 * 4. 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. Wichtig hierfür ist die unter Punkt 1 vorgenommene

 *    Parameterfindung. Stimmt dieser Wert nicht, führt das zur Wegnahme des Gases und somit zum Sinken.

 *

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

 *

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

 *

 * 7. 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_neutral_value( void ) {

        int register pitchCount = 0;

        // Sind die Motoren eingeschaltet? Ach ja, im Zustand STATE_SETUP sind die Motoren aus...

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

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

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

                        case STATE_SETUP:

                                if( stickValue < PARAM_PITCH_STICK_THRESHOLD &&

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

                                        state = STATE_SETUP1;

                                }

                                // Im Setup immer mit Leerlaufgas

                                pitchCount = 0;

                                break;

                        case STATE_SETUP1:

                                // Roll-Stick nach links

                                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;

                                        }

                                }

                                // Roll-Stick nach rechts

                                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;

                                        }

                                }

                                if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&

                                    ( lastStickValue - stickValue >= pitchNeutralDiff ) ) {

                                        state    = STATE_SETUP;

                                        // Signalisieren

                                        beeptime = 500;

                                }

                                // Im Setup immer mit Leerlaufgas

                                pitchCount = 0;

                                break;

                        case STATE_BEGIN:

                                // Schnelles Bewegen aus dem oberen Bereich des Sticks in Neutralstellung

                                if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&

                                    ( lastStickValue - stickValue >= pitchNeutralDiff ) ) {

                                        pitchOffset       = lastStickValue;

                                        state             = STATE_BEGIN1;

                                        pitchNeutralTimer = PITCH_NEUTRAL_TIMER;

                                }

                                break;

                        case STATE_BEGIN1:

                                // 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 ) <= PARAM_PITCH_STICK_THRESHOLD ) {

                                        state = STATE_ACTIVATING;

                                }

                                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 ) < PARAM_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 ) < PARAM_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:

                                // Höhenregler starten

                                altcon_start();

                                state = STATE_ACTIVE;

                                break;

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

                        case STATE_ACTIVE:

                                // Stick ist außerhalb der Neutralstellung

                                if( abs( stickValue ) > PARAM_PITCH_STICK_THRESHOLD ) {

                                        pitchOffset -= altcon_avgerror() / 4;

                                        pitchCount   = stickValue + pitchOffset;

                                        // Höhenregler abschalten

                                        altcon_stop();

                                        state        = STATE_INACTIVE;

                                }

                                break;

                }

        // Motoren sind aus

        } else {

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

        }

        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;

}

void pitch_neutral_init( void ) {

        printf("\r\npitch_mk_init()");

        pitchNeutralDiff = eeprom_read_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ] );

        pitchValueFP = pitch_neutral_value;

}