WebSVN - FlightCtrl - Diff - Rev 1590 and 1591 - /branches/V0.78f_ACC-HH



unsigned int  modell_fliegt = 0;
volatile unsigned char FCFlags = 0;
long GIER_GRAD_FAKTOR = 1291;
signed int KopplungsteilNickRoll,KopplungsteilRollNick;
signed int tmp_motorwert[MAX_MOTORS];
char VarioCharacter = ' ';
 
#define LIMIT_MIN(value, min) {if(value <= min) value = min;}
#define LIMIT_MAX(value, max) {if(value >= max) value = max;}
#define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;}

 

     static char TimerWerteausgabe = 0;
     static char NeueKompassRichtungMerken = 0;
     static long ausgleichNick, ausgleichRoll;
     int IntegralNickMalFaktor,IntegralRollMalFaktor;
         unsigned char i;
 
        Mittelwert();
    GRN_ON;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gaswert ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

                tmp_int  = (int)(IntegralNick/GIER_GRAD_FAKTOR);  // nick angle in deg
                tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR);  // roll angle in deg
                CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg
                LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle
                CosAttitude = c_cos_8192(CosAttitude);  // cos of actual attitude
                VarioCharacter = ' ';
                if(HoehenReglerAktiv && !(FCFlags & FCFLAG_NOTLANDUNG))
                {
                        #define HEIGHT_TRIM_UP          0x01
                        #define HEIGHT_TRIM_DOWN        0x02
                        static unsigned char HeightTrimmingFlag = 0x00;

                                                HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN;
                                                SollHoehe = HoehenWert; // update setpoint to current heigth
                                        }
                                        HeightTrimmingFlag |= HEIGHT_TRIM_UP;
                                        HeightTrimming += abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD));
                                        VarioCharacter = '+';
                                } // gas stick is below hoover point
                                else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit )
                                {
                                        if(HeightTrimmingFlag & HEIGHT_TRIM_UP)
                                        {
                                                HeightTrimmingFlag &= ~HEIGHT_TRIM_UP;
                                                SollHoehe = HoehenWert; // update setpoint to current heigth
                                        }
                                        HeightTrimmingFlag |= HEIGHT_TRIM_DOWN;
                                        HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD));
                                        VarioCharacter = '-';
                                }
                                else // Gas Stick in Hover Range
                                {
                                        if(HeightTrimmingFlag & (HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN))
                                        {
                                                HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN);
                                                HeightTrimming = 0;
                                                SollHoehe = HoehenWert; // update setpoint to current height
                                                if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;
                                        }
                                        VarioCharacter = '=';
                                }
                                // Trim height set point
                                if(abs(HeightTrimming) > 512)
                                {
                                        SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint

Subversion Repositories FlightCtrl

(root)/branches/V0.78f_ACC-HH_MartinR/fc.c @ 1743 - Rev 1590 → 1591

Rev 1590		Rev 1591
Line 156...		Line 156...
156	unsigned int modell_fliegt = 0;	156	unsigned int modell_fliegt = 0;
157	volatile unsigned char FCFlags = 0;	157	volatile unsigned char FCFlags = 0;
158	long GIER_GRAD_FAKTOR = 1291;	158	long GIER_GRAD_FAKTOR = 1291;
159	signed int KopplungsteilNickRoll,KopplungsteilRollNick;	159	signed int KopplungsteilNickRoll,KopplungsteilRollNick;
160	signed int tmp_motorwert[MAX_MOTORS];	160	signed int tmp_motorwert[MAX_MOTORS];
161	unsigned char LoadHandler = 0;	161	char VarioCharacter = ' ';
-		162
162	#define LIMIT_MIN(value, min) {if(value <= min) value = min;}	163	#define LIMIT_MIN(value, min) {if(value <= min) value = min;}
163	#define LIMIT_MAX(value, max) {if(value >= max) value = max;}	164	#define LIMIT_MAX(value, max) {if(value >= max) value = max;}
164	#define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;}	165	#define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;}
Line 165...		Line 166...
165		166
Line 563...		Line 564...
563	static char TimerWerteausgabe = 0;	564	static char TimerWerteausgabe = 0;
564	static char NeueKompassRichtungMerken = 0;	565	static char NeueKompassRichtungMerken = 0;
565	static long ausgleichNick, ausgleichRoll;	566	static long ausgleichNick, ausgleichRoll;
566	int IntegralNickMalFaktor,IntegralRollMalFaktor;	567	int IntegralNickMalFaktor,IntegralRollMalFaktor;
567	unsigned char i;	568	unsigned char i;
568	if(--LoadHandler == 0) LoadHandler = 5; // verteilt die Prozessorlast	-
569	Mittelwert();	569	Mittelwert();
570	GRN_ON;	570	GRN_ON;
571	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	571	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
572	// Gaswert ermitteln	572	// Gaswert ermitteln
573	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	573	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 1331...		Line 1331...
1331	tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg	1331	tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg
1332	tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg	1332	tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg
1333	CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg	1333	CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg
1334	LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle	1334	LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle
1335	CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude	1335	CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude
-		1336	VarioCharacter = ' ';
1336	if(HoehenReglerAktiv && !(FCFlags & FCFLAG_NOTLANDUNG))	1337	if(HoehenReglerAktiv && !(FCFlags & FCFLAG_NOTLANDUNG))
1337	{	1338	{
1338	#define HEIGHT_TRIM_UP 0x01	1339	#define HEIGHT_TRIM_UP 0x01
1339	#define HEIGHT_TRIM_DOWN 0x02	1340	#define HEIGHT_TRIM_DOWN 0x02
1340	static unsigned char HeightTrimmingFlag = 0x00;	1341	static unsigned char HeightTrimmingFlag = 0x00;
Line 1365...		Line 1366...
1365	HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN;	1366	HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN;
1366	SollHoehe = HoehenWert; // update setpoint to current heigth	1367	SollHoehe = HoehenWert; // update setpoint to current heigth
1367	}	1368	}
1368	HeightTrimmingFlag \|= HEIGHT_TRIM_UP;	1369	HeightTrimmingFlag \|= HEIGHT_TRIM_UP;
1369	HeightTrimming += abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD));	1370	HeightTrimming += abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD));
-		1371	VarioCharacter = '+';
1370	} // gas stick is below hoover point	1372	} // gas stick is below hoover point
1371	else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit )	1373	else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit )
1372	{	1374	{
1373	if(HeightTrimmingFlag & HEIGHT_TRIM_UP)	1375	if(HeightTrimmingFlag & HEIGHT_TRIM_UP)
1374	{	1376	{
1375	HeightTrimmingFlag &= ~HEIGHT_TRIM_UP;	1377	HeightTrimmingFlag &= ~HEIGHT_TRIM_UP;
1376	SollHoehe = HoehenWert; // update setpoint to current heigth	1378	SollHoehe = HoehenWert; // update setpoint to current heigth
1377	}	1379	}
1378	HeightTrimmingFlag \|= HEIGHT_TRIM_DOWN;	1380	HeightTrimmingFlag \|= HEIGHT_TRIM_DOWN;
1379	HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD));	1381	HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD));
-		1382	VarioCharacter = '-';
1380	}	1383	}
1381	else // Gas Stick in Hover Range	1384	else // Gas Stick in Hover Range
1382	{	1385	{
1383	if(HeightTrimmingFlag & (HEIGHT_TRIM_UP \| HEIGHT_TRIM_DOWN))	1386	if(HeightTrimmingFlag & (HEIGHT_TRIM_UP \| HEIGHT_TRIM_DOWN))
1384	{	1387	{
1385	HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP \| HEIGHT_TRIM_DOWN);	1388	HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP \| HEIGHT_TRIM_DOWN);
1386	HeightTrimming = 0;	1389	HeightTrimming = 0;
1387	SollHoehe = HoehenWert; // update setpoint to current height	1390	SollHoehe = HoehenWert; // update setpoint to current height
1388	if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;	1391	if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;
1389	}	1392	}
-		1393	VarioCharacter = '=';
1390	}	1394	}
1391	// Trim height set point	1395	// Trim height set point
1392	if(abs(HeightTrimming) > 512)	1396	if(abs(HeightTrimming) > 512)
1393	{	1397	{
1394	SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint	1398	SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint