/* 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
static int stickValue
= 0; // Aktueller Stick-Wert
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; // Standgaswert
unsigned char pitchNeutralDiffMD
= 0; // Suffix MD später entfernen
void pitch_md_init
( void ) {
pitchHover
= eeprom_read_byte
( &EEPromArray
[ EEPROM_ADR_PITCH_MD_HOVER
] );
pitchNeutralDiffMD
= eeprom_read_byte
( &EEPromArray
[ EEPROM_ADR_PITCH_NEUTRAL_DIFF
] );
}
/*
* Berechnet den aktuellen Pitch-Wert für die Regelung
*
* Funktionsweise:
*
* 1. Einmalig muß man das Standgas kalibrieren. Standgas 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) wie
* der MK gerade noch stabil auf dem Boden steht. Man hält diese Stick-Position solange, bis ein
* Signal ertönt. Damit wurde der Gaswert als Standgasparameter übernommen und im EEProm dauerhaft gespeichert.
* 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. Nun schaltet man auf Standgas durch kurzes Antippen des Sticks nach oben. Die Motoren beschleunigen langsam
* bis zum Erreichen des zuvor kalibriertes Standgaswertes.
*
* 4. Nun hebt man wie gewohnt ab. Hierzu hat man den oberen Stick-Bereich zur Verfügung. Da der Grundgaswert
* bei Mittelstellung jetzt knapp unterhalb des Schwebegaswertes liegt, kann man im Flug einfach die Höhe halten,
* indem man den Stick in Mittelstellung bewegt. Nach kurzer Zeit (quittiert mit einem Piep) wird die automatische
* Höhenregelung aktiviert und solange gehalten, bis man den Stick wieder bewegt.
*
* 5. 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
();
// 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. */
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. */
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. 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 nicht träge reagieren
actualPitchCount
= targetPitchCount
;
if( rawStickValue
< 10 || abs( stickValue
- lastStickValue
) > 1 ) {
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
&& ( 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( pitchNeutralDiffMD
< 20 ) {
pitchNeutralDiffMD
++;
// Konfiguration dauerhaft speichern
eeprom_write_byte
( &EEPromArray
[ EEPROM_ADR_PITCH_NEUTRAL_DIFF
], pitchNeutralDiffMD
);
// 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( pitchNeutralDiffMD
> 0 ) {
pitchNeutralDiffMD
--;
// Konfiguration dauerhaft speichern
eeprom_write_byte
( &EEPromArray
[ EEPROM_ADR_PITCH_NEUTRAL_DIFF
], pitchNeutralDiffMD
);
// Signalisieren
beeptime
= 500;
state
= STATE_SETUP_STICK_DELTA
;
}
}
if( ( lastStickValue
> PARAM_PITCH_STICK_THRESHOLD
) &&
( lastStickValue
- stickValue
>= pitchNeutralDiffMD
) ) {
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
= pitchHover
;
/* In diesem Zustand darf Schwebegas nicht überschritten werden, damit
* der MK nicht aus diesem Status heraus abhebt.
*/
if( targetPitchCount
> pitchHover
) {
targetPitchCount
= pitchHover
;
}
// Erst im Status weitergehen, wenn Stick nicht mehr oberhalb der Neutralstellung
} else if( pitchOffset
== pitchHover
) {
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;
/* 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
>= pitchNeutralDiffMD
) ) ||
( ( 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
();
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
();
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
();
pitchOffset
-= altcon_avgerror
() / 4;
targetPitchCount
= stickValue
+ 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;
DebugOut.
Analog[25] = pitchOffset
;
DebugOut.
Analog[26] = stickValue
;
return actualPitchCount
;
}