Rev 1112 | Rev 1132 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1112 | Rev 1122 | ||
---|---|---|---|
Line 1... | Line 1... | ||
1 | /* pitch.c |
1 | /* pitch.c |
2 | * |
2 | * |
- | 3 | * Copyright 2009 Thomas Jachmann |
|
- | 4 | * |
|
3 | * Pitch-Steuerung |
5 | * Pitch-Steuerung |
4 | */ |
6 | */ |
Line 5... | Line 7... | ||
5 | 7 | ||
6 | #include "main.h" |
8 | #include "main.h" |
- | 9 | #include "parameter.h" |
|
7 | #include "parameter.h" |
10 | #include "pitch_neutral.h" |
8 | #include "fc.h" |
11 | #include "pitch_md.h" |
Line 9... | Line -... | ||
9 | #include "pitch.h" |
- | |
10 | - | ||
11 | #define STATE_STARTUP_WAIT 0x00 // Init-Timeout beim Einschalten abwarten |
- | |
12 | #define STATE_STARTUP_INIT 0x01 // Initialisierung beim Einschalten |
- | |
13 | #define STATE_BEGIN 0x02 // Anfangszustand nach Einschalten der Motoren |
- | |
14 | #define STATE_INITIALIZING 0x03 // Initialisierungsphase |
- | |
15 | #define STATE_MANUAL 0x04 // Manuelle Kontrolle, Höhenregelung in Konfiguration deaktiviert |
- | |
16 | #define STATE_INACTIVE 0x05 // Manuelle Kontrolle |
- | |
17 | #define STATE_WAIT 0x06 // Warten auf Einschalten der Höhenregelung |
- | |
18 | #define STATE_ACTIVATING 0x07 // Aktivierung der Höhenregelung |
- | |
19 | #define STATE_ACTIVE 0x08 // Höhenregelung ist aktiv |
- | |
20 | - | ||
21 | #define PARAM_INIT_TIMEOUT 25 |
- | |
22 | - | ||
23 | - | ||
24 | char initTimer = PARAM_INIT_TIMEOUT; |
- | |
25 | int stickValue = 0; // Aktueller Stick-Wert |
- | |
26 | int lastStickValue = 0; // Vorheriger Stick-Wert |
- | |
27 | int zeroStickOffset = 0; // Offset für Stick-Kalibrierung |
- | |
28 | int pitchOffset = 0; // Aktueller Grundgaswert in Neutralstellung |
- | |
29 | char state = STATE_STARTUP_WAIT; // Zustand |
- | |
30 | - | ||
31 | /* Wird verwendet, um das Umschalten auf automatische Höhenregelung |
- | |
32 | * nach Erreichen der Neutralstellung zu verzögern. |
- | |
33 | */ |
- | |
34 | int pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
- | |
35 | - | ||
36 | // Variable zur Höhenregelung |
- | |
37 | int pressureOffset = 0; |
- | |
38 | int accZOffset = 0; |
- | |
39 | int lastError = 0; |
- | |
40 | int lastN = 0; // Zuletzt errechneter Fehlerwert |
- | |
41 | int averageN = 0; |
- | |
42 | long altIntegral = 0; |
- | |
43 | int temp; // Temporäre Werte; wird mehrfach verwendet |
- | |
Line -... | Line 12... | ||
- | 12 | #include "pitch.h" |
|
- | 13 | ||
Line -... | Line 14... | ||
- | 14 | ||
44 | 15 | // Zeiger auf den durch das Setting bestimmten Pitch-Steuerungsalgorithmus |
|
Line 45... | Line 16... | ||
45 | char pitchNeutralStartup = 1; // 1=Gas-Stick beim Einschalten in Mittelstellung |
16 | int (* pitch_value_ptr)( void ); |
46 | 17 | ||
47 | - | ||
48 | extern unsigned char Notlandung; // aus fc.c |
18 | // Prototyp |
49 | - | ||
50 | 19 | int pitch_mk_value( void ); |
|
51 | /* |
- | |
52 | * Berechnet den aktuellen Pitch-Wert für die Regelung |
- | |
53 | * |
- | |
54 | * Nach dem Einschalten der FC wird der aktuelle Gas-Stick-Wert gelesen und als Kalibrierungswert |
- | |
55 | * für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite |
- | |
56 | * keine Rolle. |
- | |
57 | * |
- | |
58 | * Nach Einschalten der Motoren geht der Stick in Neutralstellung. Diese Stick-Bewegung wird ignoriert |
- | |
59 | * und die Motoren drehen mit dem eingestellten MinGas2. Ausgehend von der Neutralstellung wird nun |
- | |
60 | * durch Bewegen des Sticks im oberen Bereich das Gas geregelt. |
- | |
61 | * |
- | |
62 | * Das erstmalige Aktivieren der automatischen Höhenregelung erfolgt durch Loslassen des Sticks im |
- | |
63 | * Schwebeflug. Der zuvor aktuelle Stick-Wert wird als Wert in Neutralstellung übernommen und die |
- | |
64 | * automatische Höhenregelung sofort aktiviert. |
- | |
65 | * |
- | |
66 | * Sobald der Stick die Neutralstellung verläßt, wird die automatische Höhenregelung deaktiviert |
- | |
67 | * und der vorige Pitch-Wert als Wert der Neutralstellung übernommen. Der Pitch läßt sich nun |
- | |
68 | * über den gesamten Stick-Bereich regeln. |
- | |
69 | * |
- | |
70 | * Erreicht der Stick ein weiteres Mal die Neutralstellung, wird die automatische Höhenregelung |
- | |
71 | * wieder aktiviert, jetzt jedoch immer mit einer zeitlichen Verzögerung. Nur so ist ein |
- | |
72 | * ungestörtes manuelles Steuern möglich. |
20 | |
73 | * |
21 | |
74 | * Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt. |
- | |
75 | * Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb |
- | |
76 | * eines Wertes zu gelangen, der die Lageregelung außer Funktion setzt, wird ein zweiter Wert für |
- | |
77 | * Minimalgas konfiguriert, der greift, sobald erstmalig die automatische Höhenregelung aktiviert wurde. |
- | |
78 | */ |
- | |
79 | int pitch( void ) { |
- | |
80 | - | ||
81 | int register pitchCount = 0; |
- | |
82 | - | ||
83 | // Sind die Motoren eingeschaltet? |
- | |
84 | if( MotorenEin ) { |
- | |
85 | - | ||
86 | // Vorigen Stick-Wert merken |
- | |
87 | lastStickValue = stickValue; |
- | |
88 | - | ||
89 | /* StickValue exponentiell angleichen, da ausgehend von der Neutralstellung |
- | |
90 | * nur jeweils die halbe Auflösung nach oben und unten zur Verfügung steht. Bei einer |
- | |
91 | * Multiplikation mit 2 ließe sich das Gas im Schwebebereich nicht fein genug einstellen. */ |
- | |
92 | temp = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - zeroStickOffset; |
- | |
93 | if( temp > 0 ) { |
- | |
94 | temp = temp + ( ( temp * temp ) / 150 ); |
- | |
95 | } else { |
- | |
96 | temp = temp - ( ( temp * temp ) / 150 ); |
- | |
97 | } |
- | |
98 | - | ||
99 | // Original-Stick-Wert holen und glätten |
- | |
100 | stickValue = ( temp + 2 * lastStickValue ) / 3; |
- | |
101 | - | ||
102 | /* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset |
- | |
103 | * zuzüglich dem Stick-Wert. */ |
- | |
104 | pitchCount = stickValue + pitchOffset; |
- | |
105 | - | ||
106 | switch( state ) { |
- | |
107 | - | ||
108 | case STATE_BEGIN: |
- | |
109 | - | ||
110 | // Schnelles Bewegen aus dem oberen Bereich des Sticks in Neutralstellung |
- | |
111 | if( ( lastStickValue > PITCH_STICK_THRESHOLD ) && |
- | |
112 | ( lastStickValue - stickValue >= PITCH_NEUTRAL_DIFF ) ) { |
- | |
113 | - | ||
114 | pitchOffset = lastStickValue; |
- | |
115 | - | ||
116 | state = STATE_INITIALIZING; |
- | |
117 | pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
- | |
118 | } |
- | |
119 | break; |
- | |
120 | - | ||
121 | case STATE_INITIALIZING: |
- | |
122 | - | ||
123 | // Während der Initialisierung das Gas konstant halten |
- | |
124 | pitchCount = pitchOffset; |
- | |
125 | - | ||
126 | pitchNeutralTimer--; |
- | |
127 | - | ||
128 | /* Läuft der Timer ab, bevor der Stick die Neutralstellung erreicht, |
- | |
129 | * wird die Aktion nicht als "schnelles Bewegen in Neutralstellung" |
- | |
130 | * gedeutet. */ |
- | |
131 | if( !pitchNeutralTimer ) { |
- | |
132 | pitchOffset = 0; |
- | |
133 | state = STATE_BEGIN; |
- | |
134 | } |
- | |
135 | - | ||
136 | // Ist die Neutralstellung erreicht? |
- | |
137 | if( abs( stickValue ) <= PITCH_STICK_THRESHOLD ) { |
- | |
138 | - | ||
139 | // Ist die Höhenregelung aktiviert? |
- | |
140 | if( EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG ) { |
- | |
141 | state = STATE_ACTIVATING; |
- | |
142 | } else { |
- | |
143 | state = STATE_MANUAL; |
- | |
144 | } |
- | |
145 | } |
- | |
146 | break; |
- | |
147 | - | ||
148 | /* Wenn die Höhenregelung per Konfiguration deaktiviert ist, verbleibt |
- | |
149 | * die Funktion in diesem Zustand. */ |
- | |
150 | case STATE_MANUAL: |
- | |
151 | - | ||
152 | // Min2-Gas einstellen für Lageregelung bei Minimalgas |
- | |
153 | if( pitchCount < PARAM_PITCH_MIN2 ) { |
- | |
154 | pitchCount = PARAM_PITCH_MIN2; |
- | |
155 | } |
- | |
156 | break; |
- | |
157 | - | ||
158 | /* Die Höhenregelung ist per Konfiguration aktiviert, jedoch befindet |
- | |
159 | * sich der Stick außerhalb des als Neutralstellung anerkannten |
- | |
160 | * Wertebereiches. Es wird manuell geregelt. */ |
- | |
161 | case STATE_INACTIVE: |
- | |
162 | - | ||
163 | // Ist ein Restart zulässig? |
- | |
164 | if( PARAM_PITCH_RESTART_ENABLED ) { |
- | |
165 | - | ||
166 | /* Wenn der Gashebel ganz unten steht, Timer für Reduzierung des Minimalgaswertes |
- | |
167 | * starten. Hierfür wird die Variable pitchNeutralTimer verwendet. */ |
- | |
168 | if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 120 ) { |
- | |
169 | pitchNeutralTimer = PITCH_MIN2_TIMER; |
- | |
170 | } else { |
- | |
171 | pitchNeutralTimer--; |
- | |
172 | - | ||
173 | /* Gashebel steht seit PITCH_MIN2_TIMER ganz unten; jetzt erfolgt die Initialisierung. */ |
- | |
174 | if( !pitchNeutralTimer ) { |
- | |
175 | state = STATE_BEGIN; |
- | |
176 | pitchOffset = 0; |
- | |
177 | - | ||
178 | // Signalisieren |
- | |
179 | beeptime = 500; |
- | |
180 | } |
- | |
181 | } |
- | |
182 | } |
- | |
183 | - | ||
184 | // Min2-Gas einstellen für Lageregelung bei Minimalgas |
- | |
185 | if( pitchCount < PARAM_PITCH_MIN2 ) { |
- | |
186 | pitchCount = PARAM_PITCH_MIN2; |
- | |
187 | } |
- | |
188 | - | ||
189 | // Stick ist innerhalb der Neutralstellung |
- | |
190 | if( abs( stickValue ) < PITCH_STICK_THRESHOLD ) { |
- | |
Line 191... | Line -... | ||
191 | - | ||
192 | // Timer neu setzen |
- | |
193 | pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
- | |
194 | state = STATE_WAIT; |
- | |
195 | } |
- | |
196 | break; |
- | |
197 | 22 | /* |
|
198 | /* Der Stick ist in den für die Neutralstellung gültigen Wertebereich |
- | |
199 | * gelangt. Nun darf innerhalb einer bestimmten Zeit keine Stick-Bewegung |
- | |
200 | * erfolgen, um die automatische Höhenregelung zu aktivieren. */ |
- | |
201 | case STATE_WAIT: |
- | |
202 | - | ||
203 | /* Stick ist innerhalb der Neutralstellung und |
23 | * Führt die Initialisierung der Pitch-Steuerung durch. Diese Funktion |
204 | Stick-Differenzial ist < 2 */ |
24 | * wird nach jeder Setting-Auswahl sowie nach jeder Setting-Änderung |
205 | if( abs( stickValue ) < PITCH_STICK_THRESHOLD && |
- | |
206 | lastStickValue == stickValue ) { |
- | |
207 | - | ||
208 | pitchNeutralTimer--; |
- | |
209 | 25 | * aufgerufen. |
|
210 | if( !pitchNeutralTimer ) { |
- | |
211 | state = STATE_ACTIVATING; |
26 | */ |
Line 212... | Line -... | ||
212 | } |
- | |
213 | - | ||
214 | // Aktivierungskriterium nicht erfüllt, zurück in INACTIVE |
- | |
215 | } else { |
27 | void pitch_init( void ) { |
216 | state = STATE_INACTIVE; |
- | |
217 | } |
- | |
218 | break; |
28 | |
219 | - | ||
220 | /* Die automatische Höhenregelung wird jetzt aktiviert. Der aktuelle |
- | |
221 | * Luftdruck wird gespeichert und notwendige Werte für den Regler |
- | |
222 | * werden initialisiert. */ |
- | |
223 | case STATE_ACTIVATING: |
- | |
224 | - | ||
225 | // Die Referenzhöhe soll zu Beginn der Neutralstellung genommen werden |
- | |
226 | pressureOffset = airPressure; |
- | |
227 | accZOffset = Mess_Integral_Hoch / 128; |
- | |
228 | - | ||
229 | lastError = 0; |
- | |
230 | lastN = 0; |
29 | // FIXME Funktioniert noch nicht |
Line 231... | Line -... | ||
231 | averageN = 0; |
- | |
232 | altIntegral = 0L; |
30 | switch( PARAM_PITCH_MODE ) { |
233 | state = STATE_ACTIVE; |
- | |
234 | - | ||
235 | // Einschalten der Höhenregelung signalisieren |
- | |
236 | beeptime = 500; |
31 | case PARAM_PITCH_MODE_NEUTRAL: |
237 | - | ||
238 | break; |
- | |
239 | - | ||
240 | /* Die automatische Höhenregelung ist aktiv. */ |
- | |
241 | case STATE_ACTIVE: |
- | |
242 | - | ||
243 | // Stick ist außerhalb der Neutralstellung |
- | |
244 | if( abs( stickValue ) > PITCH_STICK_THRESHOLD ) { |
- | |
245 | pitchOffset -= averageN / 4; |
32 | pitch_value_ptr = pitch_neutral_value; |
Line 246... | Line -... | ||
246 | pitchCount = stickValue + pitchOffset; |
- | |
247 | lastN = 0; |
- | |
248 | state = STATE_INACTIVE; |
- | |
249 | - | ||
250 | // Abschaltung der Höhenregelung signalisieren |
- | |
251 | beeptime = 500; |
- | |
252 | } |
- | |
253 | break; |
33 | break; |
254 | } |
- | |
255 | 34 | ||
256 | // Motoren sind aus |
- | |
257 | } else { |
- | |
258 | - | ||
259 | switch( state ) { |
- | |
260 | - | ||
261 | case STATE_STARTUP_WAIT: |
- | |
262 | - | ||
Line 263... | Line -... | ||
263 | if( !initTimer-- ) { |
- | |
264 | state = STATE_STARTUP_INIT; |
- | |
Line -... | Line 35... | ||
- | 35 | case PARAM_PITCH_MODE_MD: |
|
- | 36 | pitch_value_ptr = pitch_md_value; |
|
265 | } |
37 | break; |
266 | break; |
38 | |
Line -... | Line 39... | ||
- | 39 | default: |
|
267 | 40 | pitch_value_ptr = pitch_mk_value; |
|
Line 268... | Line 41... | ||
268 | case STATE_STARTUP_INIT: |
41 | } |
269 | - | ||
270 | /* Lädt den beim Einschalten der FC anliegenden Stickwert als |
- | |
271 | * Offset für die Kalibrierung des Gas-Sticks. */ |
- | |
272 | zeroStickOffset = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ]; |
- | |
273 | - | ||
274 | if( zeroStickOffset < -75 ) |
- | |
275 | pitchNeutralStartup = 0; |
42 | |
276 | - | ||
277 | // Die Einschaltphase ist jetzt beendet |
43 | // Hier können weitere Initialisierungen folgen |
278 | state = STATE_BEGIN; |
44 | } |
Line 279... | Line -... | ||
279 | - | ||
280 | break; |
- | |
Line -... | Line 45... | ||
- | 45 | ||
281 | 46 | ||
- | 47 | int pitch_value( void ) { |
|
282 | default: |
48 | switch( PARAM_PITCH_MODE ) { |
- | 49 | case PARAM_PITCH_MODE_NEUTRAL: |
|
283 | 50 | return pitch_neutral_value(); |
|
284 | /* Nach dem Einschalten der Motoren darf pitchOffset keinen hohen Wert haben, |
- | |
Line 285... | Line -... | ||
285 | * da der Kopter sonst sofort hochschießen würde. |
- | |
286 | */ |
51 | |
287 | pitchCount = 0; |
52 | case PARAM_PITCH_MODE_MD: |
288 | pitchOffset = 0; |
- | |
Line 289... | Line 53... | ||
289 | stickValue = 0; |
53 | return pitch_md_value(); |
290 | state = STATE_BEGIN; |
54 | |
291 | } |
- | |
292 | } |
- | |
293 | - | ||
294 | if( pitchOffset < 0 ) |
- | |
295 | pitchOffset = 0; |
- | |
296 | - | ||
297 | // Pitch-Wert darf nicht < 0 sein |
- | |
298 | if( pitchCount < 0 ) { |
- | |
299 | pitchCount = 0; |
- | |
300 | } |
- | |
301 | - | ||
302 | // pitchCount als Debug-Wert rausschreiben |
- | |
303 | DebugOut.Analog[26] = stickValue; |
- | |
304 | DebugOut.Analog[28] = pitchCount; |
- | |
305 | DebugOut.Analog[29] = pitchOffset; |
- | |
306 | - | ||
307 | return pitchCount; |
- | |
308 | } |
- | |
309 | - | ||
310 | - | ||
311 | /* |
- | |
312 | * Berechnet den Korrekturwert für die Höhenregelung |
- | |
313 | */ |
- | |
314 | int altitudeController( void ) { |
- | |
315 | - | ||
316 | int register n = 0; |
- | |
317 | int register error; |
- | |
318 | - | ||
319 | if( ( state == STATE_ACTIVE ) && !Notlandung ) { |
- | |
320 | - | ||
321 | // Fehlerwert für Regler ermitteln |
- | |
322 | error = airPressure - pressureOffset; |
- | |
323 | - | ||
324 | // Proportionalanteil |
- | |
325 | n = ( PARAM_ALT_P * error ) / 4; // dividiert durch ( 16 / STICK_GAIN ) = 16 / 4 = 4 |
- | |
326 | - | ||
327 | // Integralanteil |
- | |
328 | altIntegral += ( PARAM_ALT_I * error ) / 4; |
- | |
329 | - | ||
330 | // Integral begrenzen |
- | |
331 | if( altIntegral > PARAM_ALT_INT_MAX ) |
- | |
332 | altIntegral = PARAM_ALT_INT_MAX; |
- | |
333 | else if( altIntegral < -PARAM_ALT_INT_MAX ) |
- | |
334 | altIntegral = -PARAM_ALT_INT_MAX; |
- | |
335 | - | ||
336 | n += altIntegral / 4000; |
- | |
337 | - | ||
338 | // Differenzialanteil |
- | |
339 | n += ( PARAM_ALT_D * ( error - lastError ) ) / 2; |
- | |
340 | - | ||
341 | // ACC-Z-Integral zur Dämpfung einbeziehen |
- | |
342 | temp = ( ( ( Mess_Integral_Hoch / 128 ) - accZOffset ) * (signed long) PARAM_ALT_ACC ) / 32; |
- | |
343 | - | ||
344 | // Dämpfung limitieren |
- | |
345 | if( temp > ( 70 * STICK_GAIN ) ) |
- | |
346 | temp = 70 * STICK_GAIN; |
- | |
347 | else if( temp < -( 70 * STICK_GAIN ) ) |
- | |
348 | temp = -( 70 * STICK_GAIN ); |
- | |
349 | - | ||
350 | n += temp; |
- | |
351 | - | ||
352 | // Verstärkung des Fehlerwertes zur Anpassung des Gewichtes |
- | |
353 | n = n * PARAM_ALT_GAIN / 10; |
- | |
354 | - | ||
355 | int altMax = PARAM_ALT_MAX * STICK_GAIN; |
- | |
356 | - | ||
357 | // Limitierung des Korrekturwertes |
- |