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      temp;                                           // Temporäre Werte; wird mehrfach verwendet

int             altconN    = 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 ) {
        SollHoehe  = analog_airPressure();
        accZOffset = Mess_Integral_Hoch / 128;
}


/*
 * Inkrementiert die aktuelle Höhe um eins
 */
void altcon_inc( unsigned char count ) {
        SollHoehe += count;
        accZOffset = Mess_Integral_Hoch / 128;
}


/*
 * Startet den Höhenregler
 */
void altcon_start( void ) {
        enabled = 1;
}


/*
 * Stoppt den Höhenregler
 */
void altcon_stop( void ) {
        enabled = 0;
}


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

        int register    error;

        DebugOut.Analog[24] = SollHoehe;

        altconN = 0;

        if( enabled && !Notlandung ) {

                // Fehlerwert für Regler ermitteln
                error = analog_airPressure() - SollHoehe;
       
                // Proportionalanteil
                altconN = ( PARAM_ALT_P * error ) / 4;  // dividiert durch ( 16 / STICK_GAIN ) = 16 / 4 = 4

                // Differenzialanteil wird in analog.c berechnet
                altconN += analog_airPressureDiff() / 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 );

                altconN += temp;

                // Verstärkung des Fehlerwertes zur Anpassung des Gewichtes
                altconN = altconN * PARAM_ALT_GAIN / 50L;
               
                // Limitierung des Korrekturwertes nach oben
                int altMax = PARAM_ALT_MAX * STICK_GAIN;
                if( altconN < -altMax )
                        altconN = -altMax;
        }
       
        DebugOut.Analog[27] = altconN;
 
        return altconN;
}