WebSVN - FlightCtrl - Diff - Rev 1141 and 1157 - /branches/thjac/V1_11/pitch


/* 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_INITIALIZE                0x00                    // Initialisierung nach Einschalten der Motoren
#define STATE_SETUP                     0x01                    // Kalibrierung des Gas-Sticks
#define STATE_SETUP1                    0x02                    //
#define STATE_BEGIN                     0x03                    // Startphase
#define STATE_BEGIN1                    0x04                    //
#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;                                                  // Zustand
static int      temp;
 
char            pitchNeutralDiff        = 0;
 
 
/* 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
 *
 * Funktionsweise:
 *
 * 1. Vor Verwendung der Pitch-Steuerung muß ein Parameter für den Gas-Stick eingestellt werden. Hierzu
 *    bewegt man den Roll-Stick ganz nach außen und startet die Motoren. Man befindet sich im Setup-Modus
 *    und die Motoren laufen unabhängig vom Gas-Stick im Leerlauf. Im Display auf Seite 12 muß nun der
 *    beste Wert für "StickDiff" gefunden werden. Den Wert ändern kann man mit dem Roll-Stick. Man betätigt
 *    nun den Gas-Stick nach oben und läßt ihn los, so daß er in die Mittelstellung zurückspringt. Der
 *    optimale Wert sorgt dafür, daß das gewollte Loslassen von einer normalen Bewegung unterschieden wird.
 *    Jedesmal, wenn die SW das Loslassen als solches erkannt hat, ertönt ein Signal. Dieses darf jedoch
 *    nicht ertönen, wenn man den Stick mit normaler Geschwindigkeit in die Mittelstellung zurückbewegt.
 *    Wurde ein guter Wert eingestellt, verläßt man den Setup-Modus durch Ausschalten der Motoren. Der
 *    ermittelte Wert wird permanent im EEProm gespeichert und muß nicht neu ermittelt werden.
 *
 * 2. Nach dem Einschalten der FC wird die aktuelle Gas-Stick-Position gemerkt und als Kalibrierungswert
 *    für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite
 *    keine Rolle.
 *
 * 3. 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.
 *
 * 4. 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. Wichtig hierfür ist die unter Punkt 1 vorgenommene
 *    Parameterfindung. Stimmt dieser Wert nicht, führt das zur Wegnahme des Gases und somit zum Sinken.
 *
 * 5. 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.
 *
 * 6. 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.
 *
 * 7. 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? Ach ja, im Zustand STATE_SETUP sind die Motoren aus...
        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_initialStickValue();
                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. */
                pitchCount = 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;
 
                        case STATE_SETUP:
                                if( stickValue < PARAM_PITCH_STICK_THRESHOLD &&
                                    abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) < PARAM_PITCH_STICK_THRESHOLD ) {
                                        state = STATE_SETUP1;
                                }
                               
                                // Im Setup immer mit Leerlaufgas
                                pitchCount = 0;
                               
                                break;
                               
                        case STATE_SETUP1:
                       
                                // Roll-Stick nach links
                                if( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] < -20 ) {
                                        if( pitchNeutralDiff < 20 ) {
                                                pitchNeutralDiff++;
                                       
                                                // Konfiguration dauerhaft speichern
                                                eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ], pitchNeutralDiff );
                                       
                                                // Signalisieren
                                                beeptime = 500;
                                       
                                                state    = STATE_SETUP;
                                        }
                                }
 
                                // Roll-Stick nach rechts
                                if( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] > 20 ) {
                                        if( pitchNeutralDiff > 0 ) {
                                                pitchNeutralDiff--;
 
                                                // Konfiguration dauerhaft speichern
                                                eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ], pitchNeutralDiff );
 
                                                // Signalisieren
                                                beeptime = 500;
                                       
                                                state    = STATE_SETUP;
                                        }
                                }
 
                                if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&
                                    ( lastStickValue - stickValue >= pitchNeutralDiff ) ) {
                                   
                                        state    = STATE_SETUP;
                                       
                                        // Signalisieren
                                        beeptime = 500;
                                }
                               
                                // Im Setup immer mit Leerlaufgas
                                pitchCount = 0;
                               
                                break;
 
                        case STATE_BEGIN:
 
                                // Schnelles Bewegen aus dem oberen Bereich des Sticks in Neutralstellung
                                if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&
                                    ( lastStickValue - stickValue >= pitchNeutralDiff ) ) {
 
                                        pitchOffset       = lastStickValue;
                                       
                                        state             = STATE_BEGIN1;
                                        pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
                                }
                                break;
 
                        case STATE_BEGIN1:
 
                                // 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 ) {
                                        state = STATE_ACTIVATING;
                                }
                                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_INITIALIZE;
        }
 
        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;
}
 
