Rev 1132 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1132 | Rev 1133 | ||
|---|---|---|---|
| Line 1... | Line 1... | ||
| 1 | /* pitch.c |
1 | /* pitch.c |
| 2 | * |
2 | * |
| 3 | * Copyright 2009 Thomas Jachmann |
- | |
| 4 | * |
- | |
| 5 | * Pitch-Steuerung |
3 | * Pitch-Steuerung |
| 6 | */ |
4 | */ |
| Line 7... | Line 5... | ||
| 7 | 5 | ||
| 8 | #include "main.h" |
6 | #include "main.h" |
| 9 | #include "parameter.h" |
- | |
| 10 | #include "pitch_neutral.h" |
7 | #include "parameter.h" |
| 11 | #include "pitch_md.h" |
8 | #include "fc.h" |
| Line -... | Line 9... | ||
| - | 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 12... | Line 42... | ||
| 12 | #include "pitch.h" |
42 | long altIntegral = 0; |
| 13 | - | ||
| Line 14... | Line 43... | ||
| 14 | 43 | int temp; // Temporäre Werte; wird mehrfach verwendet |
|
| 15 | // Zeiger auf den durch das Setting bestimmten Pitch-Steuerungsalgorithmus |
44 | |
| Line 16... | Line 45... | ||
| 16 | int (* pitch_value_ptr)( void ); |
45 | char pitchNeutralStartup = 1; // 1=Gas-Stick beim Einschalten in Mittelstellung |
| 17 | 46 | ||
| - | 47 | ||
| 18 | // Prototyp |
48 | extern unsigned char Notlandung; // aus fc.c |
| - | 49 | ||
| 19 | int pitch_mk_value( void ); |
50 | |
| - | 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 |
|
| 20 | 72 | * ungestörtes manuelles Steuern möglich. |
|
| 21 | 73 | * |
|
| 22 | /* |
- | |
| 23 | * Führt die Initialisierung der Pitch-Steuerung durch. Diese Funktion |
- | |
| 24 | * wird nach jeder Setting-Auswahl sowie nach jeder Setting-Änderung |
- | |
| 25 | * aufgerufen. |
- | |
| 26 | */ |
- | |
| 27 | void pitch_init( void ) { |
- | |
| 28 | - | ||
| 29 | // FIXME Funktioniert noch nicht |
- | |
| 30 | switch( PARAM_PITCH_MODE ) { |
- | |
| 31 | case PARAM_PITCH_MODE_NEUTRAL: |
- | |
| Line 32... | Line -... | ||
| 32 | pitch_value_ptr = pitch_neutral_value; |
- | |
| 33 | break; |
74 | * Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt. |
| 34 | - | ||
| Line 35... | Line 75... | ||
| 35 | case PARAM_PITCH_MODE_MD: |
75 | * Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb |
| 36 | pitch_value_ptr = pitch_md_value; |
76 | * eines Wertes zu gelangen, der die Lageregelung außer Funktion setzt, wird ein zweiter Wert für |
| Line -... | Line 77... | ||
| - | 77 | * Minimalgas konfiguriert, der greift, sobald erstmalig die automatische Höhenregelung aktiviert wurde. |
|
| - | 78 | */ |
|
| Line -... | Line 79... | ||
| - | 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 | ||
| 37 | break; |
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 { |
|
| 38 | 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 ) { |
|
| 39 | default: |
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? |
|
| 40 | pitch_value_ptr = pitch_mk_value; |
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 ) { |
|
| Line -... | Line 245... | ||
| - | 245 | pitchOffset -= averageN / 4; |
|
| - | 246 | pitchCount = stickValue + pitchOffset; |
|
| - | 247 | lastN = 0; |
|
| - | 248 | state = STATE_INACTIVE; |
|
| - | 249 | ||
| 41 | } |
250 | // Abschaltung der Höhenregelung signalisieren |
| - | 251 | beeptime = 500; |
|
| 42 | 252 | } |
|
| - | 253 | break; |
|
| - | 254 | } |
|
| - | 255 | ||
| Line -... | Line 256... | ||
| - | 256 | // Motoren sind aus |
|
| - | 257 | } else { |
|
| - | 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 | ||
| 43 | // Hier können weitere Initialisierungen folgen |
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 | ||
| 44 | } |
277 | // Die Einschaltphase ist jetzt beendet |
| - | 278 | state = STATE_BEGIN; |
|
| - | 279 | ||
| - | 280 | break; |
|
| - | 281 | ||
| - | 282 | default: |
|
| - | 283 | ||
| - | 284 | /* Nach dem Einschalten der Motoren darf pitchOffset keinen hohen Wert haben, |
|
| - | 285 | * da der Kopter sonst sofort hochschießen würde. |
|
| - | 286 | */ |
|
| - | 287 | pitchCount = 0; |
|
| 45 | 288 | pitchOffset = 0; |
|
| - | 289 | stickValue = 0; |
|
| - | 290 | state = STATE_BEGIN; |
|
| - | 291 | } |
|
| - | 292 | } |
|
| - | 293 | ||
| - | 294 | if( pitchOffset < 0 ) |
|
| - | 295 | pitchOffset = 0; |
|
| 46 | 296 | ||
| Line 47... | Line 297... | ||
| 47 | int pitch_value( void ) { |
297 | // Pitch-Wert darf nicht < 0 sein |
| 48 | switch( PARAM_PITCH_MODE ) { |
298 | if( pitchCount < 0 ) { |
| 49 | case PARAM_PITCH_MODE_NEUTRAL: |
299 | pitchCount = 0; |
| 50 | return pitch_neutral_value(); |
300 | } |
| - | 301 | ||
| 51 | 302 | // pitchCount als Debug-Wert rausschreiben |
|
| - | 303 | DebugOut.Analog[26] = stickValue; |
|
| - | 304 | DebugOut.Analog[28] = pitchCount; |
|
| 52 | case PARAM_PITCH_MODE_MD: |
305 | DebugOut.Analog[29] = pitchOffset; |
| - | 306 | ||
| 53 | return pitch_md_value(); |
307 | return pitchCount; |
| 54 | 308 | } |
|
| Line 55... | Line 309... | ||
| 55 | default: |
309 | |
| 56 | return pitch_mk_value(); |
310 | |
| Line -... | Line 311... | ||
| - | 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 |
|
| 57 | } |
358 | if( n > altMax ) |
| 58 | } |
359 | n = altMax; |