/* 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
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 temp
; // Temporäre Werte; wird mehrfach verwendet
unsigned char pitchHover
= 0; // Standgaswert
void pitch_md_init
( void ) {
pitchHover
= eeprom_read_byte
( &EEPromArray
[ EEPROM_ADR_PITCH_MD_HOVER
] );
}
/*
* 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
&& ( pitchHover
!= actualPitchCount
) ) {
// Aktuellen Pitch-Wert in Konfiguration übernehmen
pitchHover
= actualPitchCount
;
// Konfiguration dauerhaft speichern
eeprom_write_byte
( &EEPromArray
[ EEPROM_ADR_PITCH_MD_HOVER
], pitchHover
);
/*
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
= pitchHover
;
} else if( pitchOffset
== pitchHover
) {
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( !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
--;
if( actualPitchCount
< 0 )
actualPitchCount
= 0;
// pitchCount als Debug-Wert rausschreiben
DebugOut.
Analog[26] = stickValue
;
DebugOut.
Analog[28] = actualPitchCount
;
DebugOut.
Analog[29] = pitchOffset
;
return actualPitchCount
;
}