WebSVN - FlightCtrl - Diff - Rev 1134 and 1141 - /branches/thjac/V1_11/pitch

 /* pitch_neutral.c
+ *
  * Copyright 2009 Thomas Jachmann
  */
 #include "main.h"
 #include "parameter.h"
 #include "fc.h"
 #include "altcon.h"
 #include "pitch.h"
 #include "pitch_neutral.h"
-#define STATE_STARTUP_WAIT              0x00                    // Init-Timeout beim Einschalten abwarten
+#define STATE_INITIALIZE                0x00                    // Initialisierung nach Einschalten der Motoren
-#define STATE_STARTUP_INIT              0x01                    // Initialisierung beim Einschalten
+#define STATE_SETUP                     0x01                    // Kalibrierung des Gas-Sticks
-#define STATE_BEGIN                     0x02                    // Anfangszustand nach Einschalten der Motoren
+#define STATE_SETUP1                    0x02                    //
-#define STATE_INITIALIZING              0x03                    // Initialisierungsphase
+#define STATE_BEGIN                     0x03                    // Startphase
-#define STATE_MANUAL                    0x04                    // Manuelle Kontrolle, Höhenregelung in Konfiguration deaktiviert
+#define STATE_BEGIN1                    0x04                    //
 #define STATE_INACTIVE                  0x05                    // Manuelle Kontrolle
 #define STATE_WAIT                      0x06                    // Warten auf Einschalten der Höhenregelung
 #define STATE_ACTIVATING                0x07                    // Aktivierung der Höhenregelung
 #define STATE_ACTIVE                    0x08                    // Höhenregelung ist aktiv
 static int      stickValue              = 0;                            // Aktueller Stick-Wert
 static int      lastStickValue          = 0;                            // Vorheriger Stick-Wert
 static int      pitchOffset             = 0;                            // Aktueller Grundgaswert in Neutralstellung
-static char     state                   = STATE_STARTUP_WAIT;           // Zustand
+static char     state;                                                  // Zustand
 static int      temp;
+char            pitchNeutralDiff        = 0;
 /* Wird verwendet, um das Umschalten auf automatische Höhenregelung
  * nach Erreichen der Neutralstellung zu verzögern.
  */
 static int      pitchNeutralTimer       = PITCH_NEUTRAL_TIMER;
