Subversion Repositories FlightCtrl

Rev

Rev 1141 | Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1134 thjac 1
/* pitch_neutral.c
2
 *
3
 * Copyright 2009 Thomas Jachmann
4
 */
5
 
6
#include "main.h"
7
#include "parameter.h"
8
#include "fc.h"
9
#include "altcon.h"
10
#include "pitch.h"
11
#include "pitch_neutral.h"
12
 
13
#define STATE_STARTUP_WAIT              0x00                    // Init-Timeout beim Einschalten abwarten
14
#define STATE_STARTUP_INIT              0x01                    // Initialisierung beim Einschalten
15
#define STATE_BEGIN                     0x02                    // Anfangszustand nach Einschalten der Motoren
16
#define STATE_INITIALIZING              0x03                    // Initialisierungsphase
17
#define STATE_MANUAL                    0x04                    // Manuelle Kontrolle, Höhenregelung in Konfiguration deaktiviert
18
#define STATE_INACTIVE                  0x05                    // Manuelle Kontrolle
19
#define STATE_WAIT                      0x06                    // Warten auf Einschalten der Höhenregelung
20
#define STATE_ACTIVATING                0x07                    // Aktivierung der Höhenregelung
21
#define STATE_ACTIVE                    0x08                    // Höhenregelung ist aktiv
22
 
23
 
24
static int      stickValue              = 0;                            // Aktueller Stick-Wert
25
static int      lastStickValue          = 0;                            // Vorheriger Stick-Wert
26
static int      pitchOffset             = 0;                            // Aktueller Grundgaswert in Neutralstellung
27
static char     state                   = STATE_STARTUP_WAIT;           // Zustand
28
static int      temp;
29
 
30
/* Wird verwendet, um das Umschalten auf automatische Höhenregelung
31
 * nach Erreichen der Neutralstellung zu verzögern.
32
 */
33
static int      pitchNeutralTimer       = PITCH_NEUTRAL_TIMER;
34
 
35
 
36
/*
37
 * Berechnet den aktuellen Pitch-Wert für die Regelung
38
 *
39
 * Nach dem Einschalten der FC wird der aktuelle Gas-Stick-Wert gelesen und als Kalibrierungswert
40
 * für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite
41
 * keine Rolle.
42
 *
43
 * Nach Einschalten der Motoren geht der Stick in Neutralstellung. Diese Stick-Bewegung wird ignoriert
44
 * und die Motoren drehen mit dem eingestellten MinGas2. Ausgehend von der Neutralstellung wird nun
45
 * durch Bewegen des Sticks im oberen Bereich das Gas geregelt.
46
 *
47
 * Das erstmalige Aktivieren der automatischen Höhenregelung erfolgt durch Loslassen des Sticks im
48
 * Schwebeflug. Der zuvor aktuelle Stick-Wert wird als Wert in Neutralstellung übernommen und die
49
 * automatische Höhenregelung sofort aktiviert.
50
 *
51
 * Sobald der Stick die Neutralstellung verläßt, wird die automatische Höhenregelung deaktiviert
52
 * und der vorige Pitch-Wert als Wert der Neutralstellung übernommen. Der Pitch läßt sich nun
53
 * über den gesamten Stick-Bereich regeln.
54
 *
55
 * Erreicht der Stick ein weiteres Mal die Neutralstellung, wird die automatische Höhenregelung
56
 * wieder aktiviert, jetzt jedoch immer mit einer zeitlichen Verzögerung. Nur so ist ein
57
 * ungestörtes manuelles Steuern möglich.
58
 *
59
 * Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt.
60
 * Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb
61
 * eines Wertes zu gelangen, der die Lageregelung außer Funktion setzt, wird ein zweiter Wert für
62
 * Minimalgas konfiguriert, der greift, sobald erstmalig die automatische Höhenregelung aktiviert wurde.
63
 */
