| 0,0 → 1,308 |
|---|
| /* pitch_md.c |
| * |
| * Copyright 2009 Thomas Jachmann |
| * |
| * Die in dieser Quelldatei enthaltenen Algorithmen ermöglichen eine MD-ähnliche Pitch-Steuerung |
| * für den MK. |
| */ |
| |
| #include "main.h" |
| #include "parameter.h" |
| #include "fc.h" |
| #include "pitch.h" |
| #include "altcon.h" |
| |
| |
| #define STATE_INITIALIZE 0x01 // Anfangszustand nach Einschalten der Motoren |
| #define STATE_SETUP 0x02 // Ermittlung von PARAM_PITCH_MD_HOVER |
| #define STATE_BEGIN 0x03 // Anfangszustand für Flugbetrieb |
| #define STATE_BEGIN1 0x04 // Anfangszustand für Flugbetrieb |
| #define STATE_READY 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 |
| |
| |
| int stickValue = 0; // Aktueller Stick-Wert |
| int lastStickValue = 0; // Vorheriger Stick-Wert |
| int actualPitchCount; // Soll-Pitch-Wert |
| int targetPitchCount; // Ist-Pitch-Wert |
| int pitchOffset; // Aktueller Grundgaswert in Neutralstellung |
| char state; // Zustand |
| int timer; |
| int delay = 0; |
| int delayCounter = 0; |
| int temp; // Temporäre Werte; wird mehrfach verwendet |
| |
| |
| /* |
| * Berechnet den aktuellen Pitch-Wert für die Regelung |
| * |
| * Nachdem die Motoren eingeschaltet sind, wird der Pitch-Stick losgelassen und damit in Mittelstellung |
| * gebracht. Die Motoren laufen zu dieser Zeit im Leerlaufgas. Vor dem Abheben müssen die Motoren in |
| * das Standgas gebracht werden. Dies geschieht durch minimales Gasgeben. Durch weiteres Gasgeben und |
| * nehmen kann abgehoben und geflogen werden. |
| * |
| * Erreicht der Stick während des Fluges die Neutralstellung und verbleibt dort für ca. 1 Sekunde ohne |
| * bewegt zu werden, aktiviert sich die Höhenregelung und hält den MK auf der aktuellen Höhe. |
| * |
| * 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_md_value( void ) { |
| |
| int register rawStickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_stickoffset(); |
| |
| // 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 = rawStickValue; |
| if( temp > 0 ) { |
| temp = temp + ( ( temp * temp ) / 150 ); |
| } else { |
| temp = temp - ( ( temp * temp ) / 150 ); |
| } |
| |
| stickValue = temp; |
| |
| /* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset |
| * zuzüglich dem Stick-Wert. */ |
| targetPitchCount = 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; |
| |
| /* Erlaubt die Ermittlung des Parameters PARAM_PITCH_MD_HOVER. Hierzu wird soviel Gas |
| * gegeben, bis der MK kurz vor dem Abheben ist, jedoch noch stabil steht. Um den |
| * Gaswert dauerhaft zu speichern, muß der Stick an der Position verweilen, bis |
| * der Summer die Übernahme akustisch quittiert. Dann müssen die Motoren wieder |
| * ausgeschaltet werden, da dieser Modus nicht für den Flug vorgesehen ist. |
| */ |
| case STATE_SETUP: |
| |
| // Im Setup-Modus soll das Gas nicht träge reagieren |
| actualPitchCount = targetPitchCount; |
| |
| if( rawStickValue < 20 || ( stickValue - lastStickValue ) ) { |
| timer = PARAM_TIMER_2S; |
| } |
| |
| /* Der Stick befindet sich eindeutig in der oberen Hälfte und wurde |
| * seit dem letzten Zyklus nicht bewegt. */ |
| else { |
| timer--; |
| |
| /* Die Verweilzeit ist abgelaufen und der aktuelle Pitch-Wert |
| * entspricht nicht dem bereits gespeicherten Wert. */ |
| if( !timer && ( PARAM_PITCH_MD_HOVER != actualPitchCount ) ) { |
| |
| // Aktuellen Pitch-Wert in Konfiguration übernehmen |
| PARAM_PITCH_MD_HOVER = actualPitchCount; |
| |
| // Konfiguration dauerhaft speichern |
| WriteParameterSet( |
| GetActiveParamSetNumber(), |
| (unsigned char *) &EE_Parameter.Kanalbelegung[0], |
| STRUCT_PARAM_LAENGE ); |
| |
| // Signalisieren |
| beeptime = 500; |
| } |
| } |
| break; |
| |
| /* In diesem Zustand steht der MK am Boden und die Motoren laufen auf Leerlaufgas. Ein |
| * kurzes Auslenken des Sticks nach oben schaltet in das Standgas über. |
| */ |
| case STATE_BEGIN: |
| |
| // Begrenzung der Pitch-Beschleunigung am Boden |
| delay = PARAM_PITCH_MD_DELAY0; |
| |
| if( rawStickValue > PARAM_PITCH_STICK_THRESHOLD ) { |
| pitchOffset = PARAM_PITCH_MD_HOVER; |
| } else if( pitchOffset == PARAM_PITCH_MD_HOVER ) { |
| state = STATE_BEGIN1; |
| } |
| break; |
| |
| // MK soll erst abheben, weil sonst die Höhenregelung am Boden schon greift |
| case STATE_BEGIN1: |
| |
| if( abs( rawStickValue ) > 10 ) { |
| |
| // Begrenzung der Pitch-Beschleunigung im Flug |
| delay = PARAM_PITCH_MD_DELAY1; |
| |
| if( rawStickValue > 0 ) { |
| state = STATE_READY; |
| } |
| } |
| break; |
| |
| /* Die Motoren laufen jetzt mindestens mit Standgas. Der MK ist bereit zum Abheben. |
| * Das Minimalgas kann jetzt nicht mehr unterschritten werden. |
| */ |
| case STATE_READY: |
| |
| // Ist ein Restart zulässig? |
| if( PARAM_PITCH_RESTART_ENABLED ) { |
| |
| /* Wenn der Gashebel ganz unten steht, Timer für Restart der Pitch-Regelung |
| * starten. Hierfür wird die Variable pitchNeutralTimer verwendet. */ |
| if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 120 ) { |
| timer = PITCH_MIN2_TIMER; |
| } else { |
| timer--; |
| |
| /* Gashebel steht seit PITCH_MIN2_TIMER ganz unten; jetzt erfolgt die Initialisierung. */ |
| if( !timer ) { |
| state = STATE_BEGIN; |
| pitchOffset = 0; |
| targetPitchCount = 0; |
| actualPitchCount = 0; |
| |
| // Signalisieren |
| beeptime = 500; |
| } |
| } |
| } |
| |
| // Min2-Gas einstellen für Lageregelung bei Minimalgas |
| if( targetPitchCount < PARAM_PITCH_MIN2 ) { |
| targetPitchCount = PARAM_PITCH_MIN2; |
| } |
| |
| // Stick ist innerhalb der Neutralstellung |
| if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD ) { |
| |
| // Aktuelle Höhe festhalten (aktiviert noch nicht den Regler) |
| altcon_lock(); |
| |
| // Timer neu setzen |
| timer = 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( rawStickValue ) < PARAM_PITCH_STICK_THRESHOLD && |
| lastStickValue == stickValue ) { |
| |
| timer--; |
| |
| if( !timer ) { |
| state = STATE_ACTIVATING; |
| } |
| |
| // Aktivierungskriterium nicht erfüllt, zurück in INACTIVE |
| } else { |
| state = STATE_READY; |
| } |
| break; |
| |
| /* Die automatische Höhenregelung wird jetzt aktiviert. |
| */ |
| case STATE_ACTIVATING: |
| |
| // Aktivierung des Höhenreglers mit der zuvor gemerkten Sollhöhe |
| altcon_start(); |
| |
| state = STATE_ACTIVE; |
| break; |
| |
| /* Die automatische Höhenregelung ist aktiv. */ |
| case STATE_ACTIVE: |
| |
| // Stick ist außerhalb der Neutralstellung |
| if( abs( rawStickValue ) > PARAM_PITCH_STICK_THRESHOLD ) { |
| |
| // Höhenregler deaktivieren |
| altcon_stop(); |
| |
| pitchOffset -= altcon_avgerror() / 4; |
| targetPitchCount = stickValue + pitchOffset; |
| state = STATE_READY; |
| } |
| break; |
| } |
| |
| // Motoren sind aus |
| } else { |
| |
| /* Nach dem Einschalten der Motoren wird pitchOffset auf PARAM_PITCH_OVER gesetzt. |
| */ |
| actualPitchCount = 0; |
| targetPitchCount = 0; |
| pitchOffset = 0; |
| stickValue = 0; |
| state = STATE_INITIALIZE; |
| } |
| |
| if( pitchOffset < 0 ) |
| pitchOffset = 0; |
| |
| if( !delayCounter ) { |
| |
| /* Durch die Sollwertvorgabe kann hier eine einstellbare Trägheit auf dem Pitch-Wert |
| * abgebildet werden. */ |
| int pitchDelta = targetPitchCount - actualPitchCount; |
| |
| if( pitchDelta > 3 ) |
| pitchDelta = 3; |
| if( pitchDelta < -3 ) |
| pitchDelta = -3; |
| |
| actualPitchCount += pitchDelta; |
| |
| delayCounter = delay + 1; |
| } |
| |
| delayCounter--; |
| |
| // Pitch-Wert darf nicht < 0 sein |
| if( actualPitchCount < 0 ) { |
| actualPitchCount = 0; |
| } |
| |
| // pitchCount als Debug-Wert rausschreiben |
| DebugOut.Analog[26] = stickValue; |
| DebugOut.Analog[28] = actualPitchCount; |
| DebugOut.Analog[29] = pitchOffset; |
| |
| return actualPitchCount; |
| } |