Subversion Repositories FlightCtrl

/* pitch.c
 *
 * Copyright 2009 Thomas Jachmann
 *
 * Pitch-Steuerung
 */

#include "main.h"
#include "parameter.h"
#include "pitch_neutral.h"
#include "pitch_md.h"
#include "pitch_inc.h"
#include "pitch.h"

#define UNDEFINED               255
#define INVALID                 255

int  pitchInitialStickValue = UNDEFINED;
unsigned char pitchMode;


char *pitchModeStrings[] = {
        "Normal   ",
        "Neutral  ",
        "MD       ",
        "Increment"
        "Undefined",
        "Undefined",
        "Undefined",
        "Invalid  ",
};


// Zeiger auf den durch das Setting bestimmten Pitch-Steuerungsalgorithmus
int (* pitchValueFP)( void );

// Prototyp
void pitch_mk_init( void );
int pitch_mk_value( void );

/*
 * Führt die Initialisierung der Pitch-Steuerung durch. Diese Funktion
 * wird nach jeder Setting-Auswahl sowie nach jeder Setting-Änderung
 * aufgerufen.
 */
void pitch_init( void ) {


        pitchMode = PARAM_PITCH_MODE;
        pitchValueFP = pitch_mk_value;   // Reasonable default

        // Nur beim Einschalten ermitteln, da beim Setting-Wechsel ungültig
        if( pitchInitialStickValue == UNDEFINED ) {
       
                // Warten, bis ein gültiger Wert von der Fernsteuerung anliegt
                unsigned int timer = SetDelay( 500 );
                while( !CheckDelay( timer ) );

                pitchInitialStickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ];
        }
       
        /* Die Stick-Position beim Einschalten muß mit dem im Setting eingestellten
         * Pitch-Modus übereinstimmen. Sonst wird die Pitch-Steuerung aus Sicherheits-
         * gründen deaktiviert. Selbiges gilt, wenn die Höhenregelung deaktiviert wurde.
         */

        switch( pitchMode ) {
                // Pitch-Modi mit neutralisiertem Gas
                case PARAM_PITCH_MODE_NEUTRAL:
                        printf("\r\nPARAM_PITCH_MODE_NEUTRAL\r\n");
                case PARAM_PITCH_MODE_MD:
                        printf("\r\nPARAM_PITCH_MODE_MD\r\n");
                        if( !( EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG ) || abs( pitchInitialStickValue ) > PARAM_PITCH_STICK_THRESHOLD )
                                pitchMode = PARAM_PITCH_MODE_INVALID;
                        else  {
                            if( pitchMode == PARAM_PITCH_MODE_NEUTRAL)
                                pitch_neutral_init();
                            else if( pitchMode == PARAM_PITCH_MODE_MD)
                                pitch_md_init();
                        }
                        break;

                // Pitch-Modi mit incrementellem Gas
                case PARAM_PITCH_MODE_INC:
                        printf("\r\nPARAM_PITCH_MODE_INC\r\n");
                        if( abs( pitchInitialStickValue ) > PARAM_PITCH_STICK_THRESHOLD )
                                pitchMode = PARAM_PITCH_MODE_INVALID;
                        else
                            pitch_inc_init();
                        break;

                // Pitch-Modi mit 0-Gas
                case PARAM_PITCH_MODE_NORMAL:
                        printf("\r\nPARAM_PITCH_MODE_NORMAL\r\n");
                        if( pitchInitialStickValue > 35 - 120 )
                                pitchMode = PARAM_PITCH_MODE_INVALID;
                        else
                            pitch_mk_init();
                        break;
        }
       
        if( pitchMode == PARAM_PITCH_MODE_INVALID)
            printf("\r\nPARAM_PITCH_MODE_INVALID, stick: %d\r\n", pitchInitialStickValue); //TODO


/*
 // Moved to pitch_xxx_init()
        switch( pitchMode ) {
                case PARAM_PITCH_MODE_NEUTRAL:
                        pitch_value_ptr = &pitch_neutral_value;
                        break;

                case PARAM_PITCH_MODE_MD:
                        pitch_value_ptr = &pitch_md_value;
                        break;

                case PARAM_PITCH_MODE_INC:
                        pitch_value_ptr = &pitch_inc_value;
                        break;

                default:
                        pitch_value_ptr = &pitch_mk_value;
        }
*/
       
        // Hier können weitere Initialisierungen folgen
}


/*
int pitch_value( void ) {   // -> int (* pitch_value)( void );

        switch( pitchMode ) {
                case PARAM_PITCH_MODE_NEUTRAL:
                        return pitch_neutral_value();

                case PARAM_PITCH_MODE_MD:
                        return pitch_md_value();

                case PARAM_PITCH_MODE_NORMAL:
                        return pitch_mk_value();

                case PARAM_PITCH_MODE_INC:
                        return pitch_inc_value();
                        break;

                default:
                        return 0;
        }


        return pitch_value_ptr();

}
*/


/*
 * Führt eine Pitch-Berechnung aus, die der Original-SW entspricht.
 */
int pitch_mk_value( void ) {
        register int stickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ];
        register int pitchCount = stickValue + 120;
       
        DebugOut.Analog[26] = stickValue;
        DebugOut.Analog[28] = pitchCount;

        return pitchCount;
}

void pitch_mk_init( void ) {

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

    pitchValueFP = pitch_mk_value;
}