/* 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_SETUP_HOVER 0x03 // Konfiguration pitchHover
#define STATE_SETUP_STICK_DELTA 0x04 // Konfiguration pitchNeutralDiff
#define STATE_SETUP_STICK_DELTA1 0x05
#define STATE_BEGIN 0x06 // Anfangszustand für Flugbetrieb
#define STATE_BEGIN1 0x07 // Anfangszustand für Flugbetrieb
#define STATE_READY0 0x08 // Manuelle Kontrolle
#define STATE_READY 0x09 // Manuelle Kontrolle
#define STATE_READY1 0x0a
#define STATE_WAIT 0x0b // Warten auf Einschalten der Höhenregelung
#define STATE_ACTIVATING 0x0c // Aktivierung der Höhenregelung
#define STATE_ACTIVE 0x0d // Höhenregelung ist aktiv
#define STATE_RESTART 0x0e // Neustart
#define STATE_AUTOSTART0 0x0f // Autostartsequenz 0
#define STATE_AUTOSTART1 0x10 // Autostartsequenz 1
static int lastStickValue
= 0; // Vorheriger Stick-Wert
static int actualPitchCount
= 0; // Soll-Pitch-Wert
static int targetPitchCount
= 0; // Ist-Pitch-Wert
static int pitchOffset
; // Aktueller Grundgaswert in Neutralstellung
static char state
; // Zustand
static int timer
;
static int delay
= 0;
static int delayCounter
= 0;
static int peakPitchCount
;
unsigned char pitchHover
= 0; // Schwebegaswert
unsigned char pitchStand
= 0; // Standgaswert
unsigned char pitchNeutralDiff
= 0;
void pitch_md_init
( void ) {
pitchHover
= eeprom_read_byte
( &EEPromArray
[ EEPROM_ADR_PITCH_MD_HOVER
] );
pitchStand
= pitchHover
* PARAM_PITCH_HOVER_PERCENT
/ 100;
pitchNeutralDiff
= eeprom_read_byte
( &EEPromArray
[ EEPROM_ADR_PITCH_NEUTRAL_DIFF
] );
}
/*
* Berechnet den aktuellen Pitch-Wert für die Regelung
*
* Funktionsweise:
*
* 1. Einmalig muß man das Schwebegas einstellen. Schwebegas ist ein neuer Parameter.
* Dies tut man durch Betätigen des Roll-Sticks nach links (Vollausschlag) und
* Starten der Motoren. Nun gibt man soviel Gas (in der oberen Hälfte des Stick-Bereichs), bis der MK
* kurz über dem Boden schwebt (im Bodeneffekt). Man hält diese Stick-Position solange, bis ein
* Signal ertönt (ca. 1 Sekunde). Das Signal ist die Bestätigung für die Übernahme des aktuellen Gaswertes
* als Schwebegas. Der Wert wird dauerhaft gespeichert und kann im LCD-Display des MK-Tool im letzten Bild
* überprüft werden. Diese Kalibrierung muß mit vollem Akku und nach jeder Gewichtsänderung des MK einmal
* durchgeführt werden.
*
* 2. Für den Flug startet man die Motoren und läßt den Stick los. Die Motoren laufen im Leerlaufgas.
*
* 3. Man kann nun entweder mit oder ohne Unterstützung der Höhenregelung fliegen.
*
* Mit Unterstützung geht man wie folgt vor:
*
* Die Startsequenz wird durch Betätigung des Gasknüppels nach oben eingeleitet, bis ein akustisches Signal ertönt.
* Hierbei drehen die Motoren noch im Leerlauf. Erst nach Rückstellung des Sticks in die Mittelposition beschleunigen
* die Motoren langsam bis zum unter Punkt 1 eingestellten Schwebegas. Nun schaltet automatisch die Höhenregelung zu
* und der MK steigt bis auf eine im UserParameter eingestellte Höhe (normalerweise 1 Meter). In Mittelstellung ist
* nun die Höhenregelung aktiv, im oberen und unteren Stick-Bereich steigt oder sinkt der MK.
*
* Ohne Unterstützung geht man wie folgt vor:
*
* Man schaltet die Motoren durch Betätigung des Gasknüppels nach oben in das Standgas. Hier reicht ein kurzes Antippen
* des Sticks. Als Standgas wird 70% des unter Punkt 1 eingestellten Schwebegaswertes angenommen. Nun kann man
* manuell gesteuert durch Ausnutzung nur der oberen Hälfte des Stick-Bereichs fliegen. Dies entspricht der herkömmlichen
* Steuerung. Da nur der halbe Stick-Weg zur Verfügung steht, ist der Stick-Weg um die Mittelstellung herum höher aufgelöst.
* So kann das Gas trotzdem einigermaßen fein gesteuert werden.
*
* 4. Nach der Landung drückt man den Stick ganz nach unten, bis ein Signal ertönt. Jetzt laufen die Motoren
* wieder im Leerlaufgas. Man kann nun neu Starten (ab Punkt 3) oder die Motoren ausschalten.
*/
int pitch_md_value
( void ) {
int register rawStickValue
= PPM_in
[ EE_Parameter.
Kanalbelegung[ K_GAS
] ] - pitch_initialStickValue
();
int register stickValue
;
// Sind die Motoren eingeschaltet?
if( MotorenEin
) {
/* 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. */
if( rawStickValue
> 0 ) {
stickValue
= rawStickValue
+ ( ( (long) rawStickValue
* (long) rawStickValue
) / 150L );
} else {
stickValue
= rawStickValue
- ( ( (long) rawStickValue
* (long) rawStickValue
) / 150L );
}
/* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset
* zuzüglich dem Stick-Wert. Die folgenden Bedingungen erzeugen einen toten Bereich
* um die Mittelstellung herum, in dem sich targetPitchCount nicht ändert. So wird
* ein sanfterer Übergang vom höhengeregelten in den manuell gesteuerten Flug erreicht.
*/
if( stickValue
> PARAM_PITCH_STICK_THRESHOLD
)
targetPitchCount
= stickValue
- PARAM_PITCH_STICK_THRESHOLD
+ pitchOffset
;
else if( stickValue
< -PARAM_PITCH_STICK_THRESHOLD
)
targetPitchCount
= stickValue
+ PARAM_PITCH_STICK_THRESHOLD
+ pitchOffset
;
else
targetPitchCount
= 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. Es gilt:
*
* Roll-Stick links: Einstellen des Gas-Stick-Deltas
* Roll-Stick rechts: Einstellen des Schwebegases
*/
case STATE_INITIALIZE
:
if( PPM_in
[ EE_Parameter.
Kanalbelegung[ K_ROLL
] ] > 70 ) {
state
= STATE_SETUP_HOVER
;
// Signalisieren
beeptime
= 500;
} else if( PPM_in
[ EE_Parameter.
Kanalbelegung[ K_ROLL
] ] < -70 ) {
state
= STATE_SETUP_STICK_DELTA
;
// Signalisieren
beeptime
= 500;
} else {
state
= STATE_RESTART
;
}
break;
/* Erlaubt die Ermittlung des Parameters pitchHover. 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_HOVER
:
// Im Setup-Modus soll das Gas spontan reagieren
actualPitchCount
= targetPitchCount
;
if( rawStickValue
< 10 || abs( stickValue
- lastStickValue
) > 1 ) {
timer
= PARAM_TIMER_1S
;
}
/* 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
&& ( pitchHover
!= actualPitchCount
) ) {
// Aktuellen Pitch-Wert in Konfiguration übernehmen
pitchHover
= actualPitchCount
;
// Konfiguration dauerhaft speichern
eeprom_write_byte
( &EEPromArray
[ EEPROM_ADR_PITCH_MD_HOVER
], pitchHover
);
// Signalisieren
beeptime
= 500;
}
}
break;
/* Der Stick-Deltawert kann hier konfiguriert werden. Ein Betätigen des Roll-Sticks nach links
* erhöht den Deltawert um 1, ein Betätigen nach rechts veringert den Wert um 1. Der eingestellte
* Wert kann mit dem Gas-Stick getestet werden, indem dieser in den oberen Bereich bewegt und dann
* losgelassen wird. Ertönt dabei ein Signal, wurde die Bewegung erkannt. Es ist dann der richtige
* Wert gefunden, wenn das Signal beim Loslassen des Gas-Sticks ertönt, beim gesteuerten Bewegen nach
* unten jedoch nicht. Durch Ausschalten der Motoren (diese laufen während der Konfiguration nicht
* wirklich) wird der Konfigurationsmodus beendet.
*/
case STATE_SETUP_STICK_DELTA
:
if( stickValue
< PARAM_PITCH_STICK_THRESHOLD
&&
abs( PPM_in
[ EE_Parameter.
Kanalbelegung[ K_ROLL
] ] ) < PARAM_PITCH_STICK_THRESHOLD
) {
state
= STATE_SETUP_STICK_DELTA1
;
}
// Im Setup immer mit Leerlaufgas
targetPitchCount
= 0;
break;
case STATE_SETUP_STICK_DELTA1
:
// Roll-Stick nach links erhöht den Deltawert
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_STICK_DELTA
;
}
}
// Roll-Stick nach rechts verringert den Deltawert
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_STICK_DELTA
;
}
}
if( ( lastStickValue
> PARAM_PITCH_STICK_THRESHOLD
) &&
( lastStickValue
- stickValue
>= pitchNeutralDiff
) ) {
state
= STATE_SETUP_STICK_DELTA
;
// Signalisieren
beeptime
= 500;
}
// Im Setup immer mit Leerlaufgas
targetPitchCount
= 0;
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
:
// Stick ist oberhalb der Neutralstellung
if( rawStickValue
> PARAM_PITCH_STICK_THRESHOLD
) {
pitchOffset
= pitchStand
;
// Vollausschlag bedeutet: Einleitung der Autostartsequenz
if( rawStickValue
> 80 ) {
state
= STATE_AUTOSTART0
;
// Signalisieren
beeptime
= 500;
}
/* In diesem Zustand darf Standgas nicht überschritten werden, damit
* der MK nicht aus diesem Status heraus abhebt.
*/
if( targetPitchCount
> pitchStand
) {
targetPitchCount
= pitchStand
;
}
// Erst im Status weitergehen, wenn Stick nicht mehr oberhalb der Neutralstellung
} else if( pitchOffset
== pitchStand
) {
state
= STATE_BEGIN1
;
}
break;
// MK soll erst abheben, weil sonst die Höhenregelung am Boden schon greift
case STATE_BEGIN1
:
// Jetzt kann abgehoben werden
if( rawStickValue
> PARAM_PITCH_STICK_THRESHOLD
) {
// Begrenzung der Pitch-Beschleunigung nach dem Start
delay
= PARAM_PITCH_MD_DELAY1
;
state
= STATE_READY0
;
// Knüppel unterhalb der Neutralstellung bewirkt einen sofortigen Neustart
} else if( rawStickValue
< -PARAM_PITCH_STICK_THRESHOLD
) {
state
= STATE_RESTART
;
}
break;
/* Automatischer Start: Warten, bis Standgas erreicht ist, dann auf Schwebegas gehen und
* die Sollhöhe setzen.
*/
case STATE_AUTOSTART0
:
// Standgas darf in diesem Zustand nicht überschritten werden
if( targetPitchCount
> pitchStand
) {
targetPitchCount
= pitchStand
;
}
if( rawStickValue
< PARAM_PITCH_STICK_THRESHOLD
) {
// Knüppel unterhalb der Neutralstellung bewirkt einen sofortigen Neustart
if( rawStickValue
< -PARAM_PITCH_STICK_THRESHOLD
) {
state
= STATE_RESTART
;
}
// Standgas muß erreicht sein, bevor die Startsequenz weiterläuft
if( targetPitchCount
>= pitchStand
) {
// Jetzt auf Schwebegas übergehen (etwas mehr als eingestellt)
pitchOffset
= pitchHover
;
delay
= PARAM_PITCH_MD_DELAY_AUTOSTART
;
// Einstellen der zu erreichenden Flughöhe über Grund
altcon_lock
();
altcon_inc
( PARAM_PITCH_AUTOSTART_ALT
);
altcon_start
();
state
= STATE_AUTOSTART1
;
}
}
// Begrenzung der Pitch-Beschleunigung beim Autostart
delay
= PARAM_PITCH_MD_DELAY_AUTOSTART
;
break;
/* Wenn das Schwebegas erreicht wurde, kann davon ausgegangen werden, daß der MK
* nicht mehr am Boden steht.
*/
case STATE_AUTOSTART1
:
if( targetPitchCount
>= pitchHover
) {
delay
= PARAM_PITCH_MD_DELAY2
;
state
= STATE_ACTIVATING
;
// Signalisieren
beeptime
= 500;
}
// Eine Bewegung des Gasknüppels bricht die Autostartsequenz ab
if( abs( rawStickValue
) > PARAM_PITCH_STICK_THRESHOLD
) {
// Höhenregler deaktivieren
altcon_stop
();
pitchOffset
-= altcon_avgerror
() / 4;
targetPitchCount
= pitchOffset
;
state
= STATE_READY
;
}
break;
/* Die Motoren laufen jetzt mindestens mit Standgas. Der MK ist bereit zum Abheben.
*/
case STATE_READY0
:
/* 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_RESTART
;
}
}
/* Übergang in den höhengeregelten Flug durch:
* 1. Schnelles Bewegen des Sticks in Richtung Mittelstellung
* oder
* 2. langsames Bewegen des Sticks in Mittelstellung */
if( ( ( lastStickValue
> PARAM_PITCH_STICK_THRESHOLD
) &&
( lastStickValue
- stickValue
>= pitchNeutralDiff
) ) ||
( ( lastStickValue
< PARAM_PITCH_STICK_THRESHOLD
) &&
PARAM_PITCH_SOFT_ACTIVATING
) ) {
// Aktuelle Höhe festhalten (aktiviert noch nicht den Regler)
altcon_lock
();
// Höhe vorübergehend merken und beibehalten
peakPitchCount
= lastStickValue
;
state
= STATE_READY1
;
timer
= PITCH_NEUTRAL_TIMER
;
delay
= PARAM_PITCH_MD_DELAY2
;
}
break;
case STATE_READY
:
/* 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_RESTART
;
}
}
// Stick ist innerhalb der Neutralstellung
if( abs( rawStickValue
) < 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;
/* Hier wird die Zeit gemessen, in der der Stick nach dem Loslassen
* bis zur Neutralstellung benötigt. Bei Überschreiten der Zeit
* wird die Höhenregelung nicht aktiviert.
*/
case STATE_READY1
:
// Gas wird konstant gehalten
targetPitchCount
= peakPitchCount
+ pitchOffset
;
timer
--;
/* Läuft der Timer ab, bevor der Stick die Neutralstellung erreicht,
* wird die Aktion nicht als "schnelles Bewegen in Neutralstellung"
* gedeutet. */
if( !timer
) {
state
= STATE_READY0
;
}
// Ist die Neutralstellung erreicht?
if( rawStickValue
< PARAM_PITCH_STICK_THRESHOLD
) {
pitchOffset
= targetPitchCount
;
// Aktivierung des Höhenreglers mit der zuvor gemerkten Sollhöhe
altcon_start
();
// Signalisieren
beeptime
= 500;
state
= STATE_ACTIVE
;
}
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
&&
abs( stickValue
- lastStickValue
) < 2 ) {
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
:
/* Nach Aktivierung der Höhenregelung soll sich der Gaswert nicht mehr
* ändern. */
actualPitchCount
= targetPitchCount
;
pitchOffset
= targetPitchCount
;
// Aktivierung des Höhenreglers mit der zuvor gemerkten Sollhöhe
altcon_start
();
// Einschalten der Höhenregelung signalisieren
beeptime
= 500;
state
= STATE_ACTIVE
;
break;
/* Die automatische Höhenregelung ist aktiv. */
case STATE_ACTIVE
:
targetPitchCount
= pitchOffset
;
// Stick ist außerhalb der Neutralstellung
if( abs( rawStickValue
) > PARAM_PITCH_STICK_THRESHOLD
) {
// Höhenregler deaktivieren
altcon_stop
();
// Ausschalten der Höhenregelung signalisieren
beeptime
= 500;
pitchOffset
-= altcon_avgerror
() / 4;
targetPitchCount
= pitchOffset
;
state
= STATE_READY
;
}
break;
/* Neustart nach dem Flug oder vor Start. */
case STATE_RESTART
:
// Begrenzung der Pitch-Beschleunigung am Boden
delay
= PARAM_PITCH_MD_DELAY0
;
pitchOffset
= 0;
targetPitchCount
= 0;
// Folgezustand
state
= STATE_BEGIN
;
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
;
}
/* Durch die Sollwertvorgabe kann hier eine einstellbare Trägheit auf dem Pitch-Wert
* abgebildet werden. */
int pitchDelta
= targetPitchCount
- actualPitchCount
;
if( pitchDelta
> delay
)
pitchDelta
= delay
;
if( pitchDelta
< -delay
)
pitchDelta
= -delay
;
if( !delayCounter
) {
actualPitchCount
+= pitchDelta
;
delayCounter
= 5;
}
delayCounter
--;
if( actualPitchCount
< 0 )
actualPitchCount
= 0;
// Vorigen Stick-Wert merken
lastStickValue
= stickValue
;
DebugOut.
Analog[25] = pitchOffset
;
DebugOut.
Analog[26] = stickValue
;
return actualPitchCount
;
}