WebSVN - FlightCtrl - Diff - Rev 2485 and 2486



unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
unsigned char Parameter_HoehenSchalter = 0;      // Wert : 0-250
unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
unsigned char Parameter_Hoehe_TiltCompensation = 100;        // Wert : 0-250
unsigned char Parameter_Gyro_D = 8;             // Wert : 0-250
unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250
unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
unsigned char Parameter_Gyro_Gier_P = 150;      // Wert : 10-250
unsigned char Parameter_Gyro_Gier_I = 150;      // Wert : 10-250

 CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
 CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
 CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
 CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,5,255);
 CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,5,255);
 
 if(EE_Parameter.Servo3 == 247) { if(PORTC & (1<<PORTC2)) Parameter_Servo3 = EE_Parameter.Servo3OnValue; else Parameter_Servo3 = EE_Parameter.Servo3OffValue;}       // Out1 (J16)
 else if(EE_Parameter.Servo3 == 246) { if(PORTC & (1<<PORTC3)) Parameter_Servo3 = EE_Parameter.Servo3OnValue; else Parameter_Servo3 = EE_Parameter.Servo3OffValue;}
 else CHK_POTI_MM(Parameter_Servo3,EE_Parameter.Servo3, 24, 255);

 

 else CHK_POTI_MM(Parameter_Servo4,EE_Parameter.Servo4, 24, 255);

 
 CHK_POTI_MM(Parameter_Servo5,EE_Parameter.Servo5, 24, 255);
 Parameter_HoehenSchalter = GetChannelValue(EE_Parameter.HoeheChannel);
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung);
 CHK_POTI(Parameter_Hoehe_TiltCompensation,EE_Parameter.Hoehe_TiltCompensation);
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung);
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I);
 CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D);
 CHK_POTI(Parameter_Gyro_Gier_P,EE_Parameter.Gyro_Gier_P);

        ROT_ON;
        if(modell_fliegt > 1000 && Capacity.MinOfMaxPWM > 100)  // wahrscheinlich in der Luft --> langsam absenken
            {
            GasMischanteil = HooverGasEmergencyPercent;
            FC_StatusFlags |= FC_STATUS_EMERGENCY_LANDING;
 
 
 
 
 
            }
         else
                    {
                          MotorenEin = 0;
                        }  

                        HoehenReglerAktiv = 1;
                }
                // calculate cos of nick and roll angle used for projection of the vertical hoover gas
                tmp_int  = (int)(IntegralNick/GIER_GRAD_FAKTOR);  // nick angle in deg
                tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR);  // roll angle in deg
                tmp_int  = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg
