Rev 1139 | Go to most recent revision | Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1134 | 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 | } |