Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1122 thjac 1
/* pitch_md.c
2
 *
3
 * Copyright 2009 Thomas Jachmann
4
 *
5
 * Die in dieser Quelldatei enthaltenen Algorithmen ermöglichen eine MD-ähnliche Pitch-Steuerung
6
 * für den MK.
7
 */
8
 
9
#include "main.h"
10
#include "parameter.h"
11
#include "fc.h"
12
#include "pitch.h"
13
#include "altcon.h"
14
 
15
 
16
#define STATE_INITIALIZE                0x01                    // Anfangszustand nach Einschalten der Motoren
17
#define STATE_SETUP                     0x02                    // Ermittlung von PARAM_PITCH_MD_HOVER
18
#define STATE_BEGIN                     0x03                    // Anfangszustand für Flugbetrieb
19
#define STATE_BEGIN1                    0x04                    // Anfangszustand für Flugbetrieb
20
#define STATE_READY                     0x05                    // Manuelle Kontrolle
21
#define STATE_WAIT                      0x06                    // Warten auf Einschalten der Höhenregelung
22
#define STATE_ACTIVATING                0x07                    // Aktivierung der Höhenregelung
23
#define STATE_ACTIVE                    0x08                    // Höhenregelung ist aktiv
24
 
25
 
26
int     stickValue              = 0;                            // Aktueller Stick-Wert
27
int     lastStickValue          = 0;                            // Vorheriger Stick-Wert
28
int     actualPitchCount;                                       // Soll-Pitch-Wert
29
int     targetPitchCount;                                       // Ist-Pitch-Wert
30
int     pitchOffset;                                            // Aktueller Grundgaswert in Neutralstellung
31
char    state;                                                  // Zustand
32
int     timer;
33
int     delay                   = 0;
34
int     delayCounter            = 0;
35
int     temp;                                                   // Temporäre Werte; wird mehrfach verwendet
36
 
37
 
38
/*
39
 * Berechnet den aktuellen Pitch-Wert für die Regelung
40
 *
41
 * Nachdem die Motoren eingeschaltet sind, wird der Pitch-Stick losgelassen und damit in Mittelstellung
42
 * gebracht. Die Motoren laufen zu dieser Zeit im Leerlaufgas. Vor dem Abheben müssen die Motoren in
43
 * das Standgas gebracht werden. Dies geschieht durch minimales Gasgeben. Durch weiteres Gasgeben und
44
 * nehmen kann abgehoben und geflogen werden.
45
 *
46
 * Erreicht der Stick während des Fluges die Neutralstellung und verbleibt dort für ca. 1 Sekunde ohne
47
 * bewegt zu werden, aktiviert sich die Höhenregelung und hält den MK auf der aktuellen Höhe.
48
 *
49
 * Sobald der Stick die Neutralstellung verläßt, wird die automatische Höhenregelung deaktiviert
50
 * und der vorige Pitch-Wert als Wert der Neutralstellung übernommen. Der Pitch läßt sich nun
51
 * über den gesamten Stick-Bereich regeln.
52
 *
53
 * Erreicht der Stick ein weiteres Mal die Neutralstellung, wird die automatische Höhenregelung
54
 * wieder aktiviert, jetzt jedoch immer mit einer zeitlichen Verzögerung. Nur so ist ein
55
 * ungestörtes manuelles Steuern möglich.
56
 *
57
 * Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt.
58
 * Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb
59
 * eines Wertes zu gelangen, der die Lageregelung außer Funktion setzt, wird ein zweiter Wert für
60
 * Minimalgas konfiguriert, der greift, sobald erstmalig die automatische Höhenregelung aktiviert wurde.
61
 */