+void pitch_neutral_init( void ) {
+        pitchNeutralDiff = eeprom_read_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ] );
+}
 /*
  * Berechnet den aktuellen Pitch-Wert für die Regelung
+ *
+ * Funktionsweise:
+ *
+ * 1. Vor Verwendung der Pitch-Steuerung muß ein Parameter für den Gas-Stick eingestellt werden. Hierzu
+ *    bewegt man den Roll-Stick ganz nach außen und startet die Motoren. Man befindet sich im Setup-Modus
+ *    und die Motoren laufen unabhängig vom Gas-Stick im Leerlauf. Im Display auf Seite 12 muß nun der
+ *    beste Wert für "StickDiff" gefunden werden. Den Wert ändern kann man mit dem Roll-Stick. Man betätigt
+ *    nun den Gas-Stick nach oben und läßt ihn los, so daß er in die Mittelstellung zurückspringt. Der
+ *    optimale Wert sorgt dafür, daß das gewollte Loslassen von einer normalen Bewegung unterschieden wird.
+ *    Jedesmal, wenn die SW das Loslassen als solches erkannt hat, ertönt ein Signal. Dieses darf jedoch
+ *    nicht ertönen, wenn man den Stick mit normaler Geschwindigkeit in die Mittelstellung zurückbewegt.
+ *    Wurde ein guter Wert eingestellt, verläßt man den Setup-Modus durch Ausschalten der Motoren. Der
+ *    ermittelte Wert wird permanent im EEProm gespeichert und muß nicht neu ermittelt werden.
+ *
- * Nach dem Einschalten der FC wird der aktuelle Gas-Stick-Wert gelesen und als Kalibrierungswert
+ * 2. Nach dem Einschalten der FC wird die aktuelle Gas-Stick-Position gemerkt und als Kalibrierungswert
- * für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite
+ *    für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite
- * keine Rolle.
+ *    keine Rolle.
  *
- * Nach Einschalten der Motoren geht der Stick in Neutralstellung. Diese Stick-Bewegung wird ignoriert
+ * 3. Nach Einschalten der Motoren geht der Stick in Neutralstellung. Diese Stick-Bewegung wird ignoriert
- * und die Motoren drehen mit dem eingestellten MinGas2. Ausgehend von der Neutralstellung wird nun
+ *    und die Motoren drehen mit dem eingestellten MinGas2. Ausgehend von der Neutralstellung wird nun
- * durch Bewegen des Sticks im oberen Bereich das Gas geregelt.
+ *    durch Bewegen des Sticks im oberen Bereich das Gas geregelt.
  *
- * Das erstmalige Aktivieren der automatischen Höhenregelung erfolgt durch Loslassen des Sticks im
+ * 4. Das erstmalige Aktivieren der automatischen Höhenregelung erfolgt durch Loslassen des Sticks im
- * Schwebeflug. Der zuvor aktuelle Stick-Wert wird als Wert in Neutralstellung übernommen und die
+ *    Schwebeflug. Der zuvor aktuelle Stick-Wert wird als Wert in Neutralstellung übernommen und die
- * automatische Höhenregelung sofort aktiviert.
+ *    automatische Höhenregelung sofort aktiviert. Wichtig hierfür ist die unter Punkt 1 vorgenommene
+ *    Parameterfindung. Stimmt dieser Wert nicht, führt das zur Wegnahme des Gases und somit zum Sinken.
  *
- * Sobald der Stick die Neutralstellung verläßt, wird die automatische Höhenregelung deaktiviert
+ * 5. Sobald der Stick die Neutralstellung verläßt, wird die automatische Höhenregelung deaktiviert
- * und der vorige Pitch-Wert als Wert der Neutralstellung übernommen. Der Pitch läßt sich nun
+ *    und der vorige Pitch-Wert als Wert der Neutralstellung übernommen. Der Pitch läßt sich nun
- * über den gesamten Stick-Bereich regeln.
+ *    über den gesamten Stick-Bereich regeln.
  *
- * Erreicht der Stick ein weiteres Mal die Neutralstellung, wird die automatische Höhenregelung
+ * 6. Erreicht der Stick ein weiteres Mal die Neutralstellung, wird die automatische Höhenregelung
- * wieder aktiviert, jetzt jedoch immer mit einer zeitlichen Verzögerung. Nur so ist ein
+ *    wieder aktiviert, jetzt jedoch immer mit einer zeitlichen Verzögerung. Nur so ist ein
- * ungestörtes manuelles Steuern möglich.
+ *    ungestörtes manuelles Steuern möglich.
  *
- * Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt.
+ * 7. Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt.
- * Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb
+ *    Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb
- * eines Wertes zu gelangen, der die Lageregelung außer Funktion setzt, wird ein zweiter Wert für
+ *    eines Wertes zu gelangen, der die Lageregelung außer Funktion setzt, wird ein zweiter Wert für
- * Minimalgas konfiguriert, der greift, sobald erstmalig die automatische Höhenregelung aktiviert wurde.
+ *    Minimalgas konfiguriert, der greift, sobald erstmalig die automatische Höhenregelung aktiviert wurde.
  */
 int pitch_neutral_value( void ) {
         int register pitchCount = 0;
-        // Sind die Motoren eingeschaltet?
+        // Sind die Motoren eingeschaltet? Ach ja, im Zustand STATE_SETUP sind die Motoren aus...
         if( MotorenEin ) {
                 // Vorigen Stick-Wert merken
                 lastStickValue = stickValue;
                 /* StickValue exponentiell angleichen, da ausgehend von der Neutralstellung
                  * nur jeweils die halbe Auflösung nach oben und unten zur Verfügung steht. Bei einer
                  * Multiplikation mit 2 ließe sich das Gas im Schwebebereich nicht fein genug einstellen. */
-                temp = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_stickoffset();
+                temp = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_initialStickValue();
                 if( temp > 0 ) {
                         temp = temp + ( ( temp * temp ) / 150 );
                 } else {
                         temp = temp - ( ( temp * temp ) / 150 );
+                }
-                // Original-Stick-Wert holen und glätten
-                stickValue = ( temp + 2 * lastStickValue ) / 3;
+                stickValue = temp;
                 /* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset
                  * zuzüglich dem Stick-Wert. */
                 pitchCount = stickValue + pitchOffset;
                 switch( state ) {
+                        /* Entscheidet über Flugbetrieb oder Setup-Betrieb. Für den Setup-Betrieb
+                         * muß beim Einschalten der Motoren gleichzeitig der Roll-Stick ganz
+                         * betätigt werden (Richtung ist egal).
+                         */
+                        case STATE_INITIALIZE:
+                                if( abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) > 70 ) {
+                                        state = STATE_SETUP;
+                                } else {
+                                        state = STATE_BEGIN;
+                                }
+                                break;
+                        case STATE_SETUP:
+                                if( stickValue < PARAM_PITCH_STICK_THRESHOLD &&
+                                    abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) < PARAM_PITCH_STICK_THRESHOLD ) {
+                                        state = STATE_SETUP1;
+                                }
+                                // Im Setup immer mit Leerlaufgas
+                                pitchCount = 0;
+                                break;
+                        case STATE_SETUP1:
+                                // Roll-Stick nach links
+                                if( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] < -20 ) {
+                                        if( pitchNeutralDiff < 20 ) {
+                                                pitchNeutralDiff++;
+                                                // Konfiguration dauerhaft speichern
+                                                eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ], pitchNeutralDiff );
+                                                // Signalisieren
+                                                beeptime = 500;
+                                                state    = STATE_SETUP;
+                                        }
+                                }
+                                // Roll-Stick nach rechts
+                                if( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] > 20 ) {
+                                        if( pitchNeutralDiff > 0 ) {
+                                                pitchNeutralDiff--;
+                                                // Konfiguration dauerhaft speichern
+                                                eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ], pitchNeutralDiff );
+                                                // Signalisieren
+                                                beeptime = 500;
+                                                state    = STATE_SETUP;
+                                        }
+                                }
+                                if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&
+                                    ( lastStickValue - stickValue >= pitchNeutralDiff ) ) {
+                                        state    = STATE_SETUP;
+                                        // Signalisieren
+                                        beeptime = 500;
+                                }
+                                // Im Setup immer mit Leerlaufgas
+                                pitchCount = 0;
+                                break;
                         case STATE_BEGIN:
                                 // Schnelles Bewegen aus dem oberen Bereich des Sticks in Neutralstellung
                                 if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&