tmp_int = (tmp_int * Parameter_Hoehe_TiltCompensation) / 100;
                LIMIT_MAX(tmp_int, 60); // limit effective attitude angle
                CosAttitude = c_cos_8192(tmp_int);  // cos of actual attitude
                VarioCharacter = ' ';
                AltitudeSetpointTrimming = 0;
                if(HoehenReglerAktiv && !(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING))
                {
                // Holger original version

                                          if(HeightTrimming > 0)        SollHoehe += EE_Parameter.Hoehe_Verstaerkung / 3;
                                          else                    SollHoehe -= EE_Parameter.Hoehe_Verstaerkung / 3;
                                          }
                                        HeightTrimming = 0;
                                        LIMIT_MIN_MAX(HoehenWertF, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied
                                        LIMIT_MIN_MAX(SollHoehe, (HoehenWertF-1024), (HoehenWertF+1500)); // max. 15m Unterschied
                                        if(Parameter_ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100;
                                        //update hoover gas stick value when setpoint is shifted
                                        if(FromNC_AltitudeSpeed == 0) CalcStickGasHover();
/*
                       if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0)

                         {
                          tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN);
                          LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN);
                          GasReduction += tmp_long;
                         }
/*                      // ------------------------ D-Part 3: GpsZ  ----------------------------------
                        tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L;
            LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN);
                        GasReduction += tmp_int;
*/
            GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value

 
                        // ------------------------                  ----------------------------------
                        HCGas -= GasReduction;

Subversion Repositories FlightCtrl

(root)/tags/V2.14a/fc.c @ 2661 - Rev 2485 → 2486

Rev 2485		Rev 2486
Line 120...		Line 120...
120	unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250	120	unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250
121	unsigned char Parameter_HoehenSchalter = 0; // Wert : 0-250	121	unsigned char Parameter_HoehenSchalter = 0; // Wert : 0-250
122	unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32	122	unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32
123	unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250	123	unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
124	unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250	124	unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250
125	unsigned char Parameter_Hoehe_GPS_Z = 64; // Wert : 0-250	125	unsigned char Parameter_Hoehe_TiltCompensation = 100; // Wert : 0-250
126	unsigned char Parameter_Gyro_D = 8; // Wert : 0-250	126	unsigned char Parameter_Gyro_D = 8; // Wert : 0-250
127	unsigned char Parameter_Gyro_P = 150; // Wert : 10-250	127	unsigned char Parameter_Gyro_P = 150; // Wert : 10-250
128	unsigned char Parameter_Gyro_I = 150; // Wert : 0-250	128	unsigned char Parameter_Gyro_I = 150; // Wert : 0-250
129	unsigned char Parameter_Gyro_Gier_P = 150; // Wert : 10-250	129	unsigned char Parameter_Gyro_Gier_P = 150; // Wert : 10-250
130	unsigned char Parameter_Gyro_Gier_I = 150; // Wert : 10-250	130	unsigned char Parameter_Gyro_Gier_I = 150; // Wert : 10-250
Line 694...		Line 694...
694	CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);	694	CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
695	CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);	695	CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
696	CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);	696	CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
697	CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,5,255);	697	CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,5,255);
698	CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,5,255);	698	CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,5,255);
699		-
700	if(EE_Parameter.Servo3 == 247) { if(PORTC & (1<<PORTC2)) Parameter_Servo3 = EE_Parameter.Servo3OnValue; else Parameter_Servo3 = EE_Parameter.Servo3OffValue;} // Out1 (J16)	699	if(EE_Parameter.Servo3 == 247) { if(PORTC & (1<<PORTC2)) Parameter_Servo3 = EE_Parameter.Servo3OnValue; else Parameter_Servo3 = EE_Parameter.Servo3OffValue;} // Out1 (J16)
701	else if(EE_Parameter.Servo3 == 246) { if(PORTC & (1<<PORTC3)) Parameter_Servo3 = EE_Parameter.Servo3OnValue; else Parameter_Servo3 = EE_Parameter.Servo3OffValue;}	700	else if(EE_Parameter.Servo3 == 246) { if(PORTC & (1<<PORTC3)) Parameter_Servo3 = EE_Parameter.Servo3OnValue; else Parameter_Servo3 = EE_Parameter.Servo3OffValue;}
702	else CHK_POTI_MM(Parameter_Servo3,EE_Parameter.Servo3, 24, 255);	701	else CHK_POTI_MM(Parameter_Servo3,EE_Parameter.Servo3, 24, 255);
Line 703...		Line 702...
703		702
Line 706...		Line 705...
706	else CHK_POTI_MM(Parameter_Servo4,EE_Parameter.Servo4, 24, 255);	705	else CHK_POTI_MM(Parameter_Servo4,EE_Parameter.Servo4, 24, 255);
Line 707...		Line 706...
707		706
708	CHK_POTI_MM(Parameter_Servo5,EE_Parameter.Servo5, 24, 255);	707	CHK_POTI_MM(Parameter_Servo5,EE_Parameter.Servo5, 24, 255);
709	Parameter_HoehenSchalter = GetChannelValue(EE_Parameter.HoeheChannel);	708	Parameter_HoehenSchalter = GetChannelValue(EE_Parameter.HoeheChannel);
710	CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung);	709	CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung);
711	CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z);	710	CHK_POTI(Parameter_Hoehe_TiltCompensation,EE_Parameter.Hoehe_TiltCompensation);
712	CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung);	711	CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung);
713	CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I);	712	CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I);
714	CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D);	713	CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D);
715	CHK_POTI(Parameter_Gyro_Gier_P,EE_Parameter.Gyro_Gier_P);	714	CHK_POTI(Parameter_Gyro_Gier_P,EE_Parameter.Gyro_Gier_P);
Line 842...		Line 841...
842	ROT_ON;	841	ROT_ON;
843	if(modell_fliegt > 1000 && Capacity.MinOfMaxPWM > 100) // wahrscheinlich in der Luft --> langsam absenken	842	if(modell_fliegt > 1000 && Capacity.MinOfMaxPWM > 100) // wahrscheinlich in der Luft --> langsam absenken
844	{	843	{
845	GasMischanteil = HooverGasEmergencyPercent;	844	GasMischanteil = HooverGasEmergencyPercent;
846	FC_StatusFlags \|= FC_STATUS_EMERGENCY_LANDING;	845	FC_StatusFlags \|= FC_STATUS_EMERGENCY_LANDING;
847	PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;	-
848	PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;	-
849	PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;	-
850	PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;	-
851	PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;	-
852	}	846	}
853	else	847	else
854	{	848	{
855	MotorenEin = 0;	849	MotorenEin = 0;
856	}	850	}
Line 1812...		Line 1806...
1812	HoehenReglerAktiv = 1;	1806	HoehenReglerAktiv = 1;
1813	}	1807	}
1814	// calculate cos of nick and roll angle used for projection of the vertical hoover gas	1808	// calculate cos of nick and roll angle used for projection of the vertical hoover gas
1815	tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg	1809	tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg
1816	tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg	1810	tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg
1817	CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg	1811	tmp_int = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg
-		1812	tmp_int = (tmp_int * Parameter_Hoehe_TiltCompensation) / 100;
1818	LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle	1813	LIMIT_MAX(tmp_int, 60); // limit effective attitude angle
1819	CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude	1814	CosAttitude = c_cos_8192(tmp_int); // cos of actual attitude
1820	VarioCharacter = ' ';	1815	VarioCharacter = ' ';
1821	AltitudeSetpointTrimming = 0;	1816	AltitudeSetpointTrimming = 0;
1822	if(HoehenReglerAktiv && !(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING))	1817	if(HoehenReglerAktiv && !(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING))
1823	{	1818	{
1824	// Holger original version	1819	// Holger original version
Line 1939...		Line 1934...
1939	if(HeightTrimming > 0) SollHoehe += EE_Parameter.Hoehe_Verstaerkung / 3;	1934	if(HeightTrimming > 0) SollHoehe += EE_Parameter.Hoehe_Verstaerkung / 3;
1940	else SollHoehe -= EE_Parameter.Hoehe_Verstaerkung / 3;	1935	else SollHoehe -= EE_Parameter.Hoehe_Verstaerkung / 3;
1941	}	1936	}
1942	HeightTrimming = 0;	1937	HeightTrimming = 0;
1943	LIMIT_MIN_MAX(HoehenWertF, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied	1938	LIMIT_MIN_MAX(HoehenWertF, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied
1944	LIMIT_MIN_MAX(SollHoehe, (HoehenWertF-1024), (HoehenWertF+1024)); // max. 10m Unterschied	1939	LIMIT_MIN_MAX(SollHoehe, (HoehenWertF-1024), (HoehenWertF+1500)); // max. 15m Unterschied
1945	if(Parameter_ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100;	1940	if(Parameter_ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100;
1946	//update hoover gas stick value when setpoint is shifted	1941	//update hoover gas stick value when setpoint is shifted
1947	if(FromNC_AltitudeSpeed == 0) CalcStickGasHover();	1942	if(FromNC_AltitudeSpeed == 0) CalcStickGasHover();
1948	/*	1943	/*
1949	if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0)	1944	if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0)
Line 2005...		Line 2000...
2005	{	2000	{
2006	tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN);	2001	tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN);
2007	LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN);	2002	LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN);
2008	GasReduction += tmp_long;	2003	GasReduction += tmp_long;
2009	}	2004	}
2010	// ------------------------ D-Part 3: GpsZ ----------------------------------	2005	/* // ------------------------ D-Part 3: GpsZ ----------------------------------
2011	tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L;	2006	tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L;
2012	LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN);	2007	LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN);
2013	GasReduction += tmp_int;	2008	GasReduction += tmp_int;
-		2009	*/
2014	GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value	2010	GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value
Line 2015...		Line 2011...
2015		2011
2016	// ------------------------ ----------------------------------	2012	// ------------------------ ----------------------------------
2017	HCGas -= GasReduction;	2013	HCGas -= GasReduction;