| 0,0 → 1,109 |
|---|
| /* altcon.c |
| * |
| * Copyright 2009 Thomas Jachmann |
| */ |
| |
| #include "main.h" |
| #include "altcon.h" |
| #include "parameter.h" |
| #include "fc.h" |
| |
| |
| static char enabled = 0; |
| static int accZOffset = 0; |
| static int temp; // Temporäre Werte; wird mehrfach verwendet |
| |
| int altconN = 0; |
| |
| extern unsigned char Notlandung; // aus fc.c |
| extern int airPressure; |
| |
| |
| /* |
| * Höhenregler initialisieren |
| */ |
| void altcon_init( void ) { |
| altcon_stop(); |
| } |
| |
| |
| /* |
| * Speichert die aktuelle Höhe als Sollhöhe |
| */ |
| void altcon_lock( void ) { |
| SollHoehe = analog_airPressure(); |
| accZOffset = Mess_Integral_Hoch / 128; |
| } |
| |
| |
| /* |
| * Inkrementiert die aktuelle Höhe um eins |
| */ |
| void altcon_inc( unsigned char count ) { |
| SollHoehe += count; |
| accZOffset = Mess_Integral_Hoch / 128; |
| } |
| |
| |
| /* |
| * Startet den Höhenregler |
| */ |
| void altcon_start( void ) { |
| enabled = 1; |
| } |
| |
| |
| /* |
| * Stoppt den Höhenregler |
| */ |
| void altcon_stop( void ) { |
| enabled = 0; |
| } |
| |
| |
| /* |
| * Berechnet den Korrekturwert für die Höhenregelung |
| */ |
| int altcon_error( void ) { |
| |
| int register error; |
| |
| DebugOut.Analog[24] = SollHoehe; |
| |
| altconN = 0; |
| |
| if( enabled && !Notlandung ) { |
| |
| // Fehlerwert für Regler ermitteln |
| error = analog_airPressure() - SollHoehe; |
| |
| // Proportionalanteil |
| altconN = ( PARAM_ALT_P * error ) / 4; // dividiert durch ( 16 / STICK_GAIN ) = 16 / 4 = 4 |
| |
| // Differenzialanteil wird in analog.c berechnet |
| altconN += analog_airPressureDiff() / 2; |
| |
| // ACC-Z-Integral zur Dämpfung einbeziehen |
| temp = ( ( ( Mess_Integral_Hoch / 128 ) - accZOffset ) * (signed long) PARAM_ALT_ACC ) / 32; |
| |
| // Dämpfung limitieren |
| if( temp > ( 70 * STICK_GAIN ) ) |
| temp = 70 * STICK_GAIN; |
| else if( temp < -( 70 * STICK_GAIN ) ) |
| temp = -( 70 * STICK_GAIN ); |
| |
| altconN += temp; |
| |
| // Verstärkung des Fehlerwertes zur Anpassung des Gewichtes |
| altconN = altconN * PARAM_ALT_GAIN / 50L; |
| |
| // Limitierung des Korrekturwertes nach oben |
| int altMax = PARAM_ALT_MAX * STICK_GAIN; |
| if( altconN < -altMax ) |
| altconN = -altMax; |
| } |
| |
| DebugOut.Analog[27] = altconN; |
| |
| return altconN; |
| } |