Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1121 → Rev 1122

/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