WebSVN - FlightCtrl - Diff - Rev 1139 and 1154 - /branches/thjac/V1


/* 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      lastError       = 0;
static int      lastN           = 0;                            // Zuletzt errechneter Fehlerwert
static long     altIntegral     = 0;
static int      temp;                                           // Temporäre Werte; wird mehrfach verwendet
 
int             pressureOffset  = 0;
int             averageN        = 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 ) {
        pressureOffset = analog_airPressure();
}
 
 
/*
 * Startet den Höhenregler
 */
void altcon_start( void ) {
 
        enabled     = 1;
        lastError   = 0;
        lastN       = 0;
        averageN    = 0;
        altIntegral = 0L;
        accZOffset  = Mess_Integral_Hoch / 128;
       
        // Einschalten der Höhenregelung signalisieren
        beeptime    = 500;
}
 
 
/*
 * Stoppt den Höhenregler
 */
void altcon_stop( void ) {
        enabled = 0;
       
        // Ausschalten der Höhenregelung signalisieren
        beeptime = 500;
}
 
 
/*
 * Berechnet den Korrekturwert für die Höhenregelung
 */
int altcon_error( void ) {
 
        int register    n = 0;
        int register    error;
 
        if( enabled && !Notlandung ) {
 
                // Fehlerwert für Regler ermitteln
                error = analog_airPressure() - pressureOffset;
       
                // Proportionalanteil
                n = ( PARAM_ALT_P * error ) / 4;        // dividiert durch ( 16 / STICK_GAIN ) = 16 / 4 = 4
/*
                // Integralanteil
                altIntegral += ( PARAM_ALT_I * error ) / 4;
               
                // Integral begrenzen
                if( altIntegral > PARAM_ALT_INT_MAX )
                        altIntegral = PARAM_ALT_INT_MAX;
                else if( altIntegral < -PARAM_ALT_INT_MAX )
                        altIntegral = -PARAM_ALT_INT_MAX;
 
                n += altIntegral / 4000;
*/             
                // Differenzialanteil wird in analog.c berechnet
                n += analog_airPressureDiff() / 2;
//              n += ( PARAM_ALT_D * ( error - lastError ) ) / 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 );
 
                n += temp;
 
                // Verstärkung des Fehlerwertes zur Anpassung des Gewichtes
                n = n * PARAM_ALT_GAIN / 10;
               
                int altMax = PARAM_ALT_MAX * STICK_GAIN;
               
                // Limitierung des Korrekturwertes
                if( n > altMax )
                        n = altMax;
                else if( n < -altMax )
                        n = -altMax;
               
                lastN     = n;
                lastError = error;
               
                /* Berechnung einer exponentiellen Glättung für den neuen Gaswert bei Verlassen der
                 * Höhenregelung. Dies soll ein zu heftiges Reagieren mindern. */
                averageN = averageN + PARAM_ALT_EXP_SMOOTHING_FACTOR * ( n - averageN ) / 100;
        }
       
//      DebugOut.Analog[30] = altIntegral / 4000;
        DebugOut.Analog[27] = n;
 
        return n;
}
 

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

Subversion Repositories FlightCtrl

(root)/branches/thjac/V1_11/altcon.c - Rev 1139 → 1154