/branches/thjac/V1_10/altcon.c |
---|
0,0 → 1,133 |
/* altcon.c |
* |
* Copyright 2009 Thomas Jachmann |
*/ |
#include "main.h" |
#include "altcon.h" |
#include "parameter.h" |
#include "fc.h" |
char enabled = 0; |
int pressureOffset = 0; |
int accZOffset = 0; |
int lastError = 0; |
int lastN = 0; // Zuletzt errechneter Fehlerwert |
int averageN = 0; |
long altIntegral = 0; |
int temp; // Temporäre Werte; wird mehrfach verwendet |
extern unsigned char Notlandung; // aus fc.c |
extern int airPressure; |
/* |
* Höhenregler initialisieren |
*/ |
void altcon_init( void ) { |
altcon_stop(); |
} |
/* |
* Speichert die aktuelle Höhe als Sollhöhe |
*/ |
void altcon_lock( void ) { |
pressureOffset = airPressure; |
} |
/* |
* Startet den Höhenregler |
*/ |
void altcon_start( void ) { |
accZOffset = Mess_Integral_Hoch / 128; |
lastError = 0; |
lastN = 0; |
averageN = 0; |
altIntegral = 0L; |
// Einschalten der Höhenregelung signalisieren |
beeptime = 500; |
} |
/* |
* Stoppt den Höhenregler |
*/ |
void altcon_stop( void ) { |
enabled = 0; |
// Ausschalten der Höhenregelung signalisieren |
beeptime = 500; |
} |
/* |
* Berechnet den Korrekturwert für die Höhenregelung |
*/ |
int altcon_error( void ) { |
int register n = 0; |
int register error; |
if( enabled && !Notlandung ) { |
// Fehlerwert für Regler ermitteln |
error = airPressure - pressureOffset; |
// Proportionalanteil |
n = ( PARAM_ALT_P * error ) / 4; // dividiert durch ( 16 / STICK_GAIN ) = 16 / 4 = 4 |
// Integralanteil |
altIntegral += ( PARAM_ALT_I * error ) / 4; |
// Integral begrenzen |
if( altIntegral > PARAM_ALT_INT_MAX ) |
altIntegral = PARAM_ALT_INT_MAX; |
else if( altIntegral < -PARAM_ALT_INT_MAX ) |
altIntegral = -PARAM_ALT_INT_MAX; |
n += altIntegral / 4000; |
// Differenzialanteil |
n += ( PARAM_ALT_D * ( error - lastError ) ) / 2; |
// ACC-Z-Integral zur Dämpfung einbeziehen |
temp = ( ( ( Mess_Integral_Hoch / 128 ) - accZOffset ) * (signed long) PARAM_ALT_ACC ) / 32; |
// Dämpfung limitieren |
if( temp > ( 70 * STICK_GAIN ) ) |
temp = 70 * STICK_GAIN; |
else if( temp < -( 70 * STICK_GAIN ) ) |
temp = -( 70 * STICK_GAIN ); |
n += temp; |
// Verstärkung des Fehlerwertes zur Anpassung des Gewichtes |
n = n * PARAM_ALT_GAIN / 10; |
int altMax = PARAM_ALT_MAX * STICK_GAIN; |
// Limitierung des Korrekturwertes |
if( n > altMax ) |
n = altMax; |
else if( n < -altMax ) |
n = -altMax; |
lastN = n; |
lastError = error; |
/* Berechnung einer exponentiellen Glättung für den neuen Gaswert bei Verlassen der |
* Höhenregelung. Dies soll ein zu heftiges Reagieren mindern. */ |
averageN = averageN + PARAM_ALT_EXP_SMOOTHING_FACTOR * ( n - averageN ) / 100; |
} |
DebugOut.Analog[30] = altIntegral / 4000; |
DebugOut.Analog[27] = n; |
return n; |
} |
/branches/thjac/V1_10/altcon.h |
---|
0,0 → 1,19 |
/* altcon.h |
* |
* Copyright 2009 Thomas Jachmann |
*/ |
#ifndef ALTCON_H |
#define ALTCON_H |
#define altcon_avgerror() averageN |
extern int pressureOffset; |
extern int averageN; |
extern void altcon_start( void ); |
extern void altcon_lock( void ); |
extern void altcon_stop( void ); |
extern int altcon_error( void ); |
#endif // ALTCON_H |
/branches/thjac/V1_10/fc.c |
---|
55,6 → 55,7 |
#include "main.h" |
#include "parameter.h" |
#include "pitch.h" |
#include "altcon.h" |
#include "eeprom.c" |
unsigned char h,m,s; |
730,11 → 731,7 |
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
// Gaswert übernehmen |
if( pitchNeutral() ) { |
StickGas = pitch(); |
} else { |
StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
} |
StickGas = pitch_value(); |
GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / (256 / STICK_GAIN ); |
IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN ); |
1232,7 → 1229,7 |
GasMischanteil *= STICK_GAIN; |
// Fehlerwert der Höhenregelung einmischen |
GasMischanteil -= altitudeController(); |
GasMischanteil -= altcon_error(); |
// Mindestens auf Minimalgas stellen |
if( GasMischanteil < MIN_GAS ) |
/branches/thjac/V1_10/flight.pnproj |
---|
1,0 → 0,0 |
<Project name="Flight-Ctrl"><File path="uart.h"></File><File path="main.c"></File><File path="main.h"></File><File path="makefile"></File><File path="uart.c"></File><File path="printf_P.h"></File><File path="printf_P.c"></File><File path="timer0.c"></File><File path="timer0.h"></File><File path="old_macros.h"></File><File path="twimaster.c"></File><File path="version.txt"></File><File path="twimaster.h"></File><File path="rc.c"></File><File path="rc.h"></File><File path="fc.h"></File><File path="menu.h"></File><File path="menu.c"></File><File path="_Settings.h"></File><File path="analog.c"></File><File path="analog.h"></File><File path="License.txt"></File><File path="eeprom.c"></File><File path="spi.h"></File><File path="spi.c"></File><File path="led.h"></File><File path="led.c"></File><File path="fc.c"></File><File path="parameter.h"></File><File path="pitch.c"></File><File path="pitch.h"></File><File path="gps.c"></File><File path="gps.h"></File></Project> |
<Project name="Flight-Ctrl"><File path="uart.h"></File><File path="main.c"></File><File path="main.h"></File><File path="makefile"></File><File path="uart.c"></File><File path="printf_P.h"></File><File path="printf_P.c"></File><File path="timer0.c"></File><File path="timer0.h"></File><File path="old_macros.h"></File><File path="twimaster.c"></File><File path="version.txt"></File><File path="twimaster.h"></File><File path="rc.c"></File><File path="rc.h"></File><File path="fc.h"></File><File path="menu.h"></File><File path="menu.c"></File><File path="_Settings.h"></File><File path="analog.c"></File><File path="analog.h"></File><File path="License.txt"></File><File path="eeprom.c"></File><File path="spi.h"></File><File path="spi.c"></File><File path="led.h"></File><File path="led.c"></File><File path="fc.c"></File><File path="parameter.h"></File><File path="pitch.c"></File><File path="pitch.h"></File><File path="gps.c"></File><File path="gps.h"></File><File path="altcon.h"></File><File path="altcon.c"></File><File path="pitch_neutral.h"></File><File path="pitch_neutral.c"></File><File path="pitch_md.h"></File><File path="pitch_md.c"></File></Project> |
/branches/thjac/V1_10/makefile |
---|
5,7 → 5,7 |
#------------------------------------------------------------------- |
VERSION_MAJOR = 1 |
VERSION_MINOR = 10 |
VERSION_PATCH = 3 |
VERSION_PATCH = 4 |
VERSION_SERIAL_MAJOR = 10 # Serial Protocol |
VERSION_SERIAL_MINOR = 0 # Serial Protocol |
86,7 → 86,7 |
########################################################################################################## |
# List C source files here. (C dependencies are automatically generated.) |
SRC = main.c uart.c printf_P.c timer0.c analog.c menu.c |
SRC += twimaster.c rc.c fc.c spi.c led.c pitch.c |
SRC += twimaster.c rc.c fc.c spi.c led.c pitch.c pitch_neutral.c pitch_md.c altcon.c |
SRC += gps.c |
########################################################################################################## |
/branches/thjac/V1_10/menu.c |
---|
5,7 → 5,7 |
// + see the File "License.txt" for further Informations |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include "main.h" |
#include "pitch.h" |
#include "altcon.h" |
unsigned int TestInt = 0; |
#define ARRAYGROESSE 10 |
/branches/thjac/V1_10/parameter.h |
---|
4,25 → 4,91 |
#ifndef PARAMETER_H |
#define PARAMETER_H |
#define PITCH_STICK_THRESHOLD 5 |
#define PARAM_TIMER_2S 100 // 2 Sekunden |
#define PARAM_PITCH_MIN2 EE_Parameter.UserParam2 // Minimalgas |
#define PITCH_ALT_THRESHOLD PITCH_MIN2 // Schwellwert für Höhenregelung |
#define PITCH_NEUTRAL_TIMER 25 |
#define PITCH_MIN2_TIMER 100 |
#define PITCH_NEUTRAL_DIFF EE_Parameter.UserParam1 |
#define PARAM_ALT_P Parameter_Hoehe_P |
#define PARAM_ALT_I EE_Parameter.UserParam3 |
#define PARAM_ALT_D Parameter_Luftdruck_D |
/****************************************************************************************** |
* Allgemeine Parameter |
*/ |
/* Dieser Parameter legt fest, ob in '+'-Formation (normal) oder 'X'-Formation |
* geflogen werden soll. Zur Umschaltung reicht das Setzen des Parameters. Ein |
* Umbau der FC oder Einstellungen am Sender sind nicht notwendig. |
*/ |
#define PARAM_X_FORMATION ( EE_Parameter.UserParam8 & 0x01 ) // 0=+, 1=X |
/* Steuert, ob beim bzw. vor dem Start der Motoren eine Kalibrierung erfolgen |
* soll. |
*/ |
#define PARAM_CAL_ON_START ( EE_Parameter.UserParam8 & 0x02 ) // 0=deaktiviert 1=aktiviert |
/* Die Motoren können über diesen Parameter aktiviert werden. Ein Wert von 0 |
* deaktiviert die Motoren und kann zum Testen verwendet werden. |
*/ |
#define PARAM_ENGINE_ENABLED ( EE_Parameter.UserParam8 & 0x20 ) // 0=deaktiviert 1=aktiviert |
/****************************************************************************************** |
* Parameter für Höhenregler |
*/ |
#define PARAM_ALT_P Parameter_Hoehe_P |
#define PARAM_ALT_I EE_Parameter.UserParam3 |
#define PARAM_ALT_D Parameter_Luftdruck_D |
#define PARAM_ALT_ACC Parameter_Hoehe_ACC_Wirkung |
#define PARAM_ALT_GAIN EE_Parameter.Hoehe_Verstaerkung |
#define PARAM_ALT_MAX EE_Parameter.MaxHoehe |
#define PARAM_ALT_INT_MAX 1000000L |
#define PARAM_ALT_EXP_SMOOTHING_FACTOR EE_Parameter.UserParam4 // Faktor für exp. Glättung |
#define PARAM_EXP_SMOOTHING_FACTOR EE_Parameter.UserParam4 // Faktor für exp. Glättung |
/****************************************************************************************** |
* Allgemeine Pitch-Steuerungsparameter |
*/ |
#define PARAM_PITCH_STICK_THRESHOLD 5 |
#define PARAM_PITCH_MIN2 EE_Parameter.UserParam2 // Minimalgas |
#define PITCH_ALT_THRESHOLD PITCH_MIN2 // Schwellwert für Höhenregelung |
/* Setzt den zu verwendenden Pitch-Modus |
*/ |
#define PARAM_PITCH_MODE ( ( EE_Parameter.UserParam8 & 0x0C ) >> 2 ) // Pitch-Mode 0-3 |
#define PARAM_PITCH_MODE_NORMAL 0x00 |
#define PARAM_PITCH_MODE_NEUTRAL 0x01 |
#define PARAM_PITCH_MODE_MD 0x02 |
/* Wenn gesetzt, wird nach ca. 2s andauernder Stick-Stellung auf Minimum |
* ein Reset der Pitch-Regelung durchgeführt, so daß der MK ohne Aus- und |
* Einschalten der Motoren wieder starten kann. Die Pitch-Regelung |
* schaltet auf Leerlaufgas zurück und der Stick kann losgelassen werden, |
* ohne das Gas gegeben wird. |
*/ |
#define PARAM_PITCH_RESTART_ENABLED ( EE_Parameter.UserParam8 & 0x10 ) // 0=deaktiviert 1=aktiviert |
/****************************************************************************************** |
* Parameter für Neutral-Pitch-Steuerung |
*/ |
#define PARAM_PITCH_NEUTRAL_DIFF EE_Parameter.UserParam1 // Stick-Loslass-Erkennung |
/****************************************************************************************** |
* Parameter für MD-Pitch-Steuerung |
*/ |
#define PARAM_PITCH_MD_HOVER EE_Parameter.UserParam1 // Standgaswert |
#define PARAM_PITCH_MD_DELAY0 5 // Begrenzung der Pitch-Beschleunigung am Boden |
#define PARAM_PITCH_MD_DELAY1 0 // Begrenzung der Pitch-Beschleunigung im Flug |
/****************************************************************************************** |
* Parameter für LED-Ansteuerung |
*/ |
/* Helligkeit J16 |
*/ |
#define PARAM_LED_BRIGHTNESS_J16 ( EE_Parameter.UserParam5 ) // 0-250, 25x=PotiX, Wert wird durch 25 geteilt |
31,7 → 97,6 |
*/ |
#define PARAM_LED_BRIGHTNESS_J17 ( EE_Parameter.UserParam6 ) // 0-250, 25x=PotiX, Wert wird durch 25 geteilt |
/* Wenn die Unterspannungswarnung aktiv wird, kann mit diesem Parameter eingestellt |
* werden, daß sich die Blinkfrequenz der LED's verdoppelt. |
*/ |
66,36 → 131,5 |
*/ |
#define PARAM_LED_STICK_ENABLED ( EE_Parameter.UserParam7 & 0x40 ) // 0=deaktiviert 1=aktiviert, wird durch Motorkopplung übersteuert |
/* Dieser Parameter legt fest, ob in '+'-Formation (normal) oder 'X'-Formation |
* geflogen werden soll. Zur Umschaltung reicht das Setzen des Parameters. Ein |
* Umbau der FC oder Einstellungen am Sender sind nicht notwendig. |
*/ |
#define PARAM_X_FORMATION ( EE_Parameter.UserParam8 & 0x01 ) // 0=+, 1=X |
/* Steuert, ob beim bzw. vor dem Start der Motoren eine Kalibrierung erfolgen |
* soll. |
*/ |
#define PARAM_CAL_ON_START ( EE_Parameter.UserParam8 & 0x02 ) // 0=deaktiviert 1=aktiviert |
/* Wenn gesetzt, dann wird mit neutralisiertem Pitch-Stick und automatisch |
* zugeschalteter Höhenregelung geflogen. Damit bei versehentlich falscher |
* Konfiguration kein Schaden entsteht, muß zusätzlich beim Einschalten |
* der FlightControl der Gas-Stick in Mittelstellung stehen. |
*/ |
#define PARAM_PITCH_NEUTRAL ( EE_Parameter.UserParam8 & 0x04 ) // 0=normal 1=Neutral-Pitch |
/* Wenn gesetzt, wird nach ca. 2s andauernder Stick-Stellung auf Minimum |
* ein Reset der Pitch-Regelung durchgeführt, so daß der MK ohne Aus- und |
* Einschalten der Motoren wieder starten kann. Die Pitch-Regelung |
* schaltet auf Leerlaufgas zurück und der Stick kann losgelassen werden, |
* ohne das Gas gegeben wird. |
*/ |
#define PARAM_PITCH_RESTART_ENABLED ( EE_Parameter.UserParam8 & 0x08 ) // 0=deaktiviert 1=aktiviert |
/* Die Motoren können über diesen Parameter aktiviert werden. Ein Wert von 0 |
* deaktiviert die Motoren und kann zum Testen verwendet werden. |
*/ |
#define PARAM_ENGINE_ENABLED ( EE_Parameter.UserParam8 & 0x10 ) // 0=deaktiviert 1=aktiviert |
#endif // PARAMETER_H |
/branches/thjac/V1_10/pitch.c |
---|
1,375 → 1,72 |
/* pitch.c |
* |
* Copyright 2009 Thomas Jachmann |
* |
* Pitch-Steuerung |
*/ |
#include "main.h" |
#include "parameter.h" |
#include "fc.h" |
#include "pitch_neutral.h" |
#include "pitch_md.h" |
#include "pitch.h" |
#define STATE_STARTUP_WAIT 0x00 // Init-Timeout beim Einschalten abwarten |
#define STATE_STARTUP_INIT 0x01 // Initialisierung beim Einschalten |
#define STATE_BEGIN 0x02 // Anfangszustand nach Einschalten der Motoren |
#define STATE_INITIALIZING 0x03 // Initialisierungsphase |
#define STATE_MANUAL 0x04 // Manuelle Kontrolle, Höhenregelung in Konfiguration deaktiviert |
#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 |
#define PARAM_INIT_TIMEOUT 25 |
// Zeiger auf den durch das Setting bestimmten Pitch-Steuerungsalgorithmus |
int (* pitch_value_ptr)( void ); |
// Prototyp |
int pitch_mk_value( void ); |
char initTimer = PARAM_INIT_TIMEOUT; |
int stickValue = 0; // Aktueller Stick-Wert |
int lastStickValue = 0; // Vorheriger Stick-Wert |
int zeroStickOffset = 0; // Offset für Stick-Kalibrierung |
int pitchOffset = 0; // Aktueller Grundgaswert in Neutralstellung |
char state = STATE_STARTUP_WAIT; // Zustand |
/* Wird verwendet, um das Umschalten auf automatische Höhenregelung |
* nach Erreichen der Neutralstellung zu verzögern. |
*/ |
int pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
// Variable zur Höhenregelung |
int pressureOffset = 0; |
int accZOffset = 0; |
int lastError = 0; |
int lastN = 0; // Zuletzt errechneter Fehlerwert |
int averageN = 0; |
long altIntegral = 0; |
int temp; // Temporäre Werte; wird mehrfach verwendet |
char pitchNeutralStartup = 1; // 1=Gas-Stick beim Einschalten in Mittelstellung |
extern unsigned char Notlandung; // aus fc.c |
/* |
* Berechnet den aktuellen Pitch-Wert für die Regelung |
* |
* Nach dem Einschalten der FC wird der aktuelle Gas-Stick-Wert gelesen und als Kalibrierungswert |
* für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite |
* keine Rolle. |
* |
* 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. |
* |
* 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. |
* |
* 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. |
* Führt die Initialisierung der Pitch-Steuerung durch. Diese Funktion |
* wird nach jeder Setting-Auswahl sowie nach jeder Setting-Änderung |
* aufgerufen. |
*/ |
int pitch( void ) { |
void pitch_init( void ) { |
int register pitchCount = 0; |
// Sind die Motoren eingeschaltet? |
if( MotorenEin ) { |
// FIXME Funktioniert noch nicht |
switch( PARAM_PITCH_MODE ) { |
case PARAM_PITCH_MODE_NEUTRAL: |
pitch_value_ptr = pitch_neutral_value; |
break; |
// Vorigen Stick-Wert merken |
lastStickValue = stickValue; |
case PARAM_PITCH_MODE_MD: |
pitch_value_ptr = pitch_md_value; |
break; |
/* 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 ] ] - zeroStickOffset; |
if( temp > 0 ) { |
temp = temp + ( ( temp * temp ) / 150 ); |
} else { |
temp = temp - ( ( temp * temp ) / 150 ); |
} |
// Original-Stick-Wert holen und glätten |
stickValue = ( temp + 2 * lastStickValue ) / 3; |
/* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset |
* zuzüglich dem Stick-Wert. */ |
pitchCount = stickValue + pitchOffset; |
switch( state ) { |
case STATE_BEGIN: |
// Schnelles Bewegen aus dem oberen Bereich des Sticks in Neutralstellung |
if( ( lastStickValue > PITCH_STICK_THRESHOLD ) && |
( lastStickValue - stickValue >= PITCH_NEUTRAL_DIFF ) ) { |
pitchOffset = lastStickValue; |
state = STATE_INITIALIZING; |
pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
} |
break; |
case STATE_INITIALIZING: |
// 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 ) <= PITCH_STICK_THRESHOLD ) { |
// Ist die Höhenregelung aktiviert? |
if( EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG ) { |
state = STATE_ACTIVATING; |
} else { |
state = STATE_MANUAL; |
} |
} |
break; |
/* Wenn die Höhenregelung per Konfiguration deaktiviert ist, verbleibt |
* die Funktion in diesem Zustand. */ |
case STATE_MANUAL: |
// Min2-Gas einstellen für Lageregelung bei Minimalgas |
if( pitchCount < PARAM_PITCH_MIN2 ) { |
pitchCount = PARAM_PITCH_MIN2; |
} |
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 ) < 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 ) < 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: |
// Die Referenzhöhe soll zu Beginn der Neutralstellung genommen werden |
pressureOffset = airPressure; |
accZOffset = Mess_Integral_Hoch / 128; |
lastError = 0; |
lastN = 0; |
averageN = 0; |
altIntegral = 0L; |
state = STATE_ACTIVE; |
// Einschalten der Höhenregelung signalisieren |
beeptime = 500; |
break; |
/* Die automatische Höhenregelung ist aktiv. */ |
case STATE_ACTIVE: |
// Stick ist außerhalb der Neutralstellung |
if( abs( stickValue ) > PITCH_STICK_THRESHOLD ) { |
pitchOffset -= averageN / 4; |
pitchCount = stickValue + pitchOffset; |
lastN = 0; |
state = STATE_INACTIVE; |
// Abschaltung der Höhenregelung signalisieren |
beeptime = 500; |
} |
break; |
} |
// Motoren sind aus |
} else { |
default: |
pitch_value_ptr = pitch_mk_value; |
} |
switch( state ) { |
case STATE_STARTUP_WAIT: |
// Hier können weitere Initialisierungen folgen |
} |
if( !initTimer-- ) { |
state = STATE_STARTUP_INIT; |
} |
break; |
case STATE_STARTUP_INIT: |
/* Lädt den beim Einschalten der FC anliegenden Stickwert als |
* Offset für die Kalibrierung des Gas-Sticks. */ |
zeroStickOffset = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ]; |
int pitch_value( void ) { |
switch( PARAM_PITCH_MODE ) { |
case PARAM_PITCH_MODE_NEUTRAL: |
return pitch_neutral_value(); |
if( zeroStickOffset < -75 ) |
pitchNeutralStartup = 0; |
case PARAM_PITCH_MODE_MD: |
return pitch_md_value(); |
// Die Einschaltphase ist jetzt beendet |
state = STATE_BEGIN; |
break; |
default: |
/* 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_BEGIN; |
} |
default: |
return pitch_mk_value(); |
} |
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; |
} |
/* |
* Berechnet den Korrekturwert für die Höhenregelung |
* Führt eine Pitch-Berechnung aus, die der Original-SW entspricht. |
*/ |
int altitudeController( void ) { |
int register n = 0; |
int register error; |
if( ( state == STATE_ACTIVE ) && !Notlandung ) { |
// Fehlerwert für Regler ermitteln |
error = airPressure - pressureOffset; |
int pitch_mk_value( void ) { |
register int stickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ]; |
register int pitchCount = stickValue + 120; |
// Proportionalanteil |
n = ( PARAM_ALT_P * error ) / 4; // dividiert durch ( 16 / STICK_GAIN ) = 16 / 4 = 4 |
DebugOut.Analog[26] = stickValue; |
DebugOut.Analog[28] = pitchCount; |
// Integralanteil |
altIntegral += ( PARAM_ALT_I * error ) / 4; |
// Integral begrenzen |
if( altIntegral > PARAM_ALT_INT_MAX ) |
altIntegral = PARAM_ALT_INT_MAX; |
else if( altIntegral < -PARAM_ALT_INT_MAX ) |
altIntegral = -PARAM_ALT_INT_MAX; |
n += altIntegral / 4000; |
// Differenzialanteil |
n += ( PARAM_ALT_D * ( error - lastError ) ) / 2; |
// ACC-Z-Integral zur Dämpfung einbeziehen |
temp = ( ( ( Mess_Integral_Hoch / 128 ) - accZOffset ) * (signed long) PARAM_ALT_ACC ) / 32; |
// Dämpfung limitieren |
if( temp > ( 70 * STICK_GAIN ) ) |
temp = 70 * STICK_GAIN; |
else if( temp < -( 70 * STICK_GAIN ) ) |
temp = -( 70 * STICK_GAIN ); |
n += temp; |
// Verstärkung des Fehlerwertes zur Anpassung des Gewichtes |
n = n * PARAM_ALT_GAIN / 10; |
int altMax = PARAM_ALT_MAX * STICK_GAIN; |
// Limitierung des Korrekturwertes |
if( n > altMax ) |
n = altMax; |
else if( n < -altMax ) |
n = -altMax; |
lastN = n; |
lastError = error; |
/* Berechnung einer exponentiellen Glättung für den neuen Gaswert bei Verlassen der |
* Höhenregelung. Dies soll ein zu heftiges Reagieren mindern. */ |
averageN = averageN + PARAM_EXP_SMOOTHING_FACTOR * ( n - averageN ) / 100; |
} |
DebugOut.Analog[30] = altIntegral / 4000; |
DebugOut.Analog[27] = n; |
return n; |
return pitchCount; |
} |
/branches/thjac/V1_10/pitch.h |
---|
1,15 → 1,18 |
/* pitch.h |
* |
* copyright 2009 Thoams Jachmann |
*/ |
#ifndef _PITCH_H |
#define _PITCH_H |
extern int pressureOffset; |
extern char pitchNeutralStartup; |
extern int (* pitch_value_ptr)( void ); |
#define pitchNeutral() ( pitchNeutralStartup | PARAM_PITCH_NEUTRAL ) |
// TODO Soll den Stick-Wert der Mittelstellung liefern |
#define pitch_stickoffset() 0 |
int pitch( void ); |
int altitudeController( void ); |
extern int pitch_value( void ); |
#endif |
// #define pitch_value() ( pitch_value_ptr != 0 ? pitch_value_ptr() : 0 ) |
#endif // PITCH_H |
/branches/thjac/V1_10/pitch_md.c |
---|
0,0 → 1,308 |
/* 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 |
int stickValue = 0; // Aktueller Stick-Wert |
int lastStickValue = 0; // Vorheriger Stick-Wert |
int actualPitchCount; // Soll-Pitch-Wert |
int targetPitchCount; // Ist-Pitch-Wert |
int pitchOffset; // Aktueller Grundgaswert in Neutralstellung |
char state; // Zustand |
int timer; |
int delay = 0; |
int delayCounter = 0; |
int temp; // Temporäre Werte; wird mehrfach verwendet |
/* |
* 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 && ( PARAM_PITCH_MD_HOVER != actualPitchCount ) ) { |
// Aktuellen Pitch-Wert in Konfiguration übernehmen |
PARAM_PITCH_MD_HOVER = actualPitchCount; |
// Konfiguration dauerhaft speichern |
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 = PARAM_PITCH_MD_HOVER; |
} else if( pitchOffset == PARAM_PITCH_MD_HOVER ) { |
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( pitchOffset < 0 ) |
pitchOffset = 0; |
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--; |
// Pitch-Wert darf nicht < 0 sein |
if( actualPitchCount < 0 ) { |
actualPitchCount = 0; |
} |
// pitchCount als Debug-Wert rausschreiben |
DebugOut.Analog[26] = stickValue; |
DebugOut.Analog[28] = actualPitchCount; |
DebugOut.Analog[29] = pitchOffset; |
return actualPitchCount; |
} |
/branches/thjac/V1_10/pitch_md.h |
---|
0,0 → 1,12 |
/* pitch_md.h |
* |
* Copyright 2009 Thoams Jachmann |
*/ |
#ifndef PITCH_MD_H |
#define PITCH_MD_H |
extern void pitch_md_init( void ); |
extern int pitch_md_value( void ); |
#endif // PITCH_MD_H |
/branches/thjac/V1_10/pitch_neutral.c |
---|
0,0 → 1,259 |
/* 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_STARTUP_WAIT 0x00 // Init-Timeout beim Einschalten abwarten |
#define STATE_STARTUP_INIT 0x01 // Initialisierung beim Einschalten |
#define STATE_BEGIN 0x02 // Anfangszustand nach Einschalten der Motoren |
#define STATE_INITIALIZING 0x03 // Initialisierungsphase |
#define STATE_MANUAL 0x04 // Manuelle Kontrolle, Höhenregelung in Konfiguration deaktiviert |
#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 = STATE_STARTUP_WAIT; // Zustand |
static int temp; |
/* Wird verwendet, um das Umschalten auf automatische Höhenregelung |
* nach Erreichen der Neutralstellung zu verzögern. |
*/ |
static int pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
/* |
* Berechnet den aktuellen Pitch-Wert für die Regelung |
* |
* Nach dem Einschalten der FC wird der aktuelle Gas-Stick-Wert gelesen und als Kalibrierungswert |
* für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite |
* keine Rolle. |
* |
* 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. |
* |
* 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. |
* |
* 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_neutral_value( void ) { |
int register pitchCount = 0; |
// 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 = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_stickoffset(); |
if( temp > 0 ) { |
temp = temp + ( ( temp * temp ) / 150 ); |
} else { |
temp = temp - ( ( temp * temp ) / 150 ); |
} |
// Original-Stick-Wert holen und glätten |
stickValue = ( temp + 2 * lastStickValue ) / 3; |
/* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset |
* zuzüglich dem Stick-Wert. */ |
pitchCount = stickValue + pitchOffset; |
switch( state ) { |
case STATE_BEGIN: |
// Schnelles Bewegen aus dem oberen Bereich des Sticks in Neutralstellung |
if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) && |
( lastStickValue - stickValue >= PARAM_PITCH_NEUTRAL_DIFF ) ) { |
pitchOffset = lastStickValue; |
state = STATE_INITIALIZING; |
pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
} |
break; |
case STATE_INITIALIZING: |
// 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 ) { |
// Ist die Höhenregelung aktiviert? |
if( EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG ) { |
state = STATE_ACTIVATING; |
} else { |
state = STATE_MANUAL; |
} |
} |
break; |
/* Wenn die Höhenregelung per Konfiguration deaktiviert ist, verbleibt |
* die Funktion in diesem Zustand. */ |
case STATE_MANUAL: |
// Min2-Gas einstellen für Lageregelung bei Minimalgas |
if( pitchCount < PARAM_PITCH_MIN2 ) { |
pitchCount = PARAM_PITCH_MIN2; |
} |
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_BEGIN; |
} |
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; |
} |
/branches/thjac/V1_10/pitch_neutral.h |
---|
0,0 → 1,12 |
/* pitch_neutral.h |
* |
* copyright 2009 Thoams Jachmann |
*/ |
#ifndef _PITCH_NEUTRAL_H |
#define _PITCH_NEUTRAL_H |
extern void pitch_neutral_init( void ); |
extern int pitch_neutral_value( void ); |
#endif // PITCH_NEUTRAL_H |