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