Blame |
Last modification |
View Log
| RSS feed
/* 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
;
}