/* 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_INITIALIZE 0x00 // Initialisierung nach Einschalten der Motoren
#define STATE_SETUP 0x01 // Kalibrierung des Gas-Sticks
#define STATE_SETUP1 0x02 //
#define STATE_BEGIN 0x03 // Startphase
#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
; // 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.
*
* 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
* keine Rolle.
*
* 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
* durch Bewegen des Sticks im oberen Bereich das Gas geregelt.
*
* 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
* 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.
*
* 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
* über den gesamten Stick-Bereich regeln.
*
* 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
* ungestörtes manuelles Steuern möglich.
*
* 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
* 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? 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_initialStickValue
();
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. */
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
>= pitchNeutralDiff
) ) {
pitchOffset
= lastStickValue
;
state
= STATE_BEGIN1
;
pitchNeutralTimer
= PITCH_NEUTRAL_TIMER
;
}
break;
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
) {
state
= STATE_ACTIVATING
;
}
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_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
;
}