64
int pitch_neutral_value( void ) {
65
 
66
        int register pitchCount = 0;
67
 
68
        // Sind die Motoren eingeschaltet?
69
        if( MotorenEin ) {
70
 
71
                // Vorigen Stick-Wert merken
72
                lastStickValue = stickValue;
73
 
74
                /* StickValue exponentiell angleichen, da ausgehend von der Neutralstellung
75
                 * nur jeweils die halbe Auflösung nach oben und unten zur Verfügung steht. Bei einer
76
                 * Multiplikation mit 2 ließe sich das Gas im Schwebebereich nicht fein genug einstellen. */
77
                temp = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_stickoffset();
78
                if( temp > 0 ) {
79
                        temp = temp + ( ( temp * temp ) / 150 );
80
                } else {
81
                        temp = temp - ( ( temp * temp ) / 150 );
82
                }
83
 
84
                // Original-Stick-Wert holen und glätten
85
                stickValue = ( temp + 2 * lastStickValue ) / 3;
86
 
87
                /* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset
88
                 * zuzüglich dem Stick-Wert. */
89
                pitchCount = stickValue + pitchOffset;
90
 
91
                switch( state ) {
92
 
93
                        case STATE_BEGIN:
94
 
95
                                // Schnelles Bewegen aus dem oberen Bereich des Sticks in Neutralstellung
96
                                if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&
97
                                    ( lastStickValue - stickValue >= PARAM_PITCH_NEUTRAL_DIFF ) ) {
98
 
99
                                        pitchOffset       = lastStickValue;
100
 
101
                                        state             = STATE_INITIALIZING;
102
                                        pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
103
                                }
104
                                break;
105
 
106
                        case STATE_INITIALIZING:
107
 
108
                                // Während der Initialisierung das Gas konstant halten
109
                                pitchCount = pitchOffset;
110
 
111
                                pitchNeutralTimer--;
112
 
113
                                /* Läuft der Timer ab, bevor der Stick die Neutralstellung erreicht,
114
                                 * wird die Aktion nicht als "schnelles Bewegen in Neutralstellung"
115
                                 * gedeutet. */
116
                                if( !pitchNeutralTimer ) {
117
                                        pitchOffset = 0;
118
                                        state       = STATE_BEGIN;
119
                                }
120
 
121
                                // Ist die Neutralstellung erreicht?
122
                                if( abs( stickValue ) <= PARAM_PITCH_STICK_THRESHOLD ) {
123
 
124
                                        // Ist die Höhenregelung aktiviert?
125
                                        if( EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG ) {
126
                                                state = STATE_ACTIVATING;
127
                                        } else {
128
                                                state = STATE_MANUAL;
129
                                        }
130
                                }
131
                                break;
132
 
133
                        /* Wenn die Höhenregelung per Konfiguration deaktiviert ist, verbleibt
134
                         * die Funktion in diesem Zustand. */
135
                        case STATE_MANUAL:
136
 
137
                                // Min2-Gas einstellen für Lageregelung bei Minimalgas
138
                                if( pitchCount < PARAM_PITCH_MIN2 ) {
139
                                        pitchCount = PARAM_PITCH_MIN2;
140
                                }
141
                                break;
142
 
143
                        /* Die Höhenregelung ist per Konfiguration aktiviert, jedoch befindet
144
                         * sich der Stick außerhalb des als Neutralstellung anerkannten
145
                         * Wertebereiches. Es wird manuell geregelt. */
146
                        case STATE_INACTIVE:
147
 
148
                                // Ist ein Restart zulässig?
149
                                if( PARAM_PITCH_RESTART_ENABLED ) {
150
 
151
                                        /* Wenn der Gashebel ganz unten steht, Timer für Reduzierung des Minimalgaswertes
152
                                         * starten. Hierfür wird die Variable pitchNeutralTimer verwendet. */
153
                                        if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 120 ) {
154
                                                pitchNeutralTimer = PITCH_MIN2_TIMER;
155
                                        } else {
156
                                                pitchNeutralTimer--;
157
 
158
                                                /* Gashebel steht seit PITCH_MIN2_TIMER ganz unten; jetzt erfolgt die Initialisierung. */
159
                                                if( !pitchNeutralTimer ) {
160
                                                        state       = STATE_BEGIN;
161
                                                        pitchOffset = 0;
162
 
163
                                                        // Signalisieren
164
                                                        beeptime    = 500;
165
                                                }
166
                                        }
167
                                }
168
 
169
                                // Min2-Gas einstellen für Lageregelung bei Minimalgas
170
                                if( pitchCount < PARAM_PITCH_MIN2 ) {
171
                                        pitchCount = PARAM_PITCH_MIN2;
172
                                }
173
 
174
                                // Stick ist innerhalb der Neutralstellung
175
                                if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD ) {
176
 
177
                                        // Timer neu setzen
178
                                        pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
179
                                        state             = STATE_WAIT;
180
                                }
