Rev 1139 | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1134 | thjac | 1 | /* altcon.c |
| 2 | * |
||
| 3 | * Copyright 2009 Thomas Jachmann |
||
| 4 | */ |
||
| 5 | |||
| 6 | #include "main.h" |
||
| 7 | #include "altcon.h" |
||
| 8 | #include "parameter.h" |
||
| 9 | #include "fc.h" |
||
| 10 | |||
| 11 | |||
| 1139 | thjac | 12 | static char enabled = 0; |
| 13 | static int accZOffset = 0; |
||
| 14 | static int lastError = 0; |
||
| 15 | static int lastN = 0; // Zuletzt errechneter Fehlerwert |
||
| 16 | static long altIntegral = 0; |
||
| 17 | static int temp; // Temporäre Werte; wird mehrfach verwendet |
||
| 1134 | thjac | 18 | |
| 1139 | thjac | 19 | int pressureOffset = 0; |
| 20 | int averageN = 0; |
||
| 21 | |||
| 22 | extern unsigned char Notlandung; // aus fc.c |
||
| 1134 | thjac | 23 | extern int airPressure; |
| 24 | |||
| 25 | |||
| 26 | /* |
||
| 27 | * Höhenregler initialisieren |
||
| 28 | */ |
||
| 29 | void altcon_init( void ) { |
||
| 30 | altcon_stop(); |
||
| 31 | } |
||
| 32 | |||
| 33 | |||
| 34 | /* |
||
| 35 | * Speichert die aktuelle Höhe als Sollhöhe |
||
| 36 | */ |
||
| 37 | void altcon_lock( void ) { |
||
| 1154 | thjac | 38 | pressureOffset = analog_airPressure(); |
| 1134 | thjac | 39 | } |
| 40 | |||
| 41 | |||
| 42 | /* |
||
| 43 | * Startet den Höhenregler |
||
| 44 | */ |
||
| 45 | void altcon_start( void ) { |
||
| 46 | |||
| 1139 | thjac | 47 | enabled = 1; |
| 1134 | thjac | 48 | lastError = 0; |
| 49 | lastN = 0; |
||
| 50 | averageN = 0; |
||
| 51 | altIntegral = 0L; |
||
| 1139 | thjac | 52 | accZOffset = Mess_Integral_Hoch / 128; |
| 1134 | thjac | 53 | |
| 54 | // Einschalten der Höhenregelung signalisieren |
||
| 1139 | thjac | 55 | beeptime = 500; |
| 1134 | thjac | 56 | } |
| 57 | |||
| 58 | |||
| 59 | /* |
||
| 60 | * Stoppt den Höhenregler |
||
| 61 | */ |
||
| 62 | void altcon_stop( void ) { |
||
| 63 | enabled = 0; |
||
| 64 | |||
| 65 | // Ausschalten der Höhenregelung signalisieren |
||
| 1139 | thjac | 66 | beeptime = 500; |
| 1134 | thjac | 67 | } |
| 68 | |||
| 69 | |||
| 70 | /* |
||
| 71 | * Berechnet den Korrekturwert für die Höhenregelung |
||
| 72 | */ |
||
| 73 | int altcon_error( void ) { |
||
| 74 | |||
| 75 | int register n = 0; |
||
| 76 | int register error; |
||
| 77 | |||
| 78 | if( enabled && !Notlandung ) { |
||
| 79 | |||
| 80 | // Fehlerwert für Regler ermitteln |
||
| 1154 | thjac | 81 | error = analog_airPressure() - pressureOffset; |
| 1134 | thjac | 82 | |
| 83 | // Proportionalanteil |
||
| 84 | n = ( PARAM_ALT_P * error ) / 4; // dividiert durch ( 16 / STICK_GAIN ) = 16 / 4 = 4 |
||
| 1139 | thjac | 85 | /* |
| 1134 | thjac | 86 | // Integralanteil |
| 87 | altIntegral += ( PARAM_ALT_I * error ) / 4; |
||
| 88 | |||
| 89 | // Integral begrenzen |
||
| 90 | if( altIntegral > PARAM_ALT_INT_MAX ) |
||
| 91 | altIntegral = PARAM_ALT_INT_MAX; |
||
| 92 | else if( altIntegral < -PARAM_ALT_INT_MAX ) |
||
| 93 | altIntegral = -PARAM_ALT_INT_MAX; |
||
| 1139 | thjac | 94 | |
| 1134 | thjac | 95 | n += altIntegral / 4000; |
| 1139 | thjac | 96 | */ |
| 1154 | thjac | 97 | // Differenzialanteil wird in analog.c berechnet |
| 98 | n += analog_airPressureDiff() / 2; |
||
| 99 | // n += ( PARAM_ALT_D * ( error - lastError ) ) / 2; |
||
| 1134 | thjac | 100 | |
| 101 | // ACC-Z-Integral zur Dämpfung einbeziehen |
||
| 102 | temp = ( ( ( Mess_Integral_Hoch / 128 ) - accZOffset ) * (signed long) PARAM_ALT_ACC ) / 32; |
||
| 103 | |||
| 104 | // Dämpfung limitieren |
||
| 105 | if( temp > ( 70 * STICK_GAIN ) ) |
||
| 106 | temp = 70 * STICK_GAIN; |
||
| 107 | else if( temp < -( 70 * STICK_GAIN ) ) |
||
| 108 | temp = -( 70 * STICK_GAIN ); |
||
| 109 | |||
| 110 | n += temp; |
||
| 111 | |||
| 112 | // Verstärkung des Fehlerwertes zur Anpassung des Gewichtes |
||
| 113 | n = n * PARAM_ALT_GAIN / 10; |
||
| 114 | |||
| 115 | int altMax = PARAM_ALT_MAX * STICK_GAIN; |
||
| 116 | |||
| 117 | // Limitierung des Korrekturwertes |
||
| 118 | if( n > altMax ) |
||
| 119 | n = altMax; |
||
| 120 | else if( n < -altMax ) |
||
| 121 | n = -altMax; |
||
| 122 | |||
| 123 | lastN = n; |
||
| 124 | lastError = error; |
||
| 125 | |||
| 126 | /* Berechnung einer exponentiellen Glättung für den neuen Gaswert bei Verlassen der |
||
| 127 | * Höhenregelung. Dies soll ein zu heftiges Reagieren mindern. */ |
||
| 128 | averageN = averageN + PARAM_ALT_EXP_SMOOTHING_FACTOR * ( n - averageN ) / 100; |
||
| 129 | } |
||
| 130 | |||
| 1139 | thjac | 131 | // DebugOut.Analog[30] = altIntegral / 4000; |
| 1134 | thjac | 132 | DebugOut.Analog[27] = n; |
| 133 | |||
| 134 | return n; |
||
| 135 | } |