Rev 1139 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1134 | 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_neutral.h" |
||
| 11 | #include "pitch_md.h" |
||
| 12 | #include "pitch.h" |
||
| 13 | |||
| 1141 | thjac | 14 | #define UNDEFINED 255 |
| 15 | #define INVALID 255 |
||
| 1134 | thjac | 16 | |
| 1141 | thjac | 17 | int pitchInitialStickValue = UNDEFINED; |
| 1139 | thjac | 18 | unsigned char pitchMode; |
| 19 | |||
| 1141 | thjac | 20 | |
| 1139 | thjac | 21 | char *pitchModeStrings[] = { |
| 22 | "Normal ", |
||
| 23 | "Neutral ", |
||
| 1141 | thjac | 24 | "MD ", |
| 25 | "Inaktiv " |
||
| 1139 | thjac | 26 | }; |
| 27 | |||
| 28 | |||
| 1134 | thjac | 29 | // Zeiger auf den durch das Setting bestimmten Pitch-Steuerungsalgorithmus |
| 30 | int (* pitch_value_ptr)( void ); |
||
| 31 | |||
| 32 | // Prototyp |
||
| 33 | int pitch_mk_value( void ); |
||
| 34 | |||
| 35 | /* |
||
| 36 | * Führt die Initialisierung der Pitch-Steuerung durch. Diese Funktion |
||
| 37 | * wird nach jeder Setting-Auswahl sowie nach jeder Setting-Änderung |
||
| 38 | * aufgerufen. |
||
| 39 | */ |
||
| 40 | void pitch_init( void ) { |
||
| 41 | |||
| 1141 | thjac | 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 | } |
||
| 51 | |||
| 1139 | thjac | 52 | pitchMode = PARAM_PITCH_MODE; |
| 53 | |||
| 1141 | thjac | 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 | } |
||
| 74 | |||
| 75 | pitch_neutral_init(); |
||
| 1139 | thjac | 76 | pitch_md_init(); |
| 77 | |||
| 1134 | thjac | 78 | // FIXME Funktioniert noch nicht |
| 1139 | thjac | 79 | switch( pitchMode ) { |
| 1134 | thjac | 80 | case PARAM_PITCH_MODE_NEUTRAL: |
| 81 | pitch_value_ptr = pitch_neutral_value; |
||
| 82 | break; |
||
| 83 | |||
| 84 | case PARAM_PITCH_MODE_MD: |
||
| 85 | pitch_value_ptr = pitch_md_value; |
||
| 86 | break; |
||
| 87 | |||
| 88 | default: |
||
| 89 | pitch_value_ptr = pitch_mk_value; |
||
| 90 | } |
||
| 91 | |||
| 92 | // Hier können weitere Initialisierungen folgen |
||
| 93 | } |
||
| 94 | |||
| 95 | |||
| 96 | int pitch_value( void ) { |
||
| 1139 | thjac | 97 | switch( pitchMode ) { |
| 1134 | thjac | 98 | case PARAM_PITCH_MODE_NEUTRAL: |
| 99 | return pitch_neutral_value(); |
||
| 100 | |||
| 101 | case PARAM_PITCH_MODE_MD: |
||
| 102 | return pitch_md_value(); |
||
| 103 | |||
| 1141 | thjac | 104 | case PARAM_PITCH_MODE_NORMAL: |
| 105 | return pitch_mk_value(); |
||
| 106 | |||
| 1134 | thjac | 107 | default: |
| 1141 | thjac | 108 | return 0; |
| 1134 | thjac | 109 | } |
| 110 | } |
||
| 111 | |||
| 112 | |||
| 113 | /* |
||
| 114 | * Führt eine Pitch-Berechnung aus, die der Original-SW entspricht. |
||
| 115 | */ |
||
| 116 | int pitch_mk_value( void ) { |
||
| 117 | register int stickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ]; |
||
| 1141 | thjac | 118 | register int pitchCount = stickValue + 120; |
| 1134 | thjac | 119 | |
| 120 | DebugOut.Analog[26] = stickValue; |
||
| 121 | DebugOut.Analog[28] = pitchCount; |
||
| 122 | |||
| 123 | return pitchCount; |
||
| 124 | } |