-                                    ( lastStickValue - stickValue >= PARAM_PITCH_NEUTRAL_DIFF ) ) {
+                                    ( lastStickValue - stickValue >= pitchNeutralDiff ) ) {
                                         pitchOffset       = lastStickValue;
-                                        state             = STATE_INITIALIZING;
+                                        state             = STATE_BEGIN1;
                                         pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
+                                }
                                 break;
-                        case STATE_INITIALIZING:
+                        case STATE_BEGIN1:
                                 // Während der Initialisierung das Gas konstant halten
                                 pitchCount = pitchOffset;
                                 pitchNeutralTimer--;
                                 /* Läuft der Timer ab, bevor der Stick die Neutralstellung erreicht,
                                  * wird die Aktion nicht als "schnelles Bewegen in Neutralstellung"
                                  * gedeutet. */
                                 if( !pitchNeutralTimer ) {
                                         pitchOffset = 0;
                                         state       = STATE_BEGIN;
+                                }
                                 // Ist die Neutralstellung erreicht?
                                 if( abs( stickValue ) <= PARAM_PITCH_STICK_THRESHOLD ) {
-                                        // Ist die Höhenregelung aktiviert?
-                                        if( EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG ) {
-                                                state = STATE_ACTIVATING;
+                                        state = STATE_ACTIVATING;
-                                        } else {
-                                                state = STATE_MANUAL;
-                                        }
-                                }
-                                break;
-                        /* Wenn die Höhenregelung per Konfiguration deaktiviert ist, verbleibt
-                         * die Funktion in diesem Zustand. */
-                        case STATE_MANUAL:
-                                // Min2-Gas einstellen für Lageregelung bei Minimalgas
-                                if( pitchCount < PARAM_PITCH_MIN2 ) {
-                                        pitchCount = PARAM_PITCH_MIN2;
+                                }
                                 break;
                         /* Die Höhenregelung ist per Konfiguration aktiviert, jedoch befindet
                          * sich der Stick außerhalb des als Neutralstellung anerkannten
                          * Wertebereiches. Es wird manuell geregelt. */
                         case STATE_INACTIVE:
                                 // Ist ein Restart zulässig?
                                 if( PARAM_PITCH_RESTART_ENABLED ) {
                                         /* Wenn der Gashebel ganz unten steht, Timer für Reduzierung des Minimalgaswertes
                                          * starten. Hierfür wird die Variable pitchNeutralTimer verwendet. */
                                         if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 120 ) {
                                                 pitchNeutralTimer = PITCH_MIN2_TIMER;
                                         } else {
                                                 pitchNeutralTimer--;
                                                 /* Gashebel steht seit PITCH_MIN2_TIMER ganz unten; jetzt erfolgt die Initialisierung. */
                                                 if( !pitchNeutralTimer ) {
                                                         state       = STATE_BEGIN;
                                                         pitchOffset = 0;
                                                         // Signalisieren
                                                         beeptime    = 500;
+                                                }
+                                        }
+                                }
                                 // Min2-Gas einstellen für Lageregelung bei Minimalgas
                                 if( pitchCount < PARAM_PITCH_MIN2 ) {
                                         pitchCount = PARAM_PITCH_MIN2;
+                                }
                                 // Stick ist innerhalb der Neutralstellung
                                 if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD ) {
                                         // Timer neu setzen
                                         pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
                                         state             = STATE_WAIT;
+                                }
                                 break;
                         /* Der Stick ist in den für die Neutralstellung gültigen Wertebereich
                          * gelangt. Nun darf innerhalb einer bestimmten Zeit keine Stick-Bewegung
                          * erfolgen, um die automatische Höhenregelung zu aktivieren. */
                         case STATE_WAIT:
                                 /* Stick ist innerhalb der Neutralstellung und
                                    Stick-Differenzial ist < 2 */
                                 if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD &&
                                     lastStickValue == stickValue ) {
                                         pitchNeutralTimer--;
                                         if( !pitchNeutralTimer ) {
                                                 state = STATE_ACTIVATING;
+                                        }
                                 // Aktivierungskriterium nicht erfüllt, zurück in INACTIVE
                                 } else {
                                         state = STATE_INACTIVE;
+                                }
                                 break;
                         /* Die automatische Höhenregelung wird jetzt aktiviert. Der aktuelle
                          * Luftdruck wird gespeichert und notwendige Werte für den Regler
                          * werden initialisiert. */
                         case STATE_ACTIVATING:
                                 // Höhenregler starten
                                 altcon_start();
                                 state = STATE_ACTIVE;
                                 break;
                         /* Die automatische Höhenregelung ist aktiv. */
                         case STATE_ACTIVE:
                                 // Stick ist außerhalb der Neutralstellung
                                 if( abs( stickValue ) > PARAM_PITCH_STICK_THRESHOLD ) {
                                         pitchOffset -= altcon_avgerror() / 4;
                                         pitchCount   = stickValue + pitchOffset;
                                         // Höhenregler abschalten
                                         altcon_stop();
                                         state        = STATE_INACTIVE;
+                                }
                                 break;
+                }
         // Motoren sind aus
         } else {
                 /* Nach dem Einschalten der Motoren darf pitchOffset keinen hohen Wert haben,
                  * da der Kopter sonst sofort hochschießen würde.
                  */
                 pitchCount  = 0;
                 pitchOffset = 0;
                 stickValue  = 0;
-                state       = STATE_BEGIN;
+                state       = STATE_INITIALIZE;
+        }
         if( pitchOffset < 0 )
                 pitchOffset = 0;
         // Pitch-Wert darf nicht < 0 sein
         if( pitchCount < 0 ) {
                 pitchCount = 0;
+        }
         // pitchCount als Debug-Wert rausschreiben
         DebugOut.Analog[26] = stickValue;
         DebugOut.Analog[28] = pitchCount;
         DebugOut.Analog[29] = pitchOffset;
         return pitchCount;
+}

Subversion Repositories FlightCtrl

(root)/branches/thjac/V1_11/pitch_neutral.c @ 2650 - Rev 1134 → 1141