Subversion Repositories FlightCtrl

/* altcon.c
 *
 * Copyright 2009 Thomas Jachmann
 */

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


static char     enabled         = 0;
static int      accZOffset      = 0;
static int      lastError       = 0;
static int      lastN           = 0;                            // Zuletzt errechneter Fehlerwert
static long     altIntegral     = 0;
static int      temp;                                           // Temporäre Werte; wird mehrfach verwendet

int             pressureOffset  = 0;
int             averageN        = 0;

extern unsigned char    Notlandung;                             // aus fc.c
extern int              airPressure;


/*
 * Höhenregler initialisieren
 */
void altcon_init( void ) {
        altcon_stop();
}


/*
 * Speichert die aktuelle Höhe als Sollhöhe
 */
void altcon_lock( void ) {
        pressureOffset = airPressure;
}


/*
 * Startet den Höhenregler
 */
void altcon_start( void ) {

        enabled     = 1;
        lastError   = 0;
        lastN       = 0;
        averageN    = 0;
        altIntegral = 0L;
        accZOffset  = Mess_Integral_Hoch / 128;
       
        // Einschalten der Höhenregelung signalisieren
        beeptime    = 500;
}


/*
 * Stoppt den Höhenregler
 */
void altcon_stop( void ) {
        enabled = 0;
       
        // Ausschalten der Höhenregelung signalisieren
        beeptime = 500;
}


/*
 * Berechnet den Korrekturwert für die Höhenregelung
 */
int altcon_error( void ) {

        int register    n = 0;
        int register    error;

        if( enabled && !Notlandung ) {

                // Fehlerwert für Regler ermitteln
                error = airPressure - pressureOffset;
       
                // Proportionalanteil
                n = ( PARAM_ALT_P * error ) / 4;        // dividiert durch ( 16 / STICK_GAIN ) = 16 / 4 = 4
/*
                // Integralanteil
                altIntegral += ( PARAM_ALT_I * error ) / 4;
               
                // Integral begrenzen
                if( altIntegral > PARAM_ALT_INT_MAX )
                        altIntegral = PARAM_ALT_INT_MAX;
                else if( altIntegral < -PARAM_ALT_INT_MAX )
                        altIntegral = -PARAM_ALT_INT_MAX;

                n += altIntegral / 4000;
*/             
                // Differenzialanteil
                n += ( PARAM_ALT_D * ( error - lastError ) ) / 2;

                // ACC-Z-Integral zur Dämpfung einbeziehen
                temp = ( ( ( Mess_Integral_Hoch / 128 ) - accZOffset ) * (signed long) PARAM_ALT_ACC ) / 32;

                // Dämpfung limitieren
                if( temp > ( 70 * STICK_GAIN ) )
                        temp = 70 * STICK_GAIN;
                else if( temp < -( 70 * STICK_GAIN ) )
                        temp = -( 70 * STICK_GAIN );

                n += temp;

                // Verstärkung des Fehlerwertes zur Anpassung des Gewichtes
                n = n * PARAM_ALT_GAIN / 10;
               
                int altMax = PARAM_ALT_MAX * STICK_GAIN;
               
                // Limitierung des Korrekturwertes
                if( n > altMax )
                        n = altMax;
                else if( n < -altMax )
                        n = -altMax;
               
                lastN     = n;
                lastError = error;
               
                /* Berechnung einer exponentiellen Glättung für den neuen Gaswert bei Verlassen der
                 * Höhenregelung. Dies soll ein zu heftiges Reagieren mindern. */
                averageN = averageN + PARAM_ALT_EXP_SMOOTHING_FACTOR * ( n - averageN ) / 100;
        }
       
//      DebugOut.Analog[30] = altIntegral / 4000;
        DebugOut.Analog[27] = n;
 
        return n;
}