Subversion Repositories FlightCtrl

Rev

Rev 1139 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1139 Rev 1141
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.h"
 12 
#include "pitch.h"
- 
 
 13 
 
- 
 
 14 
#define UNDEFINED               255
- 
 
 15 
#define INVALID                 255
13 
 
 16 
 
- 
 
 17 
int  pitchInitialStickValue = UNDEFINED;
14 
 
 18 
unsigned char pitchMode;
15 
unsigned char pitchMode;
 19 
 
16 
 
 20 
 
17 
char *pitchModeStrings[] = {
 21 
char *pitchModeStrings[] = {
18 
        "Normal  ",
 22 
        "Normal  ",
19 
        "Neutral ",
 23 
        "Neutral ",
20 
        "MD      "
 24 
        "MD      ",
- 
 
 25 
        "Inaktiv "
21 
};
 26 
};
22 
 
 27 
 
23 
 
 28 
 
24 
// Zeiger auf den durch das Setting bestimmten Pitch-Steuerungsalgorithmus
 29 
// Zeiger auf den durch das Setting bestimmten Pitch-Steuerungsalgorithmus
25 
int (* pitch_value_ptr)( void );
 30 
int (* pitch_value_ptr)( void );
26 
 
 31 
 
27 
// Prototyp
 32 
// Prototyp
28 
int pitch_mk_value( void );
 33 
int pitch_mk_value( void );
29 
 
 34 
 
30 
/*
 35 
/*
31 
 * Führt die Initialisierung der Pitch-Steuerung durch. Diese Funktion
 36 
 * Führt die Initialisierung der Pitch-Steuerung durch. Diese Funktion
32 
 * wird nach jeder Setting-Auswahl sowie nach jeder Setting-Änderung
 37 
 * wird nach jeder Setting-Auswahl sowie nach jeder Setting-Änderung
33 
 * aufgerufen.
 38 
 * aufgerufen.
34 
 */
 39 
 */
35 
void pitch_init( void ) {
 40 
void pitch_init( void ) {
- 
 
 41 
 
- 
 
 42 
        // Nur beim Einschalten ermitteln, da beim Setting-Wechsel ungültig
- 
 
 43 
        if( pitchInitialStickValue == UNDEFINED ) {
- 
 
 44 
       
- 
 
 45 
                // Warten, bis ein gültiger Wert von der Fernsteuerung anliegt
- 
 
 46 
                unsigned int timer = SetDelay( 500 );
- 
 
 47 
                while( !CheckDelay( timer ) );
- 
 
 48 
 
- 
 
 49 
                pitchInitialStickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ];
- 
 
 50 
        }
36 
 
 51 
       
- 
 
 52 
        pitchMode = PARAM_PITCH_MODE;
- 
 
 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 
                // Pitch-Modi mit neutralisiertem Gas
- 
 
 60 
                case PARAM_PITCH_MODE_NEUTRAL:
- 
 
 61 
                case PARAM_PITCH_MODE_MD:
- 
 
 62 
                        if( !( EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG ) )
- 
 
 63 
                                pitchMode = PARAM_PITCH_MODE_INVALID;
- 
 
 64 
                        if( abs( pitchInitialStickValue ) > PARAM_PITCH_STICK_THRESHOLD )
- 
 
 65 
                                pitchMode = PARAM_PITCH_MODE_INVALID;
- 
 
 66 
                        break;
- 
 
 67 
 
- 
 
 68 
                // Pitch-Modi mit 0-Gas
- 
 
 69 
                case PARAM_PITCH_MODE_NORMAL:
- 
 
 70 
                        if( pitchInitialStickValue > 35 - 120 )
- 
 
 71 
                                pitchMode = PARAM_PITCH_MODE_INVALID;
- 
 
 72 
                        break;
- 
 
 73 
        }
37 
        pitchMode = PARAM_PITCH_MODE;
 74 
 
38 
 
 75 
        pitch_neutral_init();
39 
        pitch_md_init();
 76 
        pitch_md_init();
40 
 
 77 
 
41 
        // FIXME Funktioniert noch nicht
 78 
        // FIXME Funktioniert noch nicht
42 
        switch( pitchMode ) {
 79 
        switch( pitchMode ) {
43 
                case PARAM_PITCH_MODE_NEUTRAL:
 80 
                case PARAM_PITCH_MODE_NEUTRAL:
44 
                        pitch_value_ptr = pitch_neutral_value;
 81 
                        pitch_value_ptr = pitch_neutral_value;
45 
                        break;
 82 
                        break;
46 
 
 83 
 
47 
                case PARAM_PITCH_MODE_MD:
 84 
                case PARAM_PITCH_MODE_MD:
48 
                        pitch_value_ptr = pitch_md_value;
 85 
                        pitch_value_ptr = pitch_md_value;
49 
                        break;
 86 
                        break;
50 
 
 87 
 
51 
                default:
 88 
                default:
52 
                        pitch_value_ptr = pitch_mk_value;
 89 
                        pitch_value_ptr = pitch_mk_value;
53 
        }
 90 
        }
54 
       
 91 
       
55 
        // Hier können weitere Initialisierungen folgen
 92 
        // Hier können weitere Initialisierungen folgen
56 
}
 93 
}
57 
 
 94 
 
58 
 
 95 
 
59 
int pitch_value( void ) {
 96 
int pitch_value( void ) {
60 
        switch( pitchMode ) {
 97 
        switch( pitchMode ) {
61 
                case PARAM_PITCH_MODE_NEUTRAL:
 98 
                case PARAM_PITCH_MODE_NEUTRAL:
62 
                        return pitch_neutral_value();
 99 
                        return pitch_neutral_value();
63 
 
 100 
 
64 
                case PARAM_PITCH_MODE_MD:
 101 
                case PARAM_PITCH_MODE_MD:
65 
                        return pitch_md_value();
 102 
                        return pitch_md_value();
66 
 
 103 
 
67 
                default:
 104 
                case PARAM_PITCH_MODE_NORMAL:
- 
 
 105 
                        return pitch_mk_value();
- 
 
 106 
 
- 
 
 107 
                default:
68 
                        return pitch_mk_value();
 108 
                        return 0;
69 
        }
 109 
        }
70 
}
 110 
}
71 
 
 111 
 
72 
 
 112 
 
73 
/*
 113 
/*
74 
 * Führt eine Pitch-Berechnung aus, die der Original-SW entspricht.
 114 
 * Führt eine Pitch-Berechnung aus, die der Original-SW entspricht.
75 
 */
 115 
 */
76 
int pitch_mk_value( void ) {
 116 
int pitch_mk_value( void ) {
77 
        register int stickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ];
 117 
        register int stickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ];
78 
        register int pitchCount = stickValue + 120;     // Warum 120? Gas= 0 ist -125
 118 
        register int pitchCount = stickValue + 120;
79 
       
 119 
       
80 
        DebugOut.Analog[26] = stickValue;
 120 
        DebugOut.Analog[26] = stickValue;
81 
        DebugOut.Analog[28] = pitchCount;
 121 
        DebugOut.Analog[28] = pitchCount;
82 
 
 122 
 
83 
        return pitchCount;
 123 
        return pitchCount;
84 
}
 124 
}
85 
 
 125