Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1263 | 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 | |||
| 12 | static char enabled = 0; |
||
| 13 | static int accZOffset = 0; |
||
| 14 | static int temp; // Temporäre Werte; wird mehrfach verwendet |
||
| 15 | |||
| 16 | int altconN = 0; |
||
| 17 | |||
| 18 | extern unsigned char Notlandung; // aus fc.c |
||
| 19 | extern int airPressure; |
||
| 20 | |||
| 21 | |||
| 22 | /* |
||
| 23 | * Höhenregler initialisieren |
||
| 24 | */ |
||
| 25 | void altcon_init( void ) { |
||
| 26 | altcon_stop(); |
||
| 27 | } |
||
| 28 | |||
| 29 | |||
| 30 | /* |
||
| 31 | * Speichert die aktuelle Höhe als Sollhöhe |
||
| 32 | */ |
||
| 33 | void altcon_lock( void ) { |
||
| 34 | SollHoehe = analog_airPressure(); |
||
| 35 | accZOffset = Mess_Integral_Hoch / 128; |
||
| 36 | } |
||
| 37 | |||
| 38 | |||
| 39 | /* |
||
| 40 | * Inkrementiert die aktuelle Höhe um eins |
||
| 41 | */ |
||
| 42 | void altcon_inc( unsigned char count ) { |
||
| 43 | SollHoehe += count; |
||
| 44 | accZOffset = Mess_Integral_Hoch / 128; |
||
| 45 | } |
||
| 46 | |||
| 47 | |||
| 48 | /* |
||
| 49 | * Startet den Höhenregler |
||
| 50 | */ |
||
| 51 | void altcon_start( void ) { |
||
| 52 | enabled = 1; |
||
| 53 | } |
||
| 54 | |||
| 55 | |||
| 56 | /* |
||
| 57 | * Stoppt den Höhenregler |
||
| 58 | */ |
||
| 59 | void altcon_stop( void ) { |
||
| 60 | enabled = 0; |
||
| 61 | } |
||
| 62 | |||
| 63 | |||
| 64 | /* |
||
| 65 | * Berechnet den Korrekturwert für die Höhenregelung |
||
| 66 | */ |
||
| 67 | int altcon_error( void ) { |
||
| 68 | |||
| 69 | int register error; |
||
| 70 | |||
| 71 | DebugOut.Analog[24] = SollHoehe; |
||
| 72 | |||
| 73 | altconN = 0; |
||
| 74 | |||
| 75 | if( enabled && !Notlandung ) { |
||
| 76 | |||
| 77 | // Fehlerwert für Regler ermitteln |
||
| 78 | error = analog_airPressure() - SollHoehe; |
||
| 79 | |||
| 80 | // Proportionalanteil |
||
| 81 | altconN = ( PARAM_ALT_P * error ) / 4; // dividiert durch ( 16 / STICK_GAIN ) = 16 / 4 = 4 |
||
| 82 | |||
| 83 | // Differenzialanteil wird in analog.c berechnet |
||
| 84 | altconN += analog_airPressureDiff() / 2; |
||
| 85 | |||
| 86 | // ACC-Z-Integral zur Dämpfung einbeziehen |
||
| 87 | temp = ( ( ( Mess_Integral_Hoch / 128 ) - accZOffset ) * (signed long) PARAM_ALT_ACC ) / 32; |
||
| 88 | |||
| 89 | // Dämpfung limitieren |
||
| 90 | if( temp > ( 70 * STICK_GAIN ) ) |
||
| 91 | temp = 70 * STICK_GAIN; |
||
| 92 | else if( temp < -( 70 * STICK_GAIN ) ) |
||
| 93 | temp = -( 70 * STICK_GAIN ); |
||
| 94 | |||
| 95 | altconN += temp; |
||
| 96 | |||
| 97 | // Verstärkung des Fehlerwertes zur Anpassung des Gewichtes |
||
| 98 | altconN = altconN * PARAM_ALT_GAIN / 50L; |
||
| 99 | |||
| 100 | // Limitierung des Korrekturwertes nach oben |
||
| 101 | int altMax = PARAM_ALT_MAX * STICK_GAIN; |
||
| 102 | if( altconN < -altMax ) |
||
| 103 | altconN = -altMax; |
||
| 104 | } |
||
| 105 | |||
| 106 | DebugOut.Analog[27] = altconN; |
||
| 107 | |||
| 108 | return altconN; |
||
| 109 | } |