Subversion Repositories FlightCtrl

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

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


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

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 ) {

        accZOffset  = Mess_Integral_Hoch / 128;
                               
        lastError   = 0;
        lastN       = 0;
        averageN    = 0;
        altIntegral = 0L;
       
        // 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;
}