Subversion Repositories FlightCtrl

Rev

Rev 1134 | Rev 1141 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1134 Rev 1139
Line 21... Line 21...
21 
#define STATE_WAIT                      0x06                    // Warten auf Einschalten der Höhenregelung
 21 
#define STATE_WAIT                      0x06                    // Warten auf Einschalten der Höhenregelung
22 
#define STATE_ACTIVATING                0x07                    // Aktivierung der Höhenregelung
 22 
#define STATE_ACTIVATING                0x07                    // Aktivierung der Höhenregelung
23 
#define STATE_ACTIVE                    0x08                    // Höhenregelung ist aktiv
 23 
#define STATE_ACTIVE                    0x08                    // Höhenregelung ist aktiv
Line 24... Line 24...
24 
 
 24 
 
25 
 
 25 
 
26 
int     stickValue              = 0;                            // Aktueller Stick-Wert
 26 
static int              stickValue              = 0;            // Aktueller Stick-Wert
27 
int     lastStickValue          = 0;                            // Vorheriger Stick-Wert
 27 
static int              lastStickValue          = 0;            // Vorheriger Stick-Wert
28 
int     actualPitchCount;                                       // Soll-Pitch-Wert
 28 
static int              actualPitchCount = 0;                   // Soll-Pitch-Wert
29 
int     targetPitchCount;                                       // Ist-Pitch-Wert
 29 
static int              targetPitchCount = 0;                   // Ist-Pitch-Wert
30 
int     pitchOffset;                                            // Aktueller Grundgaswert in Neutralstellung
 30 
static int              pitchOffset;                            // Aktueller Grundgaswert in Neutralstellung
31 
char    state;                                                  // Zustand
 31 
static char             state;                                  // Zustand
32 
int     timer;
 32 
static int      timer;
33 
int     delay                   = 0;
 33 
static int      delay                   = 0;
- 
 
 34 
static int      delayCounter            = 0;
- 
 
 35 
static int              temp;                                   // Temporäre Werte; wird mehrfach verwendet
- 
 
 36 
 
- 
 
 37 
unsigned char           pitchHover = 0;                         // Standgaswert
- 
 
 38 
 
- 
 
 39 
 
- 
 
 40 
void pitch_md_init( void ) {
Line 34... Line 41...
34 
int     delayCounter            = 0;
 41 
        pitchHover = eeprom_read_byte( &EEPromArray[ EEPROM_ADR_PITCH_MD_HOVER ] );
35 
int     temp;                                                   // Temporäre Werte; wird mehrfach verwendet
 42 
}
36 
 
 43 
 
Line 78... Line 85...
78 
                } else {
 85 
                } else {
79 
                        temp = temp - ( ( temp * temp ) / 150 );
 86 
                        temp = temp - ( ( temp * temp ) / 150 );
80 
                }
 87 
                }
Line 81... Line 88...
81 
 
 88 
 
82 
                stickValue = temp;
 89 
                stickValue = temp;
83 
               
 90 
 
84 
                /* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset
 91 
                /* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset
85 
                 * zuzüglich dem Stick-Wert. */
 92 
                 * zuzüglich dem Stick-Wert. */
Line 86... Line 93...
86 
                targetPitchCount = stickValue + pitchOffset;
 93 
                targetPitchCount = stickValue + pitchOffset;