void pitch_neutral_init( void ) {
 
        printf("\r\npitch_mk_init()");
 
        pitchNeutralDiff = eeprom_read_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ] );
 
        pitchValueFP = pitch_neutral_value;
}
 
 

Rev 1141	Rev 1157
1	/* pitch_neutral.c	1	/* pitch_neutral.c
2	*	2	*
3	* Copyright 2009 Thomas Jachmann	3	* Copyright 2009 Thomas Jachmann
4	*/	4	*/
5		5
6	#include "main.h"	6	#include "main.h"
7	#include "parameter.h"	7	#include "parameter.h"
8	#include "fc.h"	8	#include "fc.h"
9	#include "altcon.h"	9	#include "altcon.h"
10	#include "pitch.h"	10	#include "pitch.h"
11	#include "pitch_neutral.h"	11	#include "pitch_neutral.h"
12		12
13	#define STATE_INITIALIZE 0x00 // Initialisierung nach Einschalten der Motoren	13	#define STATE_INITIALIZE 0x00 // Initialisierung nach Einschalten der Motoren
14	#define STATE_SETUP 0x01 // Kalibrierung des Gas-Sticks	14	#define STATE_SETUP 0x01 // Kalibrierung des Gas-Sticks
15	#define STATE_SETUP1 0x02 //	15	#define STATE_SETUP1 0x02 //
16	#define STATE_BEGIN 0x03 // Startphase	16	#define STATE_BEGIN 0x03 // Startphase
17	#define STATE_BEGIN1 0x04 //	17	#define STATE_BEGIN1 0x04 //
18	#define STATE_INACTIVE 0x05 // Manuelle Kontrolle	18	#define STATE_INACTIVE 0x05 // Manuelle Kontrolle
19	#define STATE_WAIT 0x06 // Warten auf Einschalten der Höhenregelung	19	#define STATE_WAIT 0x06 // Warten auf Einschalten der Höhenregelung
20	#define STATE_ACTIVATING 0x07 // Aktivierung der Höhenregelung	20	#define STATE_ACTIVATING 0x07 // Aktivierung der Höhenregelung
21	#define STATE_ACTIVE 0x08 // Höhenregelung ist aktiv	21	#define STATE_ACTIVE 0x08 // Höhenregelung ist aktiv
22		22
23		23
24	static int stickValue = 0; // Aktueller Stick-Wert	24	static int stickValue = 0; // Aktueller Stick-Wert
25	static int lastStickValue = 0; // Vorheriger Stick-Wert	25	static int lastStickValue = 0; // Vorheriger Stick-Wert
26	static int pitchOffset = 0; // Aktueller Grundgaswert in Neutralstellung	26	static int pitchOffset = 0; // Aktueller Grundgaswert in Neutralstellung
27	static char state; // Zustand	27	static char state; // Zustand
28	static int temp;	28	static int temp;
29		29
30	char pitchNeutralDiff = 0;	30	char pitchNeutralDiff = 0;
31		31
32		32
33	/* Wird verwendet, um das Umschalten auf automatische Höhenregelung	33	/* Wird verwendet, um das Umschalten auf automatische Höhenregelung
34	* nach Erreichen der Neutralstellung zu verzögern.	34	* nach Erreichen der Neutralstellung zu verzögern.
35	*/	35	*/
36	static int pitchNeutralTimer = PITCH_NEUTRAL_TIMER;	36	static int pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
37		37
38		-
39	void pitch_neutral_init( void ) {	-
40	pitchNeutralDiff = eeprom_read_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ] );	-
41	}	-
42		38
43		39
44	/*	40	/*
45	* Berechnet den aktuellen Pitch-Wert für die Regelung	41	* Berechnet den aktuellen Pitch-Wert für die Regelung
46	*	42	*
47	* Funktionsweise:	43	* Funktionsweise:
48	*	44	*
49	* 1. Vor Verwendung der Pitch-Steuerung muß ein Parameter für den Gas-Stick eingestellt werden. Hierzu	45	* 1. Vor Verwendung der Pitch-Steuerung muß ein Parameter für den Gas-Stick eingestellt werden. Hierzu
50	* bewegt man den Roll-Stick ganz nach außen und startet die Motoren. Man befindet sich im Setup-Modus	46	* bewegt man den Roll-Stick ganz nach außen und startet die Motoren. Man befindet sich im Setup-Modus
51	* und die Motoren laufen unabhängig vom Gas-Stick im Leerlauf. Im Display auf Seite 12 muß nun der	47	* und die Motoren laufen unabhängig vom Gas-Stick im Leerlauf. Im Display auf Seite 12 muß nun der
52	* beste Wert für "StickDiff" gefunden werden. Den Wert ändern kann man mit dem Roll-Stick. Man betätigt	48	* beste Wert für "StickDiff" gefunden werden. Den Wert ändern kann man mit dem Roll-Stick. Man betätigt
53	* nun den Gas-Stick nach oben und läßt ihn los, so daß er in die Mittelstellung zurückspringt. Der	49	* nun den Gas-Stick nach oben und läßt ihn los, so daß er in die Mittelstellung zurückspringt. Der
54	* optimale Wert sorgt dafür, daß das gewollte Loslassen von einer normalen Bewegung unterschieden wird.	50	* optimale Wert sorgt dafür, daß das gewollte Loslassen von einer normalen Bewegung unterschieden wird.
55	* Jedesmal, wenn die SW das Loslassen als solches erkannt hat, ertönt ein Signal. Dieses darf jedoch	51	* Jedesmal, wenn die SW das Loslassen als solches erkannt hat, ertönt ein Signal. Dieses darf jedoch
56	* nicht ertönen, wenn man den Stick mit normaler Geschwindigkeit in die Mittelstellung zurückbewegt.	52	* nicht ertönen, wenn man den Stick mit normaler Geschwindigkeit in die Mittelstellung zurückbewegt.
57	* Wurde ein guter Wert eingestellt, verläßt man den Setup-Modus durch Ausschalten der Motoren. Der	53	* Wurde ein guter Wert eingestellt, verläßt man den Setup-Modus durch Ausschalten der Motoren. Der
58	* ermittelte Wert wird permanent im EEProm gespeichert und muß nicht neu ermittelt werden.	54	* ermittelte Wert wird permanent im EEProm gespeichert und muß nicht neu ermittelt werden.
59	*	55	*
60	* 2. Nach dem Einschalten der FC wird die aktuelle Gas-Stick-Position gemerkt und als Kalibrierungswert	56	* 2. Nach dem Einschalten der FC wird die aktuelle Gas-Stick-Position gemerkt und als Kalibrierungswert
61	* für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite	57	* für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite
62	* keine Rolle.	58	* keine Rolle.
63	*	59	*
64	* 3. Nach Einschalten der Motoren geht der Stick in Neutralstellung. Diese Stick-Bewegung wird ignoriert	60	* 3. Nach Einschalten der Motoren geht der Stick in Neutralstellung. Diese Stick-Bewegung wird ignoriert
65	* und die Motoren drehen mit dem eingestellten MinGas2. Ausgehend von der Neutralstellung wird nun	61	* und die Motoren drehen mit dem eingestellten MinGas2. Ausgehend von der Neutralstellung wird nun
66	* durch Bewegen des Sticks im oberen Bereich das Gas geregelt.	62	* durch Bewegen des Sticks im oberen Bereich das Gas geregelt.
67	*	63	*
68	* 4. Das erstmalige Aktivieren der automatischen Höhenregelung erfolgt durch Loslassen des Sticks im	64	* 4. Das erstmalige Aktivieren der automatischen Höhenregelung erfolgt durch Loslassen des Sticks im
69	* Schwebeflug. Der zuvor aktuelle Stick-Wert wird als Wert in Neutralstellung übernommen und die	65	* Schwebeflug. Der zuvor aktuelle Stick-Wert wird als Wert in Neutralstellung übernommen und die
70	* automatische Höhenregelung sofort aktiviert. Wichtig hierfür ist die unter Punkt 1 vorgenommene	66	* automatische Höhenregelung sofort aktiviert. Wichtig hierfür ist die unter Punkt 1 vorgenommene
71	* Parameterfindung. Stimmt dieser Wert nicht, führt das zur Wegnahme des Gases und somit zum Sinken.	67	* Parameterfindung. Stimmt dieser Wert nicht, führt das zur Wegnahme des Gases und somit zum Sinken.
72	*	68	*
73	* 5. Sobald der Stick die Neutralstellung verläßt, wird die automatische Höhenregelung deaktiviert	69	* 5. Sobald der Stick die Neutralstellung verläßt, wird die automatische Höhenregelung deaktiviert
74	* und der vorige Pitch-Wert als Wert der Neutralstellung übernommen. Der Pitch läßt sich nun	70	* und der vorige Pitch-Wert als Wert der Neutralstellung übernommen. Der Pitch läßt sich nun
75	* über den gesamten Stick-Bereich regeln.	71	* über den gesamten Stick-Bereich regeln.
76	*	72	*
77	* 6. Erreicht der Stick ein weiteres Mal die Neutralstellung, wird die automatische Höhenregelung	73	* 6. Erreicht der Stick ein weiteres Mal die Neutralstellung, wird die automatische Höhenregelung
78	* wieder aktiviert, jetzt jedoch immer mit einer zeitlichen Verzögerung. Nur so ist ein	74	* wieder aktiviert, jetzt jedoch immer mit einer zeitlichen Verzögerung. Nur so ist ein
79	* ungestörtes manuelles Steuern möglich.	75	* ungestörtes manuelles Steuern möglich.
80	*	76	*
81	* 7. Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt.	77	* 7. Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt.
82	* Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb	78	* Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb
83	* eines Wertes zu gelangen, der die Lageregelung außer Funktion setzt, wird ein zweiter Wert für	79	* eines Wertes zu gelangen, der die Lageregelung außer Funktion setzt, wird ein zweiter Wert für
84	* Minimalgas konfiguriert, der greift, sobald erstmalig die automatische Höhenregelung aktiviert wurde.	80	* Minimalgas konfiguriert, der greift, sobald erstmalig die automatische Höhenregelung aktiviert wurde.
85	*/	81	*/
86	int pitch_neutral_value( void ) {	82	int pitch_neutral_value( void ) {
87		83
88	int register pitchCount = 0;	84	int register pitchCount = 0;
89		85
90	// Sind die Motoren eingeschaltet? Ach ja, im Zustand STATE_SETUP sind die Motoren aus...	86	// Sind die Motoren eingeschaltet? Ach ja, im Zustand STATE_SETUP sind die Motoren aus...
91	if( MotorenEin ) {	87	if( MotorenEin ) {
92		88
93	// Vorigen Stick-Wert merken	89	// Vorigen Stick-Wert merken
94	lastStickValue = stickValue;	90	lastStickValue = stickValue;
95		91
96	/* StickValue exponentiell angleichen, da ausgehend von der Neutralstellung	92	/* StickValue exponentiell angleichen, da ausgehend von der Neutralstellung
97	* nur jeweils die halbe Auflösung nach oben und unten zur Verfügung steht. Bei einer	93	* nur jeweils die halbe Auflösung nach oben und unten zur Verfügung steht. Bei einer
98	* Multiplikation mit 2 ließe sich das Gas im Schwebebereich nicht fein genug einstellen. */	94	* Multiplikation mit 2 ließe sich das Gas im Schwebebereich nicht fein genug einstellen. */
99	temp = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_initialStickValue();	95	temp = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_initialStickValue();
100	if( temp > 0 ) {	96	if( temp > 0 ) {
101	temp = temp + ( ( temp * temp ) / 150 );	97	temp = temp + ( ( temp * temp ) / 150 );
102	} else {	98	} else {
103	temp = temp - ( ( temp * temp ) / 150 );	99	temp = temp - ( ( temp * temp ) / 150 );
104	}	100	}
105		101
106	stickValue = temp;	102	stickValue = temp;
107		103
108	/* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset	104	/* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset
109	* zuzüglich dem Stick-Wert. */	105	* zuzüglich dem Stick-Wert. */
110	pitchCount = stickValue + pitchOffset;	106	pitchCount = stickValue + pitchOffset;
111		107
112	switch( state ) {	108	switch( state ) {
113		109
114	/* Entscheidet über Flugbetrieb oder Setup-Betrieb. Für den Setup-Betrieb	110	/* Entscheidet über Flugbetrieb oder Setup-Betrieb. Für den Setup-Betrieb
115	* muß beim Einschalten der Motoren gleichzeitig der Roll-Stick ganz	111	* muß beim Einschalten der Motoren gleichzeitig der Roll-Stick ganz
116	* betätigt werden (Richtung ist egal).	112	* betätigt werden (Richtung ist egal).
117	*/	113	*/
118	case STATE_INITIALIZE:	114	case STATE_INITIALIZE:
119		115
120	if( abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) > 70 ) {	116	if( abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) > 70 ) {
121	state = STATE_SETUP;	117	state = STATE_SETUP;
122	} else {	118	} else {
123	state = STATE_BEGIN;	119	state = STATE_BEGIN;
124	}	120	}
125	break;	121	break;
126		122
127	case STATE_SETUP:	123	case STATE_SETUP:
128	if( stickValue < PARAM_PITCH_STICK_THRESHOLD &&	124	if( stickValue < PARAM_PITCH_STICK_THRESHOLD &&
129	abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) < PARAM_PITCH_STICK_THRESHOLD ) {	125	abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) < PARAM_PITCH_STICK_THRESHOLD ) {
130	state = STATE_SETUP1;	126	state = STATE_SETUP1;
131	}	127	}
132		128
133	// Im Setup immer mit Leerlaufgas	129	// Im Setup immer mit Leerlaufgas
134	pitchCount = 0;	130	pitchCount = 0;
135		131
136	break;	132	break;
137		133
138	case STATE_SETUP1:	134	case STATE_SETUP1:
139		135
140	// Roll-Stick nach links	136	// Roll-Stick nach links
141	if( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] < -20 ) {	137	if( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] < -20 ) {
142	if( pitchNeutralDiff < 20 ) {	138	if( pitchNeutralDiff < 20 ) {
143	pitchNeutralDiff++;	139	pitchNeutralDiff++;
144		140
145	// Konfiguration dauerhaft speichern	141	// Konfiguration dauerhaft speichern
146	eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ], pitchNeutralDiff );	142	eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ], pitchNeutralDiff );
147		143
148	// Signalisieren	144	// Signalisieren
149	beeptime = 500;	145	beeptime = 500;
150		146
151	state = STATE_SETUP;	147	state = STATE_SETUP;
152	}	148	}
153	}	149	}
154		150
155	// Roll-Stick nach rechts	151	// Roll-Stick nach rechts
156	if( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] > 20 ) {	152	if( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] > 20 ) {
157	if( pitchNeutralDiff > 0 ) {	153	if( pitchNeutralDiff > 0 ) {
158	pitchNeutralDiff--;	154	pitchNeutralDiff--;
159		155
160	// Konfiguration dauerhaft speichern	156	// Konfiguration dauerhaft speichern
161	eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ], pitchNeutralDiff );	157	eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ], pitchNeutralDiff );
162		158
163	// Signalisieren	159	// Signalisieren
164	beeptime = 500;	160	beeptime = 500;
165		161
166	state = STATE_SETUP;	162	state = STATE_SETUP;
167	}	163	}
168	}	164	}
169		165
170	if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&	166	if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&
171	( lastStickValue - stickValue >= pitchNeutralDiff ) ) {	167	( lastStickValue - stickValue >= pitchNeutralDiff ) ) {
172		168
173	state = STATE_SETUP;	169	state = STATE_SETUP;
174		170
175	// Signalisieren	171	// Signalisieren
176	beeptime = 500;	172	beeptime = 500;
177	}	173	}
178		174
179	// Im Setup immer mit Leerlaufgas	175	// Im Setup immer mit Leerlaufgas
180	pitchCount = 0;	176	pitchCount = 0;
181		177
182	break;	178	break;
183		179
184	case STATE_BEGIN:	180	case STATE_BEGIN:
185		181
186	// Schnelles Bewegen aus dem oberen Bereich des Sticks in Neutralstellung	182	// Schnelles Bewegen aus dem oberen Bereich des Sticks in Neutralstellung
187	if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&	183	if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) &&
188	( lastStickValue - stickValue >= pitchNeutralDiff ) ) {	184	( lastStickValue - stickValue >= pitchNeutralDiff ) ) {
189		185
190	pitchOffset = lastStickValue;	186	pitchOffset = lastStickValue;
191		187
192	state = STATE_BEGIN1;	188	state = STATE_BEGIN1;
193	pitchNeutralTimer = PITCH_NEUTRAL_TIMER;	189	pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
194	}	190	}
195	break;	191	break;
196		192
197	case STATE_BEGIN1:	193	case STATE_BEGIN1:
198		194
199	// Während der Initialisierung das Gas konstant halten	195	// Während der Initialisierung das Gas konstant halten
200	pitchCount = pitchOffset;	196	pitchCount = pitchOffset;
201		197
202	pitchNeutralTimer--;	198	pitchNeutralTimer--;
203		199
204	/* Läuft der Timer ab, bevor der Stick die Neutralstellung erreicht,	200	/* Läuft der Timer ab, bevor der Stick die Neutralstellung erreicht,
205	* wird die Aktion nicht als "schnelles Bewegen in Neutralstellung"	201	* wird die Aktion nicht als "schnelles Bewegen in Neutralstellung"
206	* gedeutet. */	202	* gedeutet. */
207	if( !pitchNeutralTimer ) {	203	if( !pitchNeutralTimer ) {
208	pitchOffset = 0;	204	pitchOffset = 0;
209	state = STATE_BEGIN;	205	state = STATE_BEGIN;
210	}	206	}
211		207
212	// Ist die Neutralstellung erreicht?	208	// Ist die Neutralstellung erreicht?
213	if( abs( stickValue ) <= PARAM_PITCH_STICK_THRESHOLD ) {	209	if( abs( stickValue ) <= PARAM_PITCH_STICK_THRESHOLD ) {
214	state = STATE_ACTIVATING;	210	state = STATE_ACTIVATING;
215	}	211	}
216	break;	212	break;
217		213
218	/* Die Höhenregelung ist per Konfiguration aktiviert, jedoch befindet	214	/* Die Höhenregelung ist per Konfiguration aktiviert, jedoch befindet
219	* sich der Stick außerhalb des als Neutralstellung anerkannten	215	* sich der Stick außerhalb des als Neutralstellung anerkannten
220	* Wertebereiches. Es wird manuell geregelt. */	216	* Wertebereiches. Es wird manuell geregelt. */
221	case STATE_INACTIVE:	217	case STATE_INACTIVE:
222		218
223	// Ist ein Restart zulässig?	219	// Ist ein Restart zulässig?
224	if( PARAM_PITCH_RESTART_ENABLED ) {	220	if( PARAM_PITCH_RESTART_ENABLED ) {
225		221
226	/* Wenn der Gashebel ganz unten steht, Timer für Reduzierung des Minimalgaswertes	222	/* Wenn der Gashebel ganz unten steht, Timer für Reduzierung des Minimalgaswertes
227	* starten. Hierfür wird die Variable pitchNeutralTimer verwendet. */	223	* starten. Hierfür wird die Variable pitchNeutralTimer verwendet. */
228	if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 120 ) {	224	if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 120 ) {
229	pitchNeutralTimer = PITCH_MIN2_TIMER;	225	pitchNeutralTimer = PITCH_MIN2_TIMER;
230	} else {	226	} else {
231	pitchNeutralTimer--;	227	pitchNeutralTimer--;
232		228
233	/* Gashebel steht seit PITCH_MIN2_TIMER ganz unten; jetzt erfolgt die Initialisierung. */	229	/* Gashebel steht seit PITCH_MIN2_TIMER ganz unten; jetzt erfolgt die Initialisierung. */
234	if( !pitchNeutralTimer ) {	230	if( !pitchNeutralTimer ) {
235	state = STATE_BEGIN;	231	state = STATE_BEGIN;
236	pitchOffset = 0;	232	pitchOffset = 0;
237		233
238	// Signalisieren	234	// Signalisieren
239	beeptime = 500;	235	beeptime = 500;
240	}	236	}
241	}	237	}
242	}	238	}
243		239
244	// Min2-Gas einstellen für Lageregelung bei Minimalgas	240	// Min2-Gas einstellen für Lageregelung bei Minimalgas
245	if( pitchCount < PARAM_PITCH_MIN2 ) {	241	if( pitchCount < PARAM_PITCH_MIN2 ) {
246	pitchCount = PARAM_PITCH_MIN2;	242	pitchCount = PARAM_PITCH_MIN2;
247	}	243	}
248		244
249	// Stick ist innerhalb der Neutralstellung	245	// Stick ist innerhalb der Neutralstellung
250	if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD ) {	246	if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD ) {
251		247
252	// Timer neu setzen	248	// Timer neu setzen
253	pitchNeutralTimer = PITCH_NEUTRAL_TIMER;	249	pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
254	state = STATE_WAIT;	250	state = STATE_WAIT;
255	}	251	}
256	break;	252	break;
257		253
258	/* Der Stick ist in den für die Neutralstellung gültigen Wertebereich	254	/* Der Stick ist in den für die Neutralstellung gültigen Wertebereich
259	* gelangt. Nun darf innerhalb einer bestimmten Zeit keine Stick-Bewegung	255	* gelangt. Nun darf innerhalb einer bestimmten Zeit keine Stick-Bewegung
260	* erfolgen, um die automatische Höhenregelung zu aktivieren. */	256	* erfolgen, um die automatische Höhenregelung zu aktivieren. */
261	case STATE_WAIT:	257	case STATE_WAIT:
262		258
263	/* Stick ist innerhalb der Neutralstellung und	259	/* Stick ist innerhalb der Neutralstellung und
264	Stick-Differenzial ist < 2 */	260	Stick-Differenzial ist < 2 */
265	if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD &&	261	if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD &&
266	lastStickValue == stickValue ) {	262	lastStickValue == stickValue ) {
267		263
268	pitchNeutralTimer--;	264	pitchNeutralTimer--;
269		265
270	if( !pitchNeutralTimer ) {	266	if( !pitchNeutralTimer ) {
271	state = STATE_ACTIVATING;	267	state = STATE_ACTIVATING;
272	}	268	}
273		269
274	// Aktivierungskriterium nicht erfüllt, zurück in INACTIVE	270	// Aktivierungskriterium nicht erfüllt, zurück in INACTIVE
275	} else {	271	} else {
276	state = STATE_INACTIVE;	272	state = STATE_INACTIVE;
277	}	273	}
278	break;	274	break;
279		275
280	/* Die automatische Höhenregelung wird jetzt aktiviert. Der aktuelle	276	/* Die automatische Höhenregelung wird jetzt aktiviert. Der aktuelle
281	* Luftdruck wird gespeichert und notwendige Werte für den Regler	277	* Luftdruck wird gespeichert und notwendige Werte für den Regler
282	* werden initialisiert. */	278	* werden initialisiert. */
283	case STATE_ACTIVATING:	279	case STATE_ACTIVATING:
284		280
285	// Höhenregler starten	281	// Höhenregler starten
286	altcon_start();	282	altcon_start();
287		283
288	state = STATE_ACTIVE;	284	state = STATE_ACTIVE;
289		285
290	break;	286	break;
291		287
292	/* Die automatische Höhenregelung ist aktiv. */	288	/* Die automatische Höhenregelung ist aktiv. */
293	case STATE_ACTIVE:	289	case STATE_ACTIVE:
294		290
295	// Stick ist außerhalb der Neutralstellung	291	// Stick ist außerhalb der Neutralstellung
296	if( abs( stickValue ) > PARAM_PITCH_STICK_THRESHOLD ) {	292	if( abs( stickValue ) > PARAM_PITCH_STICK_THRESHOLD ) {
297	pitchOffset -= altcon_avgerror() / 4;	293	pitchOffset -= altcon_avgerror() / 4;
298	pitchCount = stickValue + pitchOffset;	294	pitchCount = stickValue + pitchOffset;
299		295
300	// Höhenregler abschalten	296	// Höhenregler abschalten
301	altcon_stop();	297	altcon_stop();
302		298
303	state = STATE_INACTIVE;	299	state = STATE_INACTIVE;
304	}	300	}
305	break;	301	break;
306	}	302	}
307		303
308	// Motoren sind aus	304	// Motoren sind aus
309	} else {	305	} else {
310		306
311	/* Nach dem Einschalten der Motoren darf pitchOffset keinen hohen Wert haben,	307	/* Nach dem Einschalten der Motoren darf pitchOffset keinen hohen Wert haben,
312	* da der Kopter sonst sofort hochschießen würde.	308	* da der Kopter sonst sofort hochschießen würde.
313	*/	309	*/
314	pitchCount = 0;	310	pitchCount = 0;
315	pitchOffset = 0;	311	pitchOffset = 0;
316	stickValue = 0;	312	stickValue = 0;
317	state = STATE_INITIALIZE;	313	state = STATE_INITIALIZE;
318	}	314	}
319		315
320	if( pitchOffset < 0 )	316	if( pitchOffset < 0 )
321	pitchOffset = 0;	317	pitchOffset = 0;
322		318
323	// Pitch-Wert darf nicht < 0 sein	319	// Pitch-Wert darf nicht < 0 sein
324	if( pitchCount < 0 ) {	320	if( pitchCount < 0 ) {
325	pitchCount = 0;	321	pitchCount = 0;
326	}	322	}
327		323
328	// pitchCount als Debug-Wert rausschreiben	324	// pitchCount als Debug-Wert rausschreiben
329	DebugOut.Analog[26] = stickValue;	325	DebugOut.Analog[26] = stickValue;
330	DebugOut.Analog[28] = pitchCount;	326	DebugOut.Analog[28] = pitchCount;
331	DebugOut.Analog[29] = pitchOffset;	327	DebugOut.Analog[29] = pitchOffset;
332		328
333	return pitchCount;	329	return pitchCount;
334	}	330	}
-		331
-		332	void pitch_neutral_init( void ) {
-		333
-		334	printf("\r\npitch_mk_init()");
-		335
-		336	pitchNeutralDiff = eeprom_read_byte( &EEPromArray[ EEPROM_ADR_PITCH_NEUTRAL_DIFF ] );
-		337
-		338	pitchValueFP = pitch_neutral_value;
-		339	}
-		340
335		341

Subversion Repositories FlightCtrl

(root)/branches/thjac/V1_11/pitch_neutral.c @ 1161 - Rev 1141 → 1157