Rev 1139 | Go to most recent revision | Details | Compare with Previous | 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 | |||
| 1141 | thjac | 26 | static int stickValue = 0; // Aktueller Stick-Wert |
| 27 | static int lastStickValue = 0; // Vorheriger Stick-Wert |
||
| 28 | static int actualPitchCount = 0; // Soll-Pitch-Wert |
||
| 29 | static int targetPitchCount = 0; // Ist-Pitch-Wert |
||
| 30 | static int pitchOffset; // Aktueller Grundgaswert in Neutralstellung |
||
| 31 | static char state; // Zustand |
||
| 1139 | thjac | 32 | static int timer; |
| 33 | static int delay = 0; |
||
| 34 | static int delayCounter = 0; |
||
| 1141 | thjac | 35 | static int temp; // Temporäre Werte; wird mehrfach verwendet |
| 1134 | thjac | 36 | |
| 1141 | thjac | 37 | unsigned char pitchHover = 0; // Standgaswert |
| 1134 | thjac | 38 | |
| 1139 | thjac | 39 | |
| 40 | void pitch_md_init( void ) { |
||
| 41 | pitchHover = eeprom_read_byte( &EEPromArray[ EEPROM_ADR_PITCH_MD_HOVER ] ); |
||
| 42 | } |
||
| 43 | |||
| 44 | |||
| 1134 | thjac | 45 | /* |
| 46 | * Berechnet den aktuellen Pitch-Wert für die Regelung |
||
| 47 | * |
||
| 1141 | thjac | 48 | * Funktionsweise: |
| 1134 | thjac | 49 | * |
| 1141 | thjac | 50 | * 1. Einmalig muß man das Standgas kalibrieren. Standgas ist ein neuer Parameter. |
| 51 | * Dies tut man durch Beätigen des Roll-Sticks in eine beliebige Richtung (Vollausschlag) und |
||
| 52 | * Starten der Motoren. Nun gibt man soviel Gas (in der oberen Hälfte des Stick-Bereichs) wie |
||
| 53 | * der MK gerade noch stabil auf dem Boden steht. Man hält diese Stick-Position solange, bis ein |
||
| 54 | * Piepen ertönt. Damit wurde der Gaswert als Standgasparameter übernommen und im EEProm dauerhaft gespeichert. |
||
| 55 | * Diese Kalibrierung muß mit vollem Akku und nach jeder Gewichtsänderung des MK einmal durchgeführt werden. |
||
| 1134 | thjac | 56 | * |
| 1141 | thjac | 57 | * 2. Für den Flug startet man die Motoren und läßt den Stick los. Die Motoren laufen im Leerlaufgas. |
| 1134 | thjac | 58 | * |
| 1141 | thjac | 59 | * 3. Nun schaltet man auf Standgas durch kurzes Antippen des Sticks nach oben. Die Motoren beschleunigen langsam |
| 60 | * bis zum Erreichen des zuvor kalibriertes Standgaswertes. |
||
| 1134 | thjac | 61 | * |
| 1141 | thjac | 62 | * 4. Nun hebt man wie gewohnt ab. Hierzu hat man den oberen Stick-Bereich zur Verfügung. Da der Grundgaswert |
| 63 | * bei Mittelstellung jetzt knapp unterhalb des Schwebegaswertes liegt, kann man im Flug einfach die Höhe halten, |
||
| 64 | * indem man den Stick in Mittelstellung bewegt. Nach kurzer Zeit (quittiert mit einem Piep) wird die automatische |
||
| 65 | * Höhenregelung aktiviert und solange gehalten, bis man den Stick wieder bewegt. |
||
| 66 | * |
||
| 67 | * 5. Nach der Landung drückt man den Stick ganz nach unten, bis ein Piepen ertönt. Jetzt laufen die Motoren |
||
| 68 | * wieder im Leerlaufgas. Man kann nun neu Starten (ab Punkt 3) oder die Motoren ausschalten. |
||
| 1134 | thjac | 69 | */ |
| 70 | int pitch_md_value( void ) { |
||
| 71 | |||
| 1141 | thjac | 72 | int register rawStickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_initialStickValue(); |
| 1134 | thjac | 73 | |
| 74 | // Sind die Motoren eingeschaltet? |
||
| 75 | if( MotorenEin ) { |
||
| 76 | |||
| 77 | // Vorigen Stick-Wert merken |
||
| 78 | lastStickValue = stickValue; |
||
| 79 | |||
| 80 | /* StickValue exponentiell angleichen, da ausgehend von der Neutralstellung |
||
| 81 | * nur jeweils die halbe Auflösung nach oben und unten zur Verfügung steht. Bei einer |
||
| 82 | * Multiplikation mit 2 ließe sich das Gas im Schwebebereich nicht fein genug einstellen. */ |
||
| 83 | temp = rawStickValue; |
||
| 84 | if( temp > 0 ) { |
||
| 85 | temp = temp + ( ( temp * temp ) / 150 ); |
||
| 86 | } else { |
||
| 87 | temp = temp - ( ( temp * temp ) / 150 ); |
||
| 88 | } |
||
| 89 | |||
| 90 | stickValue = temp; |
||
| 1139 | thjac | 91 | |
| 1134 | thjac | 92 | /* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset |
| 93 | * zuzüglich dem Stick-Wert. */ |
||
| 94 | targetPitchCount = stickValue + pitchOffset; |
||
| 95 | |||
| 96 | switch( state ) { |
||
| 97 | |||
| 98 | /* Entscheidet über Flugbetrieb oder Setup-Betrieb. Für den Setup-Betrieb |
||
| 99 | * muß beim Einschalten der Motoren gleichzeitig der Roll-Stick ganz |
||
| 100 | * betätigt werden (Richtung ist egal). |
||
| 101 | */ |
||
| 102 | case STATE_INITIALIZE: |
||
| 103 | |||
| 104 | if( abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) > 70 ) { |
||
| 105 | state = STATE_SETUP; |
||
| 106 | } else { |
||
| 107 | state = STATE_BEGIN; |
||
| 108 | } |
||
| 109 | break; |
||
| 110 | |||
| 111 | /* Erlaubt die Ermittlung des Parameters PARAM_PITCH_MD_HOVER. Hierzu wird soviel Gas |
||
| 112 | * gegeben, bis der MK kurz vor dem Abheben ist, jedoch noch stabil steht. Um den |
||
| 113 | * Gaswert dauerhaft zu speichern, muß der Stick an der Position verweilen, bis |
||
| 114 | * der Summer die Übernahme akustisch quittiert. Dann müssen die Motoren wieder |
||
| 115 | * ausgeschaltet werden, da dieser Modus nicht für den Flug vorgesehen ist. |
||
| 116 | */ |
||
| 117 | case STATE_SETUP: |
||
| 118 | |||
| 119 | // Im Setup-Modus soll das Gas nicht träge reagieren |
||
| 120 | actualPitchCount = targetPitchCount; |
||
| 121 | |||
| 122 | if( rawStickValue < 20 || ( stickValue - lastStickValue ) ) { |
||
| 123 | timer = PARAM_TIMER_2S; |
||
| 124 | } |
||
| 125 | |||
| 126 | /* Der Stick befindet sich eindeutig in der oberen Hälfte und wurde |
||
| 127 | * seit dem letzten Zyklus nicht bewegt. */ |
||
| 128 | else { |
||
| 129 | timer--; |
||
| 130 | |||
| 131 | /* Die Verweilzeit ist abgelaufen und der aktuelle Pitch-Wert |
||
| 132 | * entspricht nicht dem bereits gespeicherten Wert. */ |
||
| 1139 | thjac | 133 | if( !timer && ( pitchHover != actualPitchCount ) ) { |
| 1134 | thjac | 134 | |
| 135 | // Aktuellen Pitch-Wert in Konfiguration übernehmen |
||
| 1139 | thjac | 136 | pitchHover = actualPitchCount; |
| 137 | |||
| 1134 | thjac | 138 | // Konfiguration dauerhaft speichern |
| 1139 | thjac | 139 | eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_MD_HOVER ], pitchHover ); |
| 1141 | thjac | 140 | |
| 1134 | thjac | 141 | // Signalisieren |
| 142 | beeptime = 500; |
||
| 143 | } |
||
| 144 | } |
||
| 145 | break; |
||
| 146 | |||
| 147 | /* In diesem Zustand steht der MK am Boden und die Motoren laufen auf Leerlaufgas. Ein |
||
| 148 | * kurzes Auslenken des Sticks nach oben schaltet in das Standgas über. |
||
| 149 | */ |
||
| 150 | case STATE_BEGIN: |
||
| 151 | |||
| 152 | // Begrenzung der Pitch-Beschleunigung am Boden |
||
| 153 | delay = PARAM_PITCH_MD_DELAY0; |
||
| 154 | |||
| 155 | if( rawStickValue > PARAM_PITCH_STICK_THRESHOLD ) { |
||
| 1139 | thjac | 156 | pitchOffset = pitchHover; |
| 157 | } else if( pitchOffset == pitchHover ) { |
||
| 1134 | thjac | 158 | state = STATE_BEGIN1; |
| 159 | } |
||
| 160 | break; |
||
| 161 | |||
| 162 | // MK soll erst abheben, weil sonst die Höhenregelung am Boden schon greift |
||
| 163 | case STATE_BEGIN1: |
||
| 164 | |||
| 165 | if( abs( rawStickValue ) > 10 ) { |
||
| 166 | |||
| 167 | // Begrenzung der Pitch-Beschleunigung im Flug |
||
| 168 | delay = PARAM_PITCH_MD_DELAY1; |
||
| 169 | |||
| 170 | if( rawStickValue > 0 ) { |
||
| 171 | state = STATE_READY; |
||
| 172 | } |
||
| 173 | } |
||
| 174 | break; |
||
| 175 | |||
| 176 | /* Die Motoren laufen jetzt mindestens mit Standgas. Der MK ist bereit zum Abheben. |
||
| 177 | * Das Minimalgas kann jetzt nicht mehr unterschritten werden. |
||
| 178 | */ |
||
| 179 | case STATE_READY: |
||
| 180 | |||
| 181 | // Ist ein Restart zulässig? |
||
| 182 | if( PARAM_PITCH_RESTART_ENABLED ) { |
||
| 183 | |||
| 184 | /* Wenn der Gashebel ganz unten steht, Timer für Restart der Pitch-Regelung |
||
| 185 | * starten. Hierfür wird die Variable pitchNeutralTimer verwendet. */ |
||
| 186 | if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 120 ) { |
||
| 187 | timer = PITCH_MIN2_TIMER; |
||
| 188 | } else { |
||
| 189 | timer--; |
||
| 190 | |||
| 191 | /* Gashebel steht seit PITCH_MIN2_TIMER ganz unten; jetzt erfolgt die Initialisierung. */ |
||
| 192 | if( !timer ) { |
||
| 193 | state = STATE_BEGIN; |
||
| 194 | pitchOffset = 0; |
||
| 195 | targetPitchCount = 0; |
||
| 196 | actualPitchCount = 0; |
||
| 197 | |||
| 198 | // Signalisieren |
||
| 199 | beeptime = 500; |
||
| 200 | } |
||
| 201 | } |
||
| 202 | } |
||
| 203 | |||
| 204 | // Min2-Gas einstellen für Lageregelung bei Minimalgas |
||
| 205 | if( targetPitchCount < PARAM_PITCH_MIN2 ) { |
||
| 206 | targetPitchCount = PARAM_PITCH_MIN2; |
||
| 207 | } |
||
| 208 | |||
| 209 | // Stick ist innerhalb der Neutralstellung |
||
| 210 | if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD ) { |
||
| 211 | |||
| 1141 | thjac | 212 | // Ab jetzt gilt eine höhere Pitch-Beschleunigung |
| 213 | delay = PARAM_PITCH_MD_DELAY2; |
||
| 214 | |||
| 1134 | thjac | 215 | // Aktuelle Höhe festhalten (aktiviert noch nicht den Regler) |
| 216 | altcon_lock(); |
||
| 217 | |||
| 218 | // Timer neu setzen |
||
| 219 | timer = PITCH_NEUTRAL_TIMER; |
||
| 220 | state = STATE_WAIT; |
||
| 221 | } |
||
| 222 | break; |
||
| 223 | |||
| 224 | /* Der Stick ist in den für die Neutralstellung gültigen Wertebereich |
||
| 225 | * gelangt. Nun darf innerhalb einer bestimmten Zeit keine Stick-Bewegung |
||
| 226 | * erfolgen, um die automatische Höhenregelung zu aktivieren. */ |
||
| 227 | case STATE_WAIT: |
||
| 228 | |||
| 229 | /* Stick ist innerhalb der Neutralstellung und |
||
| 230 | Stick-Differenzial ist < 2 */ |
||
| 231 | if( abs( rawStickValue ) < PARAM_PITCH_STICK_THRESHOLD && |
||
| 232 | lastStickValue == stickValue ) { |
||
| 233 | |||
| 234 | timer--; |
||
| 235 | |||
| 236 | if( !timer ) { |
||
| 237 | state = STATE_ACTIVATING; |
||
| 238 | } |
||
| 239 | |||
| 240 | // Aktivierungskriterium nicht erfüllt, zurück in INACTIVE |
||
| 241 | } else { |
||
| 242 | state = STATE_READY; |
||
| 243 | } |
||
| 244 | break; |
||
| 245 | |||
| 246 | /* Die automatische Höhenregelung wird jetzt aktiviert. |
||
| 247 | */ |
||
| 248 | case STATE_ACTIVATING: |
||
| 249 | |||
| 250 | // Aktivierung des Höhenreglers mit der zuvor gemerkten Sollhöhe |
||
| 251 | altcon_start(); |
||
| 252 | |||
| 253 | state = STATE_ACTIVE; |
||
| 254 | break; |
||
| 255 | |||
| 256 | /* Die automatische Höhenregelung ist aktiv. */ |
||
| 257 | case STATE_ACTIVE: |
||
| 258 | |||
| 259 | // Stick ist außerhalb der Neutralstellung |
||
| 260 | if( abs( rawStickValue ) > PARAM_PITCH_STICK_THRESHOLD ) { |
||
| 261 | |||
| 262 | // Höhenregler deaktivieren |
||
| 263 | altcon_stop(); |
||
| 264 | |||
| 265 | pitchOffset -= altcon_avgerror() / 4; |
||
| 266 | targetPitchCount = stickValue + pitchOffset; |
||
| 267 | state = STATE_READY; |
||
| 268 | } |
||
| 269 | break; |
||
| 270 | } |
||
| 271 | |||
| 272 | // Motoren sind aus |
||
| 273 | } else { |
||
| 274 | |||
| 275 | /* Nach dem Einschalten der Motoren wird pitchOffset auf PARAM_PITCH_OVER gesetzt. |
||
| 276 | */ |
||
| 277 | actualPitchCount = 0; |
||
| 278 | targetPitchCount = 0; |
||
| 279 | pitchOffset = 0; |
||
| 280 | stickValue = 0; |
||
| 281 | state = STATE_INITIALIZE; |
||
| 282 | } |
||
| 283 | |||
| 1141 | thjac | 284 | /* Durch die Sollwertvorgabe kann hier eine einstellbare Trägheit auf dem Pitch-Wert |
| 285 | * abgebildet werden. */ |
||
| 286 | int pitchDelta = targetPitchCount - actualPitchCount; |
||
| 1134 | thjac | 287 | |
| 1141 | thjac | 288 | if( pitchDelta > 3 ) |
| 289 | pitchDelta = 3; |
||
| 290 | if( pitchDelta < -3 ) |
||
| 291 | pitchDelta = -3; |
||
| 1134 | thjac | 292 | |
| 1141 | thjac | 293 | if( !delayCounter ) { |
| 1134 | thjac | 294 | actualPitchCount += pitchDelta; |
| 1141 | thjac | 295 | delayCounter = delay + 1; |
| 1134 | thjac | 296 | } |
| 297 | |||
| 298 | delayCounter--; |
||
| 299 | |||
| 1139 | thjac | 300 | if( actualPitchCount < 0 ) |
| 1134 | thjac | 301 | actualPitchCount = 0; |
| 1139 | thjac | 302 | |
| 1134 | thjac | 303 | // pitchCount als Debug-Wert rausschreiben |
| 304 | DebugOut.Analog[26] = stickValue; |
||
| 305 | DebugOut.Analog[28] = actualPitchCount; |
||
| 306 | DebugOut.Analog[29] = pitchOffset; |
||
| 307 | |||
| 308 | return actualPitchCount; |
||
| 309 | } |