Line 120... Line 127...
120 
                                else {
 127 
                                else {
121 
                                        timer--;
 128 
                                        timer--;
Line 122... Line 129...
122 
 
 129 
 
123 
                                        /* Die Verweilzeit ist abgelaufen und der aktuelle Pitch-Wert
 130 
                                        /* Die Verweilzeit ist abgelaufen und der aktuelle Pitch-Wert
124 
                                         * entspricht nicht dem bereits gespeicherten Wert. */
 131 
                                         * entspricht nicht dem bereits gespeicherten Wert. */
Line 125... Line 132...
125 
                                        if( !timer && ( PARAM_PITCH_MD_HOVER != actualPitchCount ) ) {
 132 
                                        if( !timer && ( pitchHover != actualPitchCount ) ) {
126 
 
 133 
 
127 
                                                // Aktuellen Pitch-Wert in Konfiguration übernehmen
 134 
                                                // Aktuellen Pitch-Wert in Konfiguration übernehmen
128 
                                                PARAM_PITCH_MD_HOVER = actualPitchCount;
 135 
                                                pitchHover = actualPitchCount;
- 
 
 136 
                                               
- 
 
 137 
                                                // Konfiguration dauerhaft speichern
129 
 
 138 
                                                eeprom_write_byte( &EEPromArray[ EEPROM_ADR_PITCH_MD_HOVER ], pitchHover );
130 
                                                // Konfiguration dauerhaft speichern
 139 
/*                                             
131 
                                                WriteParameterSet(
 140 
                                                WriteParameterSet(
132 
                                                        GetActiveParamSetNumber(),
 141 
                                                        GetActiveParamSetNumber(),
133 
                                                        (unsigned char *) &EE_Parameter.Kanalbelegung[0],
 142 
                                                        (unsigned char *) &EE_Parameter.Kanalbelegung[0],
134 
                                                        STRUCT_PARAM_LAENGE );
 143 
                                                        STRUCT_PARAM_LAENGE );
135 
 
 144 
*/
136 
                                                // Signalisieren
 145 
                                                // Signalisieren
137 
                                                beeptime = 500;
 146 
                                                beeptime = 500;
138 
                                        }
 147 
                                        }
Line 146... Line 155...
146 
 
 155 
 
147 
                                // Begrenzung der Pitch-Beschleunigung am Boden
 156 
                                // Begrenzung der Pitch-Beschleunigung am Boden
Line 148... Line 157...
148 
                                delay = PARAM_PITCH_MD_DELAY0;
 157 
                                delay = PARAM_PITCH_MD_DELAY0;
149 
 
 158 
 
150 
                                if( rawStickValue > PARAM_PITCH_STICK_THRESHOLD ) {
 159 
                                if( rawStickValue > PARAM_PITCH_STICK_THRESHOLD ) {
151 
                                        pitchOffset = PARAM_PITCH_MD_HOVER;
 160 
                                        pitchOffset = pitchHover;
152 
                                } else if( pitchOffset == PARAM_PITCH_MD_HOVER ) {
 161 
                                } else if( pitchOffset == pitchHover ) {
153 
                                        state = STATE_BEGIN1;
 162 
                                        state = STATE_BEGIN1;
Line 154... Line 163...
154 
                                }
 163 
                                }
Line 271... Line 280...
271 
                pitchOffset      = 0;
 280 
                pitchOffset      = 0;
272 
                stickValue       = 0;
 281 
                stickValue       = 0;
273 
                state            = STATE_INITIALIZE;
 282 
                state            = STATE_INITIALIZE;
274 
        }
 283 
        }
Line 275... Line -...
275 
 
 - 
 
276 
        if( pitchOffset < 0 )
 - 
 
277 
                pitchOffset = 0;
 - 
 
278 
 
 284 
 
Line 279... Line 285...
279 
        if( !delayCounter ) {
 285 
        if( !delayCounter ) {
280 
       
 286 
       
281 
                /* Durch die Sollwertvorgabe kann hier eine einstellbare Trägheit auf dem Pitch-Wert
 287 
                /* Durch die Sollwertvorgabe kann hier eine einstellbare Trägheit auf dem Pitch-Wert
Line 292... Line 298...
292 
                delayCounter = delay + 1;
 298 
                delayCounter = delay + 1;
293 
        }
 299 
        }
Line 294... Line 300...
294 
       
 300 
       
Line 295... Line -...
295 
        delayCounter--;
 - 
 
296 
 
 301 
        delayCounter--;
297 
        // Pitch-Wert darf nicht < 0 sein
 302 
 
298 
        if( actualPitchCount < 0 ) {
 - 
 
299 
                actualPitchCount = 0;
 303 
        if( actualPitchCount < 0 )
300 
        }
 304 
                actualPitchCount = 0;
301 
 
 305 
               
302 
        // pitchCount als Debug-Wert rausschreiben
 306 
        // pitchCount als Debug-Wert rausschreiben
303 
        DebugOut.Analog[26] = stickValue;
 307 
        DebugOut.Analog[26] = stickValue;