Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1122 | 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 | } |