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 | } |