| 0,0 → 1,259 |
|---|
| /* 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_STARTUP_INIT 0x01 // Initialisierung beim Einschalten |
| #define STATE_BEGIN 0x02 // Anfangszustand nach Einschalten der Motoren |
| #define STATE_INITIALIZING 0x03 // Initialisierungsphase |
| #define STATE_MANUAL 0x04 // Manuelle Kontrolle, Höhenregelung in Konfiguration deaktiviert |
| #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 int temp; |
| |
| /* Wird verwendet, um das Umschalten auf automatische Höhenregelung |
| * nach Erreichen der Neutralstellung zu verzögern. |
| */ |
| static int pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
| |
| |
| /* |
| * Berechnet den aktuellen Pitch-Wert für die Regelung |
| * |
| * Nach dem Einschalten der FC wird der aktuelle Gas-Stick-Wert gelesen und als Kalibrierungswert |
| * für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite |
| * keine Rolle. |
| * |
| * 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 |
| * durch Bewegen des Sticks im oberen Bereich das Gas geregelt. |
| * |
| * 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 |
| * automatische Höhenregelung sofort aktiviert. |
| * |
| * 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 |
| * über den gesamten Stick-Bereich regeln. |
| * |
| * 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 |
| * ungestörtes manuelles Steuern möglich. |
| * |
| * 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 |
| * 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. |
| */ |
| int pitch_neutral_value( void ) { |
| |
| int register pitchCount = 0; |
| |
| // Sind die Motoren eingeschaltet? |
| 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(); |
| 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; |
| |
| /* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset |
| * zuzüglich dem Stick-Wert. */ |
| pitchCount = stickValue + pitchOffset; |
| |
| switch( state ) { |
| |
| 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 ) ) { |
| |
| pitchOffset = lastStickValue; |
| |
| state = STATE_INITIALIZING; |
| pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
| } |
| break; |
| |
| case STATE_INITIALIZING: |
| |
| // 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; |
| } 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; |
| } |
| |
| 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; |
| } |