Subversion Repositories FlightCtrl

Rev

Rev 1112 | Rev 1132 | Go to most recent revision | Show entire file | Ignore 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;
-
 
Line 42... Line 12...
42
long    altIntegral             = 0;
12
#include "pitch.h"
-
 
13
 
Line 43... Line 14...
43
int     temp;                                                   // Temporäre Werte; wird mehrfach verwendet
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
 
Line -... Line 22...
-
 
22
/*
-
 
23
 * Führt die Initialisierung der Pitch-Steuerung durch. Diese Funktion
-
 
24
 * wird nach jeder Setting-Auswahl sowie nach jeder Setting-Änderung
74
 * Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt.
25
 * aufgerufen.
-
 
26
 */
75
 * Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb
27
void pitch_init( void ) {
-
 
28
 
76
 * eines Wertes zu gelangen, der die Lageregelung außer Funktion setzt, wird ein zweiter Wert für
29
        // FIXME Funktioniert noch nicht
77
 * Minimalgas konfiguriert, der greift, sobald erstmalig die automatische Höhenregelung aktiviert wurde.
30
        switch( PARAM_PITCH_MODE ) {
Line 78... Line -...
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;
31
                case PARAM_PITCH_MODE_NEUTRAL:
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;
32
                        pitch_value_ptr = pitch_neutral_value;
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 ) {
-
 
191
                               
-
 
192
                                        // Timer neu setzen
-
 
193
                                        pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
-
 
194
                                        state             = STATE_WAIT;
-
 
195
                                }
-
 
196
                                break;
-
 
197
 
-
 
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
-
 
204
                                   Stick-Differenzial ist < 2 */
-
 
205
                                if( abs( stickValue ) < PITCH_STICK_THRESHOLD &&
-
 
206
                                    lastStickValue == stickValue ) {
-
 
207
                               
-
 
208
                                        pitchNeutralTimer--;
-
 
209
                                       
-
 
210
                                        if( !pitchNeutralTimer ) {
-
 
211
                                                state = STATE_ACTIVATING;
-
 
212
                                        }
-
 
213
 
-
 
214
                                // Aktivierungskriterium nicht erfüllt, zurück in INACTIVE
-
 
215
                                } else {
-
 
216
                                        state = STATE_INACTIVE;
-
 
217
                                }
-
 
218
                                break;
-
 
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;
-
 
231
                                averageN    = 0;
-
 
232
                                altIntegral = 0L;
-
 
233
                                state       = STATE_ACTIVE;
-
 
234
                               
-
 
235
                                // Einschalten der Höhenregelung signalisieren
-
 
236
                                beeptime     = 500;
-
 
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;
-
 
246
                                        pitchCount   = stickValue + pitchOffset;
33
                        break;
247
                                        lastN        = 0;
-
 
248
                                        state        = STATE_INACTIVE;
-
 
249
                                       
-
 
Line 250... Line 34...
250
                                        // Abschaltung der Höhenregelung signalisieren
34
 
251
                                        beeptime     = 500;
35
                case PARAM_PITCH_MODE_MD:
252
                                }
-
 
253
                                break;
-
 
254
                }
-
 
255
 
-
 
256
        // Motoren sind aus
-
 
257
        } else {
-
 
Line 258... Line -...
258
       
-
 
259
                switch( state ) {
-
 
260
                       
-
 
261
                        case STATE_STARTUP_WAIT:
-
 
262
 
-
 
263
                                if( !initTimer-- ) {
-
 
264
                                        state = STATE_STARTUP_INIT;
-
 
265
                                }
-
 
266
                                break;
-
 
267
 
-
 
268
                        case STATE_STARTUP_INIT:
-
 
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;
-
 
276
 
-
 
277
                                // Die Einschaltphase ist jetzt beendet
-
 
278
                                state    = STATE_BEGIN;
-
 
279
 
-
 
280
                                break;
-
 
281
 
-
 
282
                        default:
-
 
Line 283... Line 36...
283
 
36
                        pitch_value_ptr = pitch_md_value;
284
                                /* Nach dem Einschalten der Motoren darf pitchOffset keinen hohen Wert haben,
-
 
285
                                 * da der Kopter sonst sofort hochschießen würde.
-
 
286
                                 */
37
                        break;
287
                                pitchCount  = 0;
38
 
288
                                pitchOffset = 0;
39
                default:
289
                                stickValue  = 0;
-
 
Line 290... Line -...
290
                                state       = STATE_BEGIN;
-
 
291
                }
-
 
292
        }
40
                        pitch_value_ptr = pitch_mk_value;
293
 
41
        }
Line -... Line 42...
-
 
42
       
294
        if( pitchOffset < 0 )
43
        // Hier können weitere Initialisierungen folgen
-
 
44
}
295
                pitchOffset = 0;
45
 
Line 296... Line 46...
296
               
46
 
297
        // Pitch-Wert darf nicht < 0 sein
47
int pitch_value( void ) {
298
        if( pitchCount < 0 ) {
48
        switch( PARAM_PITCH_MODE ) {
299
                pitchCount = 0;
49
                case PARAM_PITCH_MODE_NEUTRAL:
300
        }
-
 
301
 
-
 
302
        // pitchCount als Debug-Wert rausschreiben
-
 
303
        DebugOut.Analog[26] = stickValue;
-
 
304
        DebugOut.Analog[28] = pitchCount;
50
                        return pitch_neutral_value();
305
        DebugOut.Analog[29] = pitchOffset;
-
 
306
 
-
 
307
        return pitchCount;
51
 
Line 308... Line 52...
308
}
52
                case PARAM_PITCH_MODE_MD:
309
 
53
                        return pitch_md_value();
Line 310... Line -...
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
54
 
358
                if( n > altMax )
55
                default: