WebSVN - FlightCtrl - Diff - Rev 2008 and 2009



long FromNC_AltitudeSetpoint = 0;
unsigned char FromNC_AltitudeSpeed = 0;
unsigned char carefree_old = 50; // to make the Beep when switching
signed char WaypointTrimming = 0;
int CompassGierSetpoint = 0;
unsigned char CalibrationDone = 0;
int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0;
//float Ki =  FAKTOR_I;
int Ki = 10300 / 33;
unsigned char Looping_Nick = 0,Looping_Roll = 0;
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;

        Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
        Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
    ExternHoehenValue = 0;
    ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
    GierGyroFehler = 0;
 
    LED_Init();
    FC_StatusFlags |= FC_STATUS_CALIBRATE;
    FromNaviCtrl_Value.Kalman_K = -1;
    FromNaviCtrl_Value.Kalman_MaxDrift = 0;
    FromNaviCtrl_Value.Kalman_MaxFusion = 32;

     static long IntegralFehlerNick = 0;
     static long IntegralFehlerRoll = 0;
         static unsigned int RcLostTimer;
         static unsigned char delay_neutral = 0;
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
 
     static char NeueKompassRichtungMerken = 0;
     static long ausgleichNick, ausgleichRoll;
     int IntegralNickMalFaktor,IntegralRollMalFaktor;
         unsigned char i;
         unsigned int HooverGas80Percent;
        Mittelwert();
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gaswert ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        HooverGas80Percent = HoverGas/(STICK_GAIN + STICK_GAIN/4); // 80% of Hovergas
        GasMischanteil = StickGas;
    if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Empfang schlecht
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(SenderOkay < 100 && !(FC_StatusFlags2 & FC_STATUS2_RC_FAILSAVE_ACTIVE))
        {

        if(modell_fliegt > 1000 && Capacity.MinOfMaxPWM > 100)  // wahrscheinlich in der Luft --> langsam absenken
            {
            GasMischanteil = EE_Parameter.NotGas;
                        if(EE_Parameter.GlobalConfig3 & CFG3_VARIO_FAILSAFE)
                         {
                          if(HoverGas && HoverGas < 150 * STICK_GAIN)   GasMischanteil = HooverGas80Percent; // 80% of Hovergas
                         }
            FC_StatusFlags |= FC_STATUS_EMERGENCY_LANDING;
            PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
            PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;

                         SetActiveParamSet(setting);  // aktiven Datensatz merken
                        }
                         if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70)
                          {
                           WinkelOut.CalcState = 1;
                                                   CalibrationDone = 0;
                           beeptime = 1000;
                          }
                          else
                          {
                               ParamSet_ReadFromEEProm(GetActiveParamSet());

                            {
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
                            }
                                                   ServoActive = 0;
                           SetNeutral(0);
                           CalibrationDone = 1;
                                                   ServoActive = 1;
                                                   DDRD  |=0x80; // enable J7 -> Servo signal
                           Piep(GetActiveParamSet(),120);
                         }
                        }

                        {
                        MotorenEin = 0;
                        delay_neutral = 0;
                        modell_fliegt = 0;
                        SetNeutral(1);
                        CalibrationDone = 1;
                        Piep(GetActiveParamSet(),120);
                        }
                    }
                 else delay_neutral = 0;
                }

// Einschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                                                        if(++delay_einschalten > 200)
                                                        {
                                                                delay_einschalten = 0;
                                                                if(!VersionInfo.HardwareError[0] && CalibrationDone && !NC_ErrorCode)
                                                                {
                                                                        modell_fliegt = 1;
                                                                        MotorenEin = 1;
                                                                        sollGier = 0;
                                                                        Mess_Integral_Gier = 0;

        {
                // set undefined state to indicate vario off
                FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN);
        } // EOF no height control

 
    // limit gas to parameter setting
   if(NC_To_FC_Flags & NC_TO_FC_EMERGENCY_LANDING)
        {
         if(GasMischanteil/STICK_GAIN > HooverGas80Percent && HoverGas) GasMischanteil = HooverGas80Percent * STICK_GAIN;
          beeptime = 15000;
          BeepMuster = 0x0E00;
        }
  LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN);

  if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
 

Rev 2008	Rev 2009
Line 105...	Line 105...
105	long FromNC_AltitudeSetpoint = 0;	105	long FromNC_AltitudeSetpoint = 0;
106	unsigned char FromNC_AltitudeSpeed = 0;	106	unsigned char FromNC_AltitudeSpeed = 0;
107	unsigned char carefree_old = 50; // to make the Beep when switching	107	unsigned char carefree_old = 50; // to make the Beep when switching
108	signed char WaypointTrimming = 0;	108	signed char WaypointTrimming = 0;
109	int CompassGierSetpoint = 0;	109	int CompassGierSetpoint = 0;
-		110	unsigned char CalibrationDone = 0;
110	int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0;	111	int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0;
111	//float Ki = FAKTOR_I;	112	//float Ki = FAKTOR_I;
112	int Ki = 10300 / 33;	113	int Ki = 10300 / 33;
113	unsigned char Looping_Nick = 0,Looping_Roll = 0;	114	unsigned char Looping_Nick = 0,Looping_Roll = 0;
114	unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;	115	unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
Line 345...	Line 346...
345	Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;	346	Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
346	Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;	347	Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
347	ExternHoehenValue = 0;	348	ExternHoehenValue = 0;
348	ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;	349	ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
349	GierGyroFehler = 0;	350	GierGyroFehler = 0;
350	SendVersionToNavi = 1;	-
351	LED_Init();	351	LED_Init();
352	FC_StatusFlags \|= FC_STATUS_CALIBRATE;	352	FC_StatusFlags \|= FC_STATUS_CALIBRATE;
353	FromNaviCtrl_Value.Kalman_K = -1;	353	FromNaviCtrl_Value.Kalman_K = -1;
354	FromNaviCtrl_Value.Kalman_MaxDrift = 0;	354	FromNaviCtrl_Value.Kalman_MaxDrift = 0;
355	FromNaviCtrl_Value.Kalman_MaxFusion = 32;	355	FromNaviCtrl_Value.Kalman_MaxFusion = 32;
Line 666...	Line 666...
666	static long IntegralFehlerNick = 0;	666	static long IntegralFehlerNick = 0;
667	static long IntegralFehlerRoll = 0;	667	static long IntegralFehlerRoll = 0;
668	static unsigned int RcLostTimer;	668	static unsigned int RcLostTimer;
669	static unsigned char delay_neutral = 0;	669	static unsigned char delay_neutral = 0;
670	static unsigned char delay_einschalten = 0,delay_ausschalten = 0;	670	static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
671	static unsigned char calibration_done = 0;	-
672	static char NeueKompassRichtungMerken = 0;	671	static char NeueKompassRichtungMerken = 0;
673	static long ausgleichNick, ausgleichRoll;	672	static long ausgleichNick, ausgleichRoll;
674	int IntegralNickMalFaktor,IntegralRollMalFaktor;	673	int IntegralNickMalFaktor,IntegralRollMalFaktor;
675	unsigned char i;	674	unsigned char i;
-		675	unsigned int HooverGas80Percent;
676	Mittelwert();	676	Mittelwert();
677	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	677	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
678	// Gaswert ermitteln	678	// Gaswert ermitteln
679	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	679	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-		680	HooverGas80Percent = HoverGas/(STICK_GAIN + STICK_GAIN/4); // 80% of Hovergas
680	GasMischanteil = StickGas;	681	GasMischanteil = StickGas;
681	if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;	682	if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;
682		-
683	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	683	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
684	// Empfang schlecht	684	// Empfang schlecht
685	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	685	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
686	if(SenderOkay < 100 && !(FC_StatusFlags2 & FC_STATUS2_RC_FAILSAVE_ACTIVE))	686	if(SenderOkay < 100 && !(FC_StatusFlags2 & FC_STATUS2_RC_FAILSAVE_ACTIVE))
687	{	687	{
Line 696...	Line 696...
696	if(modell_fliegt > 1000 && Capacity.MinOfMaxPWM > 100) // wahrscheinlich in der Luft --> langsam absenken	696	if(modell_fliegt > 1000 && Capacity.MinOfMaxPWM > 100) // wahrscheinlich in der Luft --> langsam absenken
697	{	697	{
698	GasMischanteil = EE_Parameter.NotGas;	698	GasMischanteil = EE_Parameter.NotGas;
699	if(EE_Parameter.GlobalConfig3 & CFG3_VARIO_FAILSAFE)	699	if(EE_Parameter.GlobalConfig3 & CFG3_VARIO_FAILSAFE)
700	{	700	{
701	if(HoverGas && HoverGas < 150 * STICK_GAIN) GasMischanteil = HoverGas/(STICK_GAIN + STICK_GAIN/4); // 80% of Hovergas	701	if(HoverGas && HoverGas < 150 * STICK_GAIN) GasMischanteil = HooverGas80Percent; // 80% of Hovergas
702	}	702	}
703	FC_StatusFlags \|= FC_STATUS_EMERGENCY_LANDING;	703	FC_StatusFlags \|= FC_STATUS_EMERGENCY_LANDING;
704	PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;	704	PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
705	PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;	705	PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
706	PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;	706	PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
Line 755...	Line 755...
755	SetActiveParamSet(setting); // aktiven Datensatz merken	755	SetActiveParamSet(setting); // aktiven Datensatz merken
756	}	756	}
757	if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70)	757	if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70)
758	{	758	{
759	WinkelOut.CalcState = 1;	759	WinkelOut.CalcState = 1;
-		760	CalibrationDone = 0;
760	beeptime = 1000;	761	beeptime = 1000;
761	}	762	}
762	else	763	else
763	{	764	{
764	ParamSet_ReadFromEEProm(GetActiveParamSet());	765	ParamSet_ReadFromEEProm(GetActiveParamSet());
Line 768...	Line 769...
768	{	769	{
769	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();	770	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();
770	}	771	}
771	ServoActive = 0;	772	ServoActive = 0;
772	SetNeutral(0);	773	SetNeutral(0);
773	calibration_done = 1;	774	CalibrationDone = 1;
774	ServoActive = 1;	775	ServoActive = 1;
775	DDRD \|=0x80; // enable J7 -> Servo signal	776	DDRD \|=0x80; // enable J7 -> Servo signal
776	Piep(GetActiveParamSet(),120);	777	Piep(GetActiveParamSet(),120);
777	}	778	}
778	}	779	}
Line 784...	Line 785...
784	{	785	{
785	MotorenEin = 0;	786	MotorenEin = 0;
786	delay_neutral = 0;	787	delay_neutral = 0;
787	modell_fliegt = 0;	788	modell_fliegt = 0;
788	SetNeutral(1);	789	SetNeutral(1);
789	calibration_done = 1;	790	CalibrationDone = 1;
790	Piep(GetActiveParamSet(),120);	791	Piep(GetActiveParamSet(),120);
791	}	792	}
792	}	793	}
793	else delay_neutral = 0;	794	else delay_neutral = 0;
794	}	795	}
Line 807...	Line 808...
807	// Einschalten	808	// Einschalten
808	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	809	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
809	if(++delay_einschalten > 200)	810	if(++delay_einschalten > 200)
810	{	811	{
811	delay_einschalten = 0;	812	delay_einschalten = 0;
812	if(!VersionInfo.HardwareError[0] && calibration_done && !NC_ErrorCode)	813	if(!VersionInfo.HardwareError[0] && CalibrationDone && !NC_ErrorCode)
813	{	814	{
814	modell_fliegt = 1;	815	modell_fliegt = 1;
815	MotorenEin = 1;	816	MotorenEin = 1;
816	sollGier = 0;	817	sollGier = 0;
817	Mess_Integral_Gier = 0;	818	Mess_Integral_Gier = 0;
Line 1726...	Line 1727...
1726	{	1727	{
1727	// set undefined state to indicate vario off	1728	// set undefined state to indicate vario off
1728	FC_StatusFlags \|= (FC_STATUS_VARIO_TRIM_UP\|FC_STATUS_VARIO_TRIM_DOWN);	1729	FC_StatusFlags \|= (FC_STATUS_VARIO_TRIM_UP\|FC_STATUS_VARIO_TRIM_DOWN);
1729	} // EOF no height control	1730	} // EOF no height control
Line 1730...	Line 1731...
1730		1731
-		1732	// limit gas to parameter setting
-		1733	if(NC_To_FC_Flags & NC_TO_FC_EMERGENCY_LANDING)
-		1734	{
-		1735	if(GasMischanteil/STICK_GAIN > HooverGas80Percent && HoverGas) GasMischanteil = HooverGas80Percent * STICK_GAIN;
-		1736	beeptime = 15000;
-		1737	BeepMuster = 0x0E00;
1731	// limit gas to parameter setting	1738	}
1732	LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN);	1739	LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN);
Line 1733...	Line 1740...
1733	if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;	1740	if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
1734		1741

Subversion Repositories FlightCtrl

(root)/tags/V2.02a/fc.c @ 2403 - Rev 2008 → 2009