Rev 1141 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1141 | Rev 1157 | ||
---|---|---|---|
1 | /* pitch.c |
1 | /* pitch.c |
2 | * |
2 | * |
3 | * Copyright 2009 Thomas Jachmann |
3 | * Copyright 2009 Thomas Jachmann |
4 | * |
4 | * |
5 | * Pitch-Steuerung |
5 | * Pitch-Steuerung |
6 | */ |
6 | */ |
7 | 7 | ||
8 | #include "main.h" |
8 | #include "main.h" |
9 | #include "parameter.h" |
9 | #include "parameter.h" |
10 | #include "pitch_neutral.h" |
10 | #include "pitch_neutral.h" |
11 | #include "pitch_md.h" |
11 | #include "pitch_md.h" |
- | 12 | #include "pitch_inc.h" |
|
12 | #include "pitch.h" |
13 | #include "pitch.h" |
13 | 14 | ||
14 | #define UNDEFINED 255 |
15 | #define UNDEFINED 255 |
15 | #define INVALID 255 |
16 | #define INVALID 255 |
16 | 17 | ||
17 | int pitchInitialStickValue = UNDEFINED; |
18 | int pitchInitialStickValue = UNDEFINED; |
18 | unsigned char pitchMode; |
19 | unsigned char pitchMode; |
19 | 20 | ||
20 | 21 | ||
21 | char *pitchModeStrings[] = { |
22 | char *pitchModeStrings[] = { |
22 | "Normal ", |
23 | "Normal ", |
23 | "Neutral ", |
24 | "Neutral ", |
24 | "MD ", |
25 | "MD ", |
- | 26 | "Increment" |
|
- | 27 | "Undefined", |
|
- | 28 | "Undefined", |
|
- | 29 | "Undefined", |
|
25 | "Inaktiv " |
30 | "Invalid ", |
26 | }; |
31 | }; |
27 | 32 | ||
28 | 33 | ||
29 | // Zeiger auf den durch das Setting bestimmten Pitch-Steuerungsalgorithmus |
34 | // Zeiger auf den durch das Setting bestimmten Pitch-Steuerungsalgorithmus |
30 | int (* pitch_value_ptr)( void ); |
35 | int (* pitchValueFP)( void ); |
31 | 36 | ||
- | 37 | // Prototyp |
|
32 | // Prototyp |
38 | void pitch_mk_init( void ); |
33 | int pitch_mk_value( void ); |
39 | int pitch_mk_value( void ); |
34 | 40 | ||
35 | /* |
41 | /* |
36 | * Führt die Initialisierung der Pitch-Steuerung durch. Diese Funktion |
42 | * Führt die Initialisierung der Pitch-Steuerung durch. Diese Funktion |
37 | * wird nach jeder Setting-Auswahl sowie nach jeder Setting-Änderung |
43 | * wird nach jeder Setting-Auswahl sowie nach jeder Setting-Änderung |
38 | * aufgerufen. |
44 | * aufgerufen. |
39 | */ |
45 | */ |
40 | void pitch_init( void ) { |
46 | void pitch_init( void ) { |
- | 47 | ||
- | 48 | ||
- | 49 | pitchMode = PARAM_PITCH_MODE; |
|
- | 50 | pitchValueFP = pitch_mk_value; // Reasonable default |
|
41 | 51 | ||
42 | // Nur beim Einschalten ermitteln, da beim Setting-Wechsel ungültig |
52 | // Nur beim Einschalten ermitteln, da beim Setting-Wechsel ungültig |
43 | if( pitchInitialStickValue == UNDEFINED ) { |
53 | if( pitchInitialStickValue == UNDEFINED ) { |
44 | 54 | ||
45 | // Warten, bis ein gültiger Wert von der Fernsteuerung anliegt |
55 | // Warten, bis ein gültiger Wert von der Fernsteuerung anliegt |
46 | unsigned int timer = SetDelay( 500 ); |
56 | unsigned int timer = SetDelay( 500 ); |
47 | while( !CheckDelay( timer ) ); |
57 | while( !CheckDelay( timer ) ); |
48 | 58 | ||
49 | pitchInitialStickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ]; |
59 | pitchInitialStickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ]; |
50 | } |
60 | } |
51 | - | ||
52 | pitchMode = PARAM_PITCH_MODE; |
- | |
53 | 61 | ||
54 | /* Die Stick-Position beim Einschalten muß mit dem im Setting eingestellten |
62 | /* Die Stick-Position beim Einschalten muß mit dem im Setting eingestellten |
55 | * Pitch-Modus übereinstimmen. Sonst wird die Pitch-Steuerung aus Sicherheits- |
63 | * Pitch-Modus übereinstimmen. Sonst wird die Pitch-Steuerung aus Sicherheits- |
56 | * gründen deaktiviert. Selbiges gilt, wenn die Höhenregelung deaktiviert wurde. |
64 | * gründen deaktiviert. Selbiges gilt, wenn die Höhenregelung deaktiviert wurde. |
57 | */ |
65 | */ |
- | 66 | ||
58 | switch( pitchMode ) { |
67 | switch( pitchMode ) { |
59 | // Pitch-Modi mit neutralisiertem Gas |
68 | // Pitch-Modi mit neutralisiertem Gas |
60 | case PARAM_PITCH_MODE_NEUTRAL: |
69 | case PARAM_PITCH_MODE_NEUTRAL: |
- | 70 | printf("\r\nPARAM_PITCH_MODE_NEUTRAL\r\n"); |
|
61 | case PARAM_PITCH_MODE_MD: |
71 | case PARAM_PITCH_MODE_MD: |
- | 72 | printf("\r\nPARAM_PITCH_MODE_MD\r\n"); |
|
62 | if( !( EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG ) ) |
73 | if( !( EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG ) || abs( pitchInitialStickValue ) > PARAM_PITCH_STICK_THRESHOLD ) |
63 | pitchMode = PARAM_PITCH_MODE_INVALID; |
74 | pitchMode = PARAM_PITCH_MODE_INVALID; |
- | 75 | else { |
|
- | 76 | if( pitchMode == PARAM_PITCH_MODE_NEUTRAL) |
|
- | 77 | pitch_neutral_init(); |
|
- | 78 | else if( pitchMode == PARAM_PITCH_MODE_MD) |
|
- | 79 | pitch_md_init(); |
|
- | 80 | } |
|
- | 81 | break; |
|
- | 82 | ||
- | 83 | // Pitch-Modi mit incrementellem Gas |
|
- | 84 | case PARAM_PITCH_MODE_INC: |
|
- | 85 | printf("\r\nPARAM_PITCH_MODE_INC\r\n"); |
|
64 | if( abs( pitchInitialStickValue ) > PARAM_PITCH_STICK_THRESHOLD ) |
86 | if( abs( pitchInitialStickValue ) > PARAM_PITCH_STICK_THRESHOLD ) |
65 | pitchMode = PARAM_PITCH_MODE_INVALID; |
87 | pitchMode = PARAM_PITCH_MODE_INVALID; |
- | 88 | else |
|
- | 89 | pitch_inc_init(); |
|
66 | break; |
90 | break; |
67 | 91 | ||
68 | // Pitch-Modi mit 0-Gas |
92 | // Pitch-Modi mit 0-Gas |
69 | case PARAM_PITCH_MODE_NORMAL: |
93 | case PARAM_PITCH_MODE_NORMAL: |
- | 94 | printf("\r\nPARAM_PITCH_MODE_NORMAL\r\n"); |
|
70 | if( pitchInitialStickValue > 35 - 120 ) |
95 | if( pitchInitialStickValue > 35 - 120 ) |
71 | pitchMode = PARAM_PITCH_MODE_INVALID; |
96 | pitchMode = PARAM_PITCH_MODE_INVALID; |
- | 97 | else |
|
- | 98 | pitch_mk_init(); |
|
72 | break; |
99 | break; |
73 | } |
100 | } |
- | 101 | ||
- | 102 | if( pitchMode == PARAM_PITCH_MODE_INVALID) |
|
- | 103 | printf("\r\nPARAM_PITCH_MODE_INVALID, stick: %d\r\n", pitchInitialStickValue); //TODO |
|
74 | - | ||
75 | pitch_neutral_init(); |
- | |
- | 104 | ||
76 | pitch_md_init(); |
105 | |
77 | 106 | /* |
|
78 | // FIXME Funktioniert noch nicht |
107 | // Moved to pitch_xxx_init() |
79 | switch( pitchMode ) { |
108 | switch( pitchMode ) { |
80 | case PARAM_PITCH_MODE_NEUTRAL: |
109 | case PARAM_PITCH_MODE_NEUTRAL: |
81 | pitch_value_ptr = pitch_neutral_value; |
110 | pitch_value_ptr = &pitch_neutral_value; |
82 | break; |
111 | break; |
83 | 112 | ||
84 | case PARAM_PITCH_MODE_MD: |
113 | case PARAM_PITCH_MODE_MD: |
85 | pitch_value_ptr = pitch_md_value; |
114 | pitch_value_ptr = &pitch_md_value; |
- | 115 | break; |
|
- | 116 | ||
- | 117 | case PARAM_PITCH_MODE_INC: |
|
- | 118 | pitch_value_ptr = &pitch_inc_value; |
|
86 | break; |
119 | break; |
87 | 120 | ||
88 | default: |
121 | default: |
89 | pitch_value_ptr = pitch_mk_value; |
122 | pitch_value_ptr = &pitch_mk_value; |
90 | } |
123 | } |
- | 124 | */ |
|
91 | 125 | ||
92 | // Hier können weitere Initialisierungen folgen |
126 | // Hier können weitere Initialisierungen folgen |
93 | } |
127 | } |
94 | 128 | ||
- | 129 | ||
95 | 130 | /* |
|
- | 131 | int pitch_value( void ) { // -> int (* pitch_value)( void ); |
|
96 | int pitch_value( void ) { |
132 | |
97 | switch( pitchMode ) { |
133 | switch( pitchMode ) { |
98 | case PARAM_PITCH_MODE_NEUTRAL: |
134 | case PARAM_PITCH_MODE_NEUTRAL: |
99 | return pitch_neutral_value(); |
135 | return pitch_neutral_value(); |
100 | 136 | ||
101 | case PARAM_PITCH_MODE_MD: |
137 | case PARAM_PITCH_MODE_MD: |
102 | return pitch_md_value(); |
138 | return pitch_md_value(); |
103 | 139 | ||
104 | case PARAM_PITCH_MODE_NORMAL: |
140 | case PARAM_PITCH_MODE_NORMAL: |
105 | return pitch_mk_value(); |
141 | return pitch_mk_value(); |
- | 142 | ||
- | 143 | case PARAM_PITCH_MODE_INC: |
|
- | 144 | return pitch_inc_value(); |
|
- | 145 | break; |
|
106 | 146 | ||
107 | default: |
147 | default: |
108 | return 0; |
148 | return 0; |
109 | } |
149 | } |
- | 150 | ||
- | 151 | ||
- | 152 | return pitch_value_ptr(); |
|
- | 153 | ||
110 | } |
154 | } |
- | 155 | */ |
|
111 | 156 | ||
112 | 157 | ||
113 | /* |
158 | /* |
114 | * Führt eine Pitch-Berechnung aus, die der Original-SW entspricht. |
159 | * Führt eine Pitch-Berechnung aus, die der Original-SW entspricht. |
115 | */ |
160 | */ |
116 | int pitch_mk_value( void ) { |
161 | int pitch_mk_value( void ) { |
117 | register int stickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ]; |
162 | register int stickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ]; |
118 | register int pitchCount = stickValue + 120; |
163 | register int pitchCount = stickValue + 120; |
119 | 164 | ||
120 | DebugOut.Analog[26] = stickValue; |
165 | DebugOut.Analog[26] = stickValue; |
121 | DebugOut.Analog[28] = pitchCount; |
166 | DebugOut.Analog[28] = pitchCount; |
122 | 167 | ||
123 | return pitchCount; |
168 | return pitchCount; |
124 | } |
169 | } |
- | 170 | ||
- | 171 | void pitch_mk_init( void ) { |
|
- | 172 | ||
- | 173 | printf("\r\npitch_mk_init()"); |
|
- | 174 | ||
- | 175 | pitchValueFP = pitch_mk_value; |
|
- | 176 | } |
|
- | 177 | ||
- | 178 | ||
125 | 179 |