181
                                break;
182
 
183
                        /* Der Stick ist in den für die Neutralstellung gültigen Wertebereich
184
                         * gelangt. Nun darf innerhalb einer bestimmten Zeit keine Stick-Bewegung
185
                         * erfolgen, um die automatische Höhenregelung zu aktivieren. */
186
                        case STATE_WAIT:
187
 
188
                                /* Stick ist innerhalb der Neutralstellung und
189
                                   Stick-Differenzial ist < 2 */
190
                                if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD &&
191
                                    lastStickValue == stickValue ) {
192
 
193
                                        pitchNeutralTimer--;
194
 
195
                                        if( !pitchNeutralTimer ) {
196
                                                state = STATE_ACTIVATING;
197
                                        }
198
 
199
                                // Aktivierungskriterium nicht erfüllt, zurück in INACTIVE
200
                                } else {
201
                                        state = STATE_INACTIVE;
202
                                }
203
                                break;
204
 
205
                        /* Die automatische Höhenregelung wird jetzt aktiviert. Der aktuelle
206
                         * Luftdruck wird gespeichert und notwendige Werte für den Regler
207
                         * werden initialisiert. */
208
                        case STATE_ACTIVATING:
209
 
210
                                // Höhenregler starten
211
                                altcon_start();
212
 
213
                                state = STATE_ACTIVE;
214
 
215
                                break;
216
 
217
                        /* Die automatische Höhenregelung ist aktiv. */                
218
                        case STATE_ACTIVE:
219
 
220
                                // Stick ist außerhalb der Neutralstellung
221
                                if( abs( stickValue ) > PARAM_PITCH_STICK_THRESHOLD ) {
222
                                        pitchOffset -= altcon_avgerror() / 4;
223
                                        pitchCount   = stickValue + pitchOffset;
224
 
225
                                        // Höhenregler abschalten
226
                                        altcon_stop();
227
 
228
                                        state        = STATE_INACTIVE;
229
                                }
230
                                break;
231
                }
232
 
233
        // Motoren sind aus
234
        } else {
235
 
236
                /* Nach dem Einschalten der Motoren darf pitchOffset keinen hohen Wert haben,
237
                 * da der Kopter sonst sofort hochschießen würde.
238
                 */
239
                pitchCount  = 0;
240
                pitchOffset = 0;
241
                stickValue  = 0;
242
                state       = STATE_BEGIN;
243
        }
244
 
245
        if( pitchOffset < 0 )
246
                pitchOffset = 0;
247
 
248
        // Pitch-Wert darf nicht < 0 sein
249
        if( pitchCount < 0 ) {
250
                pitchCount = 0;
251
        }
252
 
253
        // pitchCount als Debug-Wert rausschreiben
254
        DebugOut.Analog[26] = stickValue;
255
        DebugOut.Analog[28] = pitchCount;
256
        DebugOut.Analog[29] = pitchOffset;
257
 
258
        return pitchCount;
259
}