62
int pitch_md_value( void ) {
63
 
64
        int register rawStickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_stickoffset();
65
 
66
        // Sind die Motoren eingeschaltet?
67
        if( MotorenEin ) {
68
 
69
                // Vorigen Stick-Wert merken
70
                lastStickValue = stickValue;
71
 
72
                /* StickValue exponentiell angleichen, da ausgehend von der Neutralstellung
73
                 * nur jeweils die halbe Auflösung nach oben und unten zur Verfügung steht. Bei einer
74
                 * Multiplikation mit 2 ließe sich das Gas im Schwebebereich nicht fein genug einstellen. */
75
                temp = rawStickValue;
76
                if( temp > 0 ) {
77
                        temp = temp + ( ( temp * temp ) / 150 );
78
                } else {
79
                        temp = temp - ( ( temp * temp ) / 150 );
80
                }
81
 
82
                stickValue = temp;
83
 
84
                /* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset
85
                 * zuzüglich dem Stick-Wert. */
86
                targetPitchCount = stickValue + pitchOffset;
87
 
88
                switch( state ) {
89
 
90
                        /* Entscheidet über Flugbetrieb oder Setup-Betrieb. Für den Setup-Betrieb
91
                         * muß beim Einschalten der Motoren gleichzeitig der Roll-Stick ganz
92
                         * betätigt werden (Richtung ist egal).
93
                         */
94
                        case STATE_INITIALIZE:
95
 
96
                                if( abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) > 70 ) {
97
                                        state = STATE_SETUP;
98
                                } else {
99
                                        state = STATE_BEGIN;
100
                                }
101
                                break;
102
 
103
                        /* Erlaubt die Ermittlung des Parameters PARAM_PITCH_MD_HOVER. Hierzu wird soviel Gas
104
                         * gegeben, bis der MK kurz vor dem Abheben ist, jedoch noch stabil steht. Um den
105
                         * Gaswert dauerhaft zu speichern, muß der Stick an der Position verweilen, bis
106
                         * der Summer die Übernahme akustisch quittiert. Dann müssen die Motoren wieder
107
                         * ausgeschaltet werden, da dieser Modus nicht für den Flug vorgesehen ist.
108
                         */            
109
                        case STATE_SETUP:
110
 
111
                                // Im Setup-Modus soll das Gas nicht träge reagieren
112
                                actualPitchCount = targetPitchCount;
113
 
114
                                if( rawStickValue < 20 || ( stickValue - lastStickValue ) ) {
115
                                        timer = PARAM_TIMER_2S;
116
                                }
117
 
118
                                /* Der Stick befindet sich eindeutig in der oberen Hälfte und wurde
119
                                 * seit dem letzten Zyklus nicht bewegt. */
120
                                else {
121
                                        timer--;
122
 
123
                                        /* Die Verweilzeit ist abgelaufen und der aktuelle Pitch-Wert
124
                                         * entspricht nicht dem bereits gespeicherten Wert. */
125
                                        if( !timer && ( PARAM_PITCH_MD_HOVER != actualPitchCount ) ) {
126
 
127
                                                // Aktuellen Pitch-Wert in Konfiguration übernehmen
128
                                                PARAM_PITCH_MD_HOVER = actualPitchCount;
129
 
130
                                                // Konfiguration dauerhaft speichern
131
                                                WriteParameterSet(
132
                                                        GetActiveParamSetNumber(),
133
                                                        (unsigned char *) &EE_Parameter.Kanalbelegung[0],
134
                                                        STRUCT_PARAM_LAENGE );
135
 
136
                                                // Signalisieren
137
                                                beeptime = 500;
138
                                        }
139
                                }
140
                                break;
141
 
142
                        /* In diesem Zustand steht der MK am Boden und die Motoren laufen auf Leerlaufgas. Ein
143
                         * kurzes Auslenken des Sticks nach oben schaltet in das Standgas über.
144
                         */
145
                        case STATE_BEGIN:
146
 
147
                                // Begrenzung der Pitch-Beschleunigung am Boden
148
                                delay = PARAM_PITCH_MD_DELAY0;
149
 
150
                                if( rawStickValue > PARAM_PITCH_STICK_THRESHOLD ) {
151
                                        pitchOffset = PARAM_PITCH_MD_HOVER;
152
                                } else if( pitchOffset == PARAM_PITCH_MD_HOVER ) {
153
                                        state = STATE_BEGIN1;
154
                                }
155
                                break;
156
 
157
                        // MK soll erst abheben, weil sonst die Höhenregelung am Boden schon greift
158
                        case STATE_BEGIN1:
159
 
160
                                if( abs( rawStickValue ) > 10 ) {
161
 
162
                                        // Begrenzung der Pitch-Beschleunigung im Flug
163
                                        delay = PARAM_PITCH_MD_DELAY1;
164
 
165
                                        if( rawStickValue > 0 ) {
166
                                                state = STATE_READY;
167
                                        }
168
                                }
169
                                break;
170
 
171
                        /* Die Motoren laufen jetzt mindestens mit Standgas. Der MK ist bereit zum Abheben.
172
                         * Das Minimalgas kann jetzt nicht mehr unterschritten werden.
173
                         */
174
                        case STATE_READY:
175
 
176
                                // Ist ein Restart zulässig?
177
                                if( PARAM_PITCH_RESTART_ENABLED ) {
178
 
179
                                        /* Wenn der Gashebel ganz unten steht, Timer für Restart der Pitch-Regelung
180
                                         * starten. Hierfür wird die Variable pitchNeutralTimer verwendet. */
181
                                        if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 120 ) {
182
                                                timer = PITCH_MIN2_TIMER;
183
                                        } else {
184
                                                timer--;
185
 
186
                                                /* Gashebel steht seit PITCH_MIN2_TIMER ganz unten; jetzt erfolgt die Initialisierung. */
187
                                                if( !timer ) {
188
                                                        state            = STATE_BEGIN;
189
                                                        pitchOffset      = 0;
190
                                                        targetPitchCount = 0;
191
                                                        actualPitchCount = 0;
192
 
193
                                                        // Signalisieren
194
                                                        beeptime    = 500;
195
                                                }
196
                                        }
197
                                }
198
 
199
                                // Min2-Gas einstellen für Lageregelung bei Minimalgas
200
                                if( targetPitchCount < PARAM_PITCH_MIN2 ) {
201
                                        targetPitchCount = PARAM_PITCH_MIN2;
202
                                }
203
 
204
                                // Stick ist innerhalb der Neutralstellung
205
                                if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD ) {
206
 
207
                                        // Aktuelle Höhe festhalten (aktiviert noch nicht den Regler)
208
                                        altcon_lock();
209
 
210
                                        // Timer neu setzen
211
                                        timer = PITCH_NEUTRAL_TIMER;
212
                                        state = STATE_WAIT;
213
                                }
