| 0,0 → 1,133 |
|---|
| /* 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; |
| } |