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