/* 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
/*
* 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 Beätigen des Roll-Sticks in eine beliebige Richtung (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
* Piepen 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 Piepen 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. */
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
);
// 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
) {
// Ab jetzt gilt eine höhere Pitch-Beschleunigung
delay
= PARAM_PITCH_MD_DELAY2
;
// 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
;
}
/* 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;
if( !delayCounter
) {
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
;
}
void pitch_md_init
( void ) {
printf("\r\npitch_mk_init()");
pitchHover
= eeprom_read_byte
( &EEPromArray
[ EEPROM_ADR_PITCH_MD_HOVER
] );
pitchValueFP
= pitch_md_value
;
}