Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1263 thjac 1
/* pitch.c
2
 *
3
 * Copyright 2009 Thomas Jachmann
4
 *
5
 * Pitch-Steuerung
6
 */
7
 
8
#include "main.h"
9
#include "parameter.h"
10
#include "pitch_md.h"
11
#include "pitch.h"
12
 
13
#define UNDEFINED                       255
14
 
15
int  pitchInitialStickValue = UNDEFINED;
16
unsigned char pitchMode    = PARAM_PITCH_MODE_MK;
17
 
18
 
19
char *pitchModeStrings[] = {
20
        "MK",
21
        "MD",
22
        "??"
23
};
24
 
25
 
26
// Prototypen
27
int pitch_mk_value( void );
28
 
29
 
30
/*
31
 * Führt die Initialisierung der Pitch-Steuerung durch. Diese Funktion
32
 * wird nach jeder Setting-Auswahl sowie nach jeder Setting-Änderung
33
 * aufgerufen.
34
 */
35
void pitch_init( void ) {
36
 
37
        // Der Schalter Höhenregelung bestimmt den Pitch-Modus
38
        if( EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG ) {
39
                pitchMode = PARAM_PITCH_MODE_MD;
40
        } else {
41
                pitchMode = PARAM_PITCH_MODE_MK;
42
        }
43
 
44
        // Nur beim Einschalten ermitteln, da beim Setting-Wechsel ungültig
45
        if( pitchInitialStickValue == UNDEFINED ) {
46
 
47
                // Warten, bis ein gültiger Wert von der Fernsteuerung anliegt
48
                unsigned int timer = SetDelay( 500 );
49
                while( !CheckDelay( timer ) );
50
 
51
                pitchInitialStickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ];
52
        }
53
 
54
        /* Die Stick-Position beim Einschalten muß mit dem im Setting eingestellten
55
         * Pitch-Modus übereinstimmen. Sonst wird die Pitch-Steuerung aus Sicherheits-
56
         * gründen deaktiviert. Selbiges gilt, wenn die Höhenregelung deaktiviert wurde.
57
         */
58
        switch( pitchMode ) {
59
 
60
                // Pitch-Modus mit 0-Gas
61
                case PARAM_PITCH_MODE_MK:
62
                        if( pitchInitialStickValue > 35 - 120 ) {
63
                                pitchMode = PARAM_PITCH_MODE_INVALID;
64
                        }
65
                        break;
66
 
67
                // Pitch-Modus mit neutralisiertem Gas und Standgas
68
                case PARAM_PITCH_MODE_MD:
69
                        if( abs( pitchInitialStickValue ) > PARAM_PITCH_STICK_THRESHOLD ) {
70
                                pitchMode = PARAM_PITCH_MODE_INVALID;
71
                        } else  {
72
                                pitch_md_init();
73
                        }
74
                        break;
75
        }
76
}
77
 
78
 
79
int pitch_value( void ) {
80
 
81
        switch( pitchMode ) {
82
 
83
                // Pitch-Modus mit 0-Gas
84
                case PARAM_PITCH_MODE_MK:
85
                        return pitch_mk_value();
86
 
87
                // Pitch-Modus mit neutralisiertem Gas und Standgas
88
                case PARAM_PITCH_MODE_MD:
89
                        return pitch_md_value();
90
 
91
                default:
92
                        return 0;
93
        }
94
}
95
 
96
 
97
/*
98
 * Führt eine Pitch-Berechnung aus, die der Original-SW entspricht.
99
 */
100
int pitch_mk_value( void ) {
101
 
102
        register int stickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ];
103
        register int pitchCount = stickValue + 120;
104
 
105
        DebugOut.Analog[26] = stickValue;
106
 
107
        return pitchCount;
108
}