214
                                break;
215
 
216
                        /* Der Stick ist in den für die Neutralstellung gültigen Wertebereich
217
                         * gelangt. Nun darf innerhalb einer bestimmten Zeit keine Stick-Bewegung
218
                         * erfolgen, um die automatische Höhenregelung zu aktivieren. */
219
                        case STATE_WAIT:
220
 
221
                                /* Stick ist innerhalb der Neutralstellung und
222
                                   Stick-Differenzial ist < 2 */
223
                                if( abs( rawStickValue ) < PARAM_PITCH_STICK_THRESHOLD &&
224
                                    lastStickValue == stickValue ) {
225
 
226
                                        timer--;
227
 
228
                                        if( !timer ) {
229
                                                state = STATE_ACTIVATING;
230
                                        }
231
 
232
                                // Aktivierungskriterium nicht erfüllt, zurück in INACTIVE
233
                                } else {
234
                                        state = STATE_READY;
235
                                }
236
                                break;
237
 
238
                        /* Die automatische Höhenregelung wird jetzt aktiviert.
239
                         */
240
                        case STATE_ACTIVATING:
241
 
242
                                // Aktivierung des Höhenreglers mit der zuvor gemerkten Sollhöhe
243
                                altcon_start();
244
 
245
                                state = STATE_ACTIVE;
246
                                break;
247
 
248
                        /* Die automatische Höhenregelung ist aktiv. */                
249
                        case STATE_ACTIVE:
250
 
251
                                // Stick ist außerhalb der Neutralstellung
252
                                if( abs( rawStickValue ) > PARAM_PITCH_STICK_THRESHOLD ) {
253
 
254
                                        // Höhenregler deaktivieren
255
                                        altcon_stop();
256
 
257
                                        pitchOffset     -= altcon_avgerror() / 4;
258
                                        targetPitchCount = stickValue + pitchOffset;
259
                                        state            = STATE_READY;
260
                                }
261
                                break;
262
                }
263
 
264
        // Motoren sind aus
265
        } else {
266
 
267
                /* Nach dem Einschalten der Motoren wird pitchOffset auf PARAM_PITCH_OVER gesetzt.
268
                 */
269
                actualPitchCount = 0;
270
                targetPitchCount = 0;
271
                pitchOffset      = 0;
272
                stickValue       = 0;
273
                state            = STATE_INITIALIZE;
274
        }
275
 
276
        if( pitchOffset < 0 )
277
                pitchOffset = 0;
278
 
279
        if( !delayCounter ) {
280
 
281
                /* Durch die Sollwertvorgabe kann hier eine einstellbare Trägheit auf dem Pitch-Wert
282
                 * abgebildet werden. */
283
                int pitchDelta = targetPitchCount - actualPitchCount;
284
 
285
                if( pitchDelta > 3 )
286
                        pitchDelta = 3;
287
                if( pitchDelta < -3 )
288
                        pitchDelta = -3;
289
 
290
                actualPitchCount += pitchDelta;
291
 
292
                delayCounter = delay + 1;
293
        }
294
 
295
        delayCounter--;
296
 
297
        // Pitch-Wert darf nicht < 0 sein
298
        if( actualPitchCount < 0 ) {
299
                actualPitchCount = 0;
300
        }
301
 
302
        // pitchCount als Debug-Wert rausschreiben
303
        DebugOut.Analog[26] = stickValue;
304
        DebugOut.Analog[28] = actualPitchCount;
305
        DebugOut.Analog[29] = pitchOffset;
306
 
307
        return actualPitchCount;
308
}