Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1122 thjac 1 
/* altcon.c
2 
 *
3 
 * Copyright 2009 Thomas Jachmann
4 
 */
5 
 
6 
#include "main.h"
7 
#include "altcon.h"
8 
#include "parameter.h"
9 
#include "fc.h"
10 
 
11 
 
12 
char    enabled         = 0;
13 
int     pressureOffset  = 0;
14 
int     accZOffset      = 0;
15 
int     lastError       = 0;
16 
int     lastN           = 0;                            // Zuletzt errechneter Fehlerwert
17 
int     averageN        = 0;
18 
long    altIntegral     = 0;
19 
int     temp;                                           // Temporäre Werte; wird mehrfach verwendet
20 
 
21 
extern unsigned char    Notlandung;                     // aus fc.c
22 
extern int              airPressure;
23 
 
24 
 
25 
/*
26 
 * Höhenregler initialisieren
27 
 */
28 
void altcon_init( void ) {
29 
        altcon_stop();
30 
}
31 
 
32 
 
33 
/*
34 
 * Speichert die aktuelle Höhe als Sollhöhe
35 
 */
36 
void altcon_lock( void ) {
37 
        pressureOffset = airPressure;
38 
}
39 
 
40 
 
41 
/*
42 
 * Startet den Höhenregler
43 
 */
44 
void altcon_start( void ) {
45 
 
46 
        accZOffset  = Mess_Integral_Hoch / 128;
47 
 
48 
        lastError   = 0;
49 
        lastN       = 0;
50 
        averageN    = 0;
51 
        altIntegral = 0L;
52 
 
53 
        // Einschalten der Höhenregelung signalisieren
54 
        beeptime     = 500;
55 
}
56 
 
57 
 
58 
/*
59 
 * Stoppt den Höhenregler
60 
 */
61 
void altcon_stop( void ) {
62 
        enabled = 0;
63 
 
64 
        // Ausschalten der Höhenregelung signalisieren
65 
        beeptime     = 500;
66 
}
67 
 
68 
 
69 
/*
70 
 * Berechnet den Korrekturwert für die Höhenregelung
71 
 */
72 
int altcon_error( void ) {
73 
 
74 
        int register    n = 0;
75 
        int register    error;
76 
 
77 
        if( enabled && !Notlandung ) {
78 
 
79 
                // Fehlerwert für Regler ermitteln
80 
                error = airPressure - pressureOffset;
81 
 
82 
                // Proportionalanteil
83 
                n = ( PARAM_ALT_P * error ) / 4;        // dividiert durch ( 16 / STICK_GAIN ) = 16 / 4 = 4
84 
 
85 
                // Integralanteil
86 
                altIntegral += ( PARAM_ALT_I * error ) / 4;
87 
 
88 
                // Integral begrenzen
89 
                if( altIntegral > PARAM_ALT_INT_MAX )
90 
                        altIntegral = PARAM_ALT_INT_MAX;
91 
                else if( altIntegral < -PARAM_ALT_INT_MAX )
92 
                        altIntegral = -PARAM_ALT_INT_MAX;
93 
 
94 
                n += altIntegral / 4000;
95 
 
96 
                // Differenzialanteil
97 
                n += ( PARAM_ALT_D * ( error - lastError ) ) / 2;
98 
 
99 
                // ACC-Z-Integral zur Dämpfung einbeziehen
100 
                temp = ( ( ( Mess_Integral_Hoch / 128 ) - accZOffset ) * (signed long) PARAM_ALT_ACC ) / 32;
101 
 
102 
                // Dämpfung limitieren
103 
                if( temp > ( 70 * STICK_GAIN ) )
104 
                        temp = 70 * STICK_GAIN;
105 
                else if( temp < -( 70 * STICK_GAIN ) )
106 
                        temp = -( 70 * STICK_GAIN );
107 
 
108 
                n += temp;
109 
 
110 
                // Verstärkung des Fehlerwertes zur Anpassung des Gewichtes
111 
                n = n * PARAM_ALT_GAIN / 10;
112 
 
113 
                int altMax = PARAM_ALT_MAX * STICK_GAIN;
114 
 
115 
                // Limitierung des Korrekturwertes
116 
                if( n > altMax )
117 
                        n = altMax;
118 
                else if( n < -altMax )
119 
                        n = -altMax;
120 
 
121 
                lastN     = n;
122 
                lastError = error;
123 
 
124 
                /* Berechnung einer exponentiellen Glättung für den neuen Gaswert bei Verlassen der
125 
                 * Höhenregelung. Dies soll ein zu heftiges Reagieren mindern. */
126 
                averageN = averageN + PARAM_ALT_EXP_SMOOTHING_FACTOR * ( n - averageN ) / 100;
127 
        }
128 
 
129 
        DebugOut.Analog[30] = altIntegral / 4000;
130 
        DebugOut.Analog[27] = n;
131 
 
132 
        return n;
133 
}