Rev 1134 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1134 | Rev 1141 | ||
|---|---|---|---|
| Line 8... | Line 8... | ||
| 8 | #include "fc.h" |
8 | #include "fc.h" |
| 9 | #include "altcon.h" |
9 | #include "altcon.h" |
| 10 | #include "pitch.h" |
10 | #include "pitch.h" |
| 11 | #include "pitch_neutral.h" |
11 | #include "pitch_neutral.h" |
| Line 12... | Line 12... | ||
| 12 | 12 | ||
| 13 | #define STATE_STARTUP_WAIT 0x00 // Init-Timeout beim Einschalten abwarten |
13 | #define STATE_INITIALIZE 0x00 // Initialisierung nach Einschalten der Motoren |
| 14 | #define STATE_STARTUP_INIT 0x01 // Initialisierung beim Einschalten |
14 | #define STATE_SETUP 0x01 // Kalibrierung des Gas-Sticks |
| 15 | #define STATE_BEGIN 0x02 // Anfangszustand nach Einschalten der Motoren |
15 | #define STATE_SETUP1 0x02 // |
| 16 | #define STATE_INITIALIZING 0x03 // Initialisierungsphase |
16 | #define STATE_BEGIN 0x03 // Startphase |
| 17 | #define STATE_MANUAL 0x04 // Manuelle Kontrolle, Höhenregelung in Konfiguration deaktiviert |
17 | #define STATE_BEGIN1 0x04 // |
| 18 | #define STATE_INACTIVE 0x05 // Manuelle Kontrolle |
18 | #define STATE_INACTIVE 0x05 // Manuelle Kontrolle |
| 19 | #define STATE_WAIT 0x06 // Warten auf Einschalten der Höhenregelung |
19 | #define STATE_WAIT 0x06 // Warten auf Einschalten der Höhenregelung |
| 20 | #define STATE_ACTIVATING 0x07 // Aktivierung der Höhenregelung |
20 | #define STATE_ACTIVATING 0x07 // Aktivierung der Höhenregelung |
| Line 21... | Line 21... | ||
| 21 | #define STATE_ACTIVE 0x08 // Höhenregelung ist aktiv |
21 | #define STATE_ACTIVE 0x08 // Höhenregelung ist aktiv |
| 22 | 22 | ||
| 23 | 23 | ||
| 24 | static int stickValue = 0; // Aktueller Stick-Wert |
24 | static int stickValue = 0; // Aktueller Stick-Wert |
| 25 | static int lastStickValue = 0; // Vorheriger Stick-Wert |
25 | static int lastStickValue = 0; // Vorheriger Stick-Wert |
| Line -... | Line 26... | ||
| - | 26 | static int pitchOffset = 0; // Aktueller Grundgaswert in Neutralstellung |
|
| - | 27 | static char state; // Zustand |
|
| - | 28 | static int temp; |
|
| 26 | static int pitchOffset = 0; // Aktueller Grundgaswert in Neutralstellung |
29 | |
| 27 | static char state = STATE_STARTUP_WAIT; // Zustand |
30 | char pitchNeutralDiff = 0; |
| 28 | static int temp; |
31 | |
| 29 | 32 | ||
| Line -... | Line 33... | ||
| - | 33 | /* Wird verwendet, um das Umschalten auf automatische Höhenregelung |
|
| - | 34 | * nach Erreichen der Neutralstellung zu verzögern. |
|
| - | 35 | */ |
|
| - | 36 | static int pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
|
| - | 37 | ||
| 30 | /* Wird verwendet, um das Umschalten auf automatische Höhenregelung |
38 | |
| 31 | * nach Erreichen der Neutralstellung zu verzögern. |
39 | void pitch_neutral_init( void ) { |
| 32 | */ |
40 | pitchNeutralDiff = eeprom_read_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ] ); |
| - | 41 | } |
|
| - | 42 | ||
| - | 43 | ||
| - | 44 | /* |
|
| - | 45 | * Berechnet den aktuellen Pitch-Wert für die Regelung |
|
| - | 46 | * |
|
| - | 47 | * Funktionsweise: |
|
| - | 48 | * |
|
| - | 49 | * 1. Vor Verwendung der Pitch-Steuerung muß ein Parameter für den Gas-Stick eingestellt werden. Hierzu |
|
| - | 50 | * bewegt man den Roll-Stick ganz nach außen und startet die Motoren. Man befindet sich im Setup-Modus |
|
| - | 51 | * und die Motoren laufen unabhängig vom Gas-Stick im Leerlauf. Im Display auf Seite 12 muß nun der |
|
| - | 52 | * beste Wert für "StickDiff" gefunden werden. Den Wert ändern kann man mit dem Roll-Stick. Man betätigt |
|
| - | 53 | * nun den Gas-Stick nach oben und läßt ihn los, so daß er in die Mittelstellung zurückspringt. Der |
|
| 33 | static int pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
54 | * optimale Wert sorgt dafür, daß das gewollte Loslassen von einer normalen Bewegung unterschieden wird. |
| 34 | 55 | * Jedesmal, wenn die SW das Loslassen als solches erkannt hat, ertönt ein Signal. Dieses darf jedoch |
|
| 35 | 56 | * nicht ertönen, wenn man den Stick mit normaler Geschwindigkeit in die Mittelstellung zurückbewegt. |
|
| 36 | /* |
57 | * Wurde ein guter Wert eingestellt, verläßt man den Setup-Modus durch Ausschalten der Motoren. Der |
| 37 | * Berechnet den aktuellen Pitch-Wert für die Regelung |
58 | * ermittelte Wert wird permanent im EEProm gespeichert und muß nicht neu ermittelt werden. |
| 38 | * |
59 | * |
| 39 | * Nach dem Einschalten der FC wird der aktuelle Gas-Stick-Wert gelesen und als Kalibrierungswert |
60 | * 2. Nach dem Einschalten der FC wird die aktuelle Gas-Stick-Position gemerkt und als Kalibrierungswert |
| 40 | * für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite |
61 | * für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite |
| 41 | * keine Rolle. |
62 | * keine Rolle. |
| 42 | * |
63 | * |
| 43 | * Nach Einschalten der Motoren geht der Stick in Neutralstellung. Diese Stick-Bewegung wird ignoriert |
64 | * 3. Nach Einschalten der Motoren geht der Stick in Neutralstellung. Diese Stick-Bewegung wird ignoriert |
| - | 65 | * und die Motoren drehen mit dem eingestellten MinGas2. Ausgehend von der Neutralstellung wird nun |
|
| 44 | * und die Motoren drehen mit dem eingestellten MinGas2. Ausgehend von der Neutralstellung wird nun |
66 | * durch Bewegen des Sticks im oberen Bereich das Gas geregelt. |
| 45 | * durch Bewegen des Sticks im oberen Bereich das Gas geregelt. |
67 | * |
| 46 | * |
68 | * 4. Das erstmalige Aktivieren der automatischen Höhenregelung erfolgt durch Loslassen des Sticks im |
| 47 | * Das erstmalige Aktivieren der automatischen Höhenregelung erfolgt durch Loslassen des Sticks im |
69 | * Schwebeflug. Der zuvor aktuelle Stick-Wert wird als Wert in Neutralstellung übernommen und die |
| 48 | * Schwebeflug. Der zuvor aktuelle Stick-Wert wird als Wert in Neutralstellung übernommen und die |
70 | * automatische Höhenregelung sofort aktiviert. Wichtig hierfür ist die unter Punkt 1 vorgenommene |
| 49 | * automatische Höhenregelung sofort aktiviert. |
71 | * Parameterfindung. Stimmt dieser Wert nicht, führt das zur Wegnahme des Gases und somit zum Sinken. |
| 50 | * |
72 | * |
| 51 | * Sobald der Stick die Neutralstellung verläßt, wird die automatische Höhenregelung deaktiviert |
73 | * 5. 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 |
74 | * und der vorige Pitch-Wert als Wert der Neutralstellung übernommen. Der Pitch läßt sich nun |
| 53 | * über den gesamten Stick-Bereich regeln. |
75 | * über den gesamten Stick-Bereich regeln. |
| 54 | * |
76 | * |
| 55 | * Erreicht der Stick ein weiteres Mal die Neutralstellung, wird die automatische Höhenregelung |
77 | * 6. 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 |
78 | * wieder aktiviert, jetzt jedoch immer mit einer zeitlichen Verzögerung. Nur so ist ein |
| 57 | * ungestörtes manuelles Steuern möglich. |
79 | * ungestörtes manuelles Steuern möglich. |
| 58 | * |
80 | * |
| Line 59... | Line 81... | ||
| 59 | * Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt. |
81 | * 7. Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt. |
| Line 60... | Line 82... | ||
| 60 | * Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb |
82 | * 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 |
83 | * eines Wertes zu gelangen, der die Lageregelung außer Funktion setzt, wird ein zweiter Wert für |
| Line 62... | Line 84... | ||
| 62 | * Minimalgas konfiguriert, der greift, sobald erstmalig die automatische Höhenregelung aktiviert wurde. |
84 | * Minimalgas konfiguriert, der greift, sobald erstmalig die automatische Höhenregelung aktiviert wurde. |
| 63 | */ |
85 | */ |
| Line 64... | Line 86... | ||
| 64 | int pitch_neutral_value( void ) { |
86 | int pitch_neutral_value( void ) { |
| 65 | 87 | ||
| 66 | int register pitchCount = 0; |
88 | int register pitchCount = 0; |
| 67 | 89 | ||
| 68 | // Sind die Motoren eingeschaltet? |
90 | // Sind die Motoren eingeschaltet? Ach ja, im Zustand STATE_SETUP sind die Motoren aus... |
| 69 | if( MotorenEin ) { |
91 | if( MotorenEin ) { |
| 70 | 92 | ||
| 71 | // Vorigen Stick-Wert merken |
93 | // Vorigen Stick-Wert merken |
| 72 | lastStickValue = stickValue; |
94 | lastStickValue = stickValue; |
| Line 73... | Line -... | ||
| 73 | - | ||
| 74 | /* StickValue exponentiell angleichen, da ausgehend von der Neutralstellung |
95 | |
| 75 | * nur jeweils die halbe Auflösung nach oben und unten zur Verfügung steht. Bei einer |
96 | /* StickValue exponentiell angleichen, da ausgehend von der Neutralstellung |
| 76 | * Multiplikation mit 2 ließe sich das Gas im Schwebebereich nicht fein genug einstellen. */ |
97 | * nur jeweils die halbe Auflösung nach oben und unten zur Verfügung steht. Bei einer |
| 77 | temp = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_stickoffset(); |
98 | * Multiplikation mit 2 ließe sich das Gas im Schwebebereich nicht fein genug einstellen. */ |
| 78 | if( temp > 0 ) { |
99 | temp = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_initialStickValue(); |
| Line 79... | Line 100... | ||
| 79 | temp = temp + ( ( temp * temp ) / 150 ); |
100 | if( temp > 0 ) { |
| - | 101 | temp = temp + ( ( temp * temp ) / 150 ); |
|
| - | 102 | } else { |
|
| - | 103 | temp = temp - ( ( temp * temp ) / 150 ); |
|
| - | 104 | } |
|
| - | 105 | ||
| - | 106 | stickValue = temp; |
|
| - | 107 | ||
| - | 108 | /* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset |
|
| - | 109 | * zuzüglich dem Stick-Wert. */ |
|
| - | 110 | pitchCount = stickValue + pitchOffset; |
|
| - | 111 | ||
| - | 112 | switch( state ) { |
|
| - | 113 | ||
| - | 114 | /* Entscheidet über Flugbetrieb oder Setup-Betrieb. Für den Setup-Betrieb |
|
| - | 115 | * muß beim Einschalten der Motoren gleichzeitig der Roll-Stick ganz |
|
| - | 116 | * betätigt werden (Richtung ist egal). |
|
| - | 117 | */ |
|
| - | 118 | case STATE_INITIALIZE: |
|
| - | 119 | ||
| - | 120 | if( abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) > 70 ) { |
|
| - | 121 | state = STATE_SETUP; |
|
| - | 122 | } else { |
|
| - | 123 | state = STATE_BEGIN; |
|
| - | 124 | } |
|
| - | 125 | break; |
|
| - | 126 | ||
| - | 127 | case STATE_SETUP: |
|
| - | 128 | if( stickValue < PARAM_PITCH_STICK_THRESHOLD && |
|
| - | 129 | abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) < PARAM_PITCH_STICK_THRESHOLD ) { |
|
| - | 130 | state = STATE_SETUP1; |
|
| - | 131 | } |
|
| - | 132 | ||
| - | 133 | // Im Setup immer mit Leerlaufgas |
|
| - | 134 | pitchCount = 0; |
|
| - | 135 | ||
| - | 136 | break; |
|
| - | 137 | ||
| - | 138 | case STATE_SETUP1: |
|
| - | 139 | ||
| - | 140 | // Roll-Stick nach links |
|
| - | 141 | if( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] < -20 ) { |
|
| - | 142 | if( pitchNeutralDiff < 20 ) { |
|
| - | 143 | pitchNeutralDiff++; |
|
| - | 144 | ||
| - | 145 | // Konfiguration dauerhaft speichern |
|
| - | 146 | eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ], pitchNeutralDiff ); |
|
| - | 147 | ||
| - | 148 | // Signalisieren |
|
| - | 149 | beeptime = 500; |
|
| - | 150 | ||
| - | 151 | state = STATE_SETUP; |
|
| - | 152 | } |
|
| - | 153 | } |
|
| - | 154 | ||
| - | 155 | // Roll-Stick nach rechts |
|
| - | 156 | if( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] > 20 ) { |
|
| - | 157 | if( pitchNeutralDiff > 0 ) { |
|
| - | 158 | pitchNeutralDiff--; |
|
| - | 159 | ||
| - | 160 | // Konfiguration dauerhaft speichern |
|
| - | 161 | eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ], pitchNeutralDiff ); |
|
| - | 162 | ||
| - | 163 | // Signalisieren |
|
| - | 164 | beeptime = 500; |
|
| - | 165 | ||
| - | 166 | state = STATE_SETUP; |
|
| - | 167 | } |
|
| - | 168 | } |
|
| - | 169 | ||
| - | 170 | if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) && |
|
| 80 | } else { |
171 | ( lastStickValue - stickValue >= pitchNeutralDiff ) ) { |
| 81 | temp = temp - ( ( temp * temp ) / 150 ); |
172 | |
| Line 82... | Line 173... | ||
| 82 | } |
173 | state = STATE_SETUP; |
| 83 | 174 | ||
| 84 | // Original-Stick-Wert holen und glätten |
175 | // Signalisieren |
| Line 85... | Line 176... | ||
| 85 | stickValue = ( temp + 2 * lastStickValue ) / 3; |
176 | beeptime = 500; |
| Line 86... | Line 177... | ||
| 86 | 177 | } |
|
| 87 | /* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset |
178 | |
| 88 | * zuzüglich dem Stick-Wert. */ |
179 | // Im Setup immer mit Leerlaufgas |
| 89 | pitchCount = stickValue + pitchOffset; |
180 | pitchCount = 0; |
| Line 90... | Line 181... | ||
| 90 | 181 | ||
| Line 91... | Line 182... | ||
| 91 | switch( state ) { |
182 | break; |
| 92 | 183 | ||
| Line 93... | Line 184... | ||
| 93 | case STATE_BEGIN: |
184 | case STATE_BEGIN: |
| Line 118... | Line 209... | ||
| 118 | state = STATE_BEGIN; |
209 | state = STATE_BEGIN; |
| 119 | } |
210 | } |
| Line 120... | Line 211... | ||
| 120 | 211 | ||
| 121 | // Ist die Neutralstellung erreicht? |
212 | // 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 ) { |
213 | if( abs( stickValue ) <= PARAM_PITCH_STICK_THRESHOLD ) { |
| 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; |
214 | state = STATE_ACTIVATING; |
| 140 | } |
215 | } |
| Line 141... | Line 216... | ||
| 141 | break; |
216 | break; |
| 142 | 217 | ||
| Line 237... | Line 312... | ||
| 237 | * da der Kopter sonst sofort hochschießen würde. |
312 | * da der Kopter sonst sofort hochschießen würde. |
| 238 | */ |
313 | */ |
| 239 | pitchCount = 0; |
314 | pitchCount = 0; |
| 240 | pitchOffset = 0; |
315 | pitchOffset = 0; |
| 241 | stickValue = 0; |
316 | stickValue = 0; |
| 242 | state = STATE_BEGIN; |
317 | state = STATE_INITIALIZE; |
| 243 | } |
318 | } |
| Line 244... | Line 319... | ||
| 244 | 319 | ||
| 245 | if( pitchOffset < 0 ) |
320 | if( pitchOffset < 0 ) |