Subversion Repositories FlightCtrl

Rev

Rev 1134 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1134 Rev 1139
Line 7... Line 7...
7 
#include "altcon.h"
 7 
#include "altcon.h"
8 
#include "parameter.h"
 8 
#include "parameter.h"
9 
#include "fc.h"
 9 
#include "fc.h"
Line 10... Line 10...
10 
 
 10 
 
11 
 
 - 
 
12 
char    enabled         = 0;
 11 
 
13 
int     pressureOffset  = 0;
 12 
static char     enabled         = 0;
14 
int     accZOffset      = 0;
 13 
static int      accZOffset      = 0;
15 
int     lastError       = 0;
 - 
 
16 
int     lastN           = 0;                            // Zuletzt errechneter Fehlerwert
 14 
static int      lastError       = 0;
17 
int     averageN        = 0;
 15 
static int      lastN           = 0;                            // Zuletzt errechneter Fehlerwert
Line -... Line 16...
- 
 
 16 
static long     altIntegral     = 0;
- 
 
 17 
static int      temp;                                           // Temporäre Werte; wird mehrfach verwendet
- 
 
 18 
 
18 
long    altIntegral     = 0;
 19 
int             pressureOffset  = 0;
19 
int     temp;                                           // Temporäre Werte; wird mehrfach verwendet
 20 
int             averageN        = 0;
Line 20... Line 21...
20 
 
 21 
 
21 
extern unsigned char    Notlandung;                     // aus fc.c
 22 
extern unsigned char    Notlandung;                             // aus fc.c
Line 41... Line 42...
41 
/*
 42 
/*
42 
 * Startet den Höhenregler
 43 
 * Startet den Höhenregler
43 
 */
 44 
 */
44 
void altcon_start( void ) {
 45 
void altcon_start( void ) {
Line 45... Line 46...
45 
 
 46 
 
46 
        accZOffset  = Mess_Integral_Hoch / 128;
 - 
 
47 
                               
 47 
        enabled     = 1;
48 
        lastError   = 0;
 48 
        lastError   = 0;
49 
        lastN       = 0;
 49 
        lastN       = 0;
50 
        averageN    = 0;
 50 
        averageN    = 0;
- 
 
 51 
        altIntegral = 0L;
Line 51... Line 52...
51 
        altIntegral = 0L;
 52 
        accZOffset  = Mess_Integral_Hoch / 128;
52 
       
 53 
       
53 
        // Einschalten der Höhenregelung signalisieren
 54 
        // Einschalten der Höhenregelung signalisieren
Line 54... Line 55...
54 
        beeptime     = 500;
 55 
        beeptime    = 500;
55 
}
 56 
}
56 
 
 57 
 
57 
 
 58 
 
58 
/*
 59 
/*
Line 59... Line 60...
59 
 * Stoppt den Höhenregler
 60 
 * Stoppt den Höhenregler
60 
 */
 61 
 */
61 
void altcon_stop( void ) {
 62 
void altcon_stop( void ) {
Line 62... Line 63...
62 
        enabled = 0;
 63 
        enabled = 0;
63 
       
 64 
       
Line 79... Line 80...
79 
                // Fehlerwert für Regler ermitteln
 80 
                // Fehlerwert für Regler ermitteln
80 
                error = airPressure - pressureOffset;
 81 
                error = airPressure - pressureOffset;
Line 81... Line 82...
81 
       
 82 
       
82 
                // Proportionalanteil
 83 
                // Proportionalanteil
83 
                n = ( PARAM_ALT_P * error ) / 4;        // dividiert durch ( 16 / STICK_GAIN ) = 16 / 4 = 4
 84 
                n = ( PARAM_ALT_P * error ) / 4;        // dividiert durch ( 16 / STICK_GAIN ) = 16 / 4 = 4
84 
 
 85 
/*
85 
                // Integralanteil
 86 
                // Integralanteil
Line 86... Line 87...
86 
                altIntegral += ( PARAM_ALT_I * error ) / 4;
 87 
                altIntegral += ( PARAM_ALT_I * error ) / 4;
87 
               
 88 
               
88 
                // Integral begrenzen
 89 
                // Integral begrenzen
89 
                if( altIntegral > PARAM_ALT_INT_MAX )
 90 
                if( altIntegral > PARAM_ALT_INT_MAX )
90 
                        altIntegral = PARAM_ALT_INT_MAX;
 91 
                        altIntegral = PARAM_ALT_INT_MAX;
91 
                else if( altIntegral < -PARAM_ALT_INT_MAX )
 92 
                else if( altIntegral < -PARAM_ALT_INT_MAX )
92 
                        altIntegral = -PARAM_ALT_INT_MAX;
 93 
                        altIntegral = -PARAM_ALT_INT_MAX;
93 
               
 94 
 
94 
                n += altIntegral / 4000;
 95 
                n += altIntegral / 4000;
95 
               
 96 
*/             
Line 96... Line 97...
96 
                // Differenzialanteil
 97 
                // Differenzialanteil
97 
                n += ( PARAM_ALT_D * ( error - lastError ) ) / 2;
 98 
                n += ( PARAM_ALT_D * ( error - lastError ) ) / 2;
Line 124... Line 125...
124 
                /* Berechnung einer exponentiellen Glättung für den neuen Gaswert bei Verlassen der
 125 
                /* Berechnung einer exponentiellen Glättung für den neuen Gaswert bei Verlassen der
125 
                 * Höhenregelung. Dies soll ein zu heftiges Reagieren mindern. */
 126 
                 * Höhenregelung. Dies soll ein zu heftiges Reagieren mindern. */
126 
                averageN = averageN + PARAM_ALT_EXP_SMOOTHING_FACTOR * ( n - averageN ) / 100;
 127 
                averageN = averageN + PARAM_ALT_EXP_SMOOTHING_FACTOR * ( n - averageN ) / 100;
127 
        }
 128 
        }
Line 128... Line 129...
128 
       
 129 
       
129 
        DebugOut.Analog[30] = altIntegral / 4000;
 130 
//      DebugOut.Analog[30] = altIntegral / 4000;
Line 130... Line 131...
130 
        DebugOut.Analog[27] = n;
 131 
        DebugOut.Analog[27] = n;
131 
 
 132