Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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