WebSVN - FlightCtrl - Diff - Rev 2090 and 2146



//############################################################################
{
        unsigned char i;
        unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;
    VersionInfo.HardwareError[0] = 0;
//    HEF4017Reset_ON;
        NeutralAccX = 0;
        NeutralAccY = 0;
        NeutralAccZ = 0;

 

     Poti[i] =  PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 127;
        }
    SenderOkay = 100;
    if(ServoActive)
         {
//              HEF4017Reset_ON;
                DDRD  |=0x80; // enable J7 -> Servo signal
     }

 
        if((AdNeutralNick < 150 * 16) || (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; };

     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 signed char move_safety_switch = 0;
     static long ausgleichNick, ausgleichRoll;
     int IntegralNickMalFaktor,IntegralRollMalFaktor;
         unsigned char i;
        Mittelwert();
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

                                                   LIBFC_ReceiverInit(EE_Parameter.Receiver);
                           if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
                            {
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
                            }
//                                                 ServoActive = 0;
                           SetNeutral(0);
                           CalibrationDone = 1;
                                                   ServoActive = 1;
                                                   DDRD  |=0x80; // enable J7 -> Servo signal
                           Piep(GetActiveParamSet(),120);

                 else delay_neutral = 0;
                }
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gas ist unten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
            if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)
                {
                                        if(PPM_diff[EE_Parameter.MotorSafetySwitch & 127] > 5) move_safety_switch = 100;
                                        else
                                        if(PPM_diff[EE_Parameter.MotorSafetySwitch & 127] < -5) move_safety_switch = -100;
                                        // Motoren Starten
                                        if(!MotorenEin)
                        {
                                                if((((PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) && ((!(EE_Parameter.GlobalConfig3 & CFG3_MOTOR_SWITCH_MODE) && PPM_in[EE_Parameter.MotorSafetySwitch] < -75) || EE_Parameter.MotorSafetySwitch == 0)))
                                                || (((EE_Parameter.GlobalConfig3 & CFG3_MOTOR_SWITCH_MODE) && PPM_in[EE_Parameter.MotorSafetySwitch] > -10 && move_safety_switch == 100)))
                                                {
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Einschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                                                        if(CalibrationDone) FC_StatusFlags |= FC_STATUS_START;
                                                        if(++delay_einschalten > 253)
                                                        {
                                                                delay_einschalten = 0;
                                                                if(!VersionInfo.HardwareError[0] && CalibrationDone && !NC_ErrorCode)
                                                                {
                                                                        modell_fliegt = 1;

                                                                        Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
                                                                        Mess_IntegralNick2 = IntegralNick;
                                                                        Mess_IntegralRoll2 = IntegralRoll;
                                                                        SummeNick = 0;
                                                                        SummeRoll = 0;
 
//                                                                      ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
                                                                        NeueKompassRichtungMerken = 100; // 2 sekunden
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
                                                                        SpeakHoTT = SPEAK_STARTING;
#endif
                                                                }
                                                                else
                                                                {
                                                                        beeptime = 1500; // indicate missing calibration
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
                                                                        if(!CalibrationDone) SpeakHoTT = SPEAK_ERR_CALIBARTION;
#endif
                                                                }
                                                        }
                                                }
                                                else delay_einschalten = 0;

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Auschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                                        else // only if motors are running
                                        {
//                                              if((PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) && (PPM_in[EE_Parameter.MotorSafetySwitch] < -75 || EE_Parameter.MotorSafetySwitch == 0))
                                                if((((PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) && ((!(EE_Parameter.GlobalConfig3 & CFG3_MOTOR_SWITCH_MODE) && PPM_in[EE_Parameter.MotorSafetySwitch] < -75) || EE_Parameter.MotorSafetySwitch == 0)))
                                                || (((EE_Parameter.GlobalConfig3 & CFG3_MOTOR_SWITCH_MODE) && PPM_in[EE_Parameter.MotorSafetySwitch] < -50 && move_safety_switch == -100)))
                                                {
                                                        if(++delay_ausschalten > 250)  // nicht sofort
                                                        {
                                                                MotorenEin = 0;
                                                                delay_ausschalten = 0;
                                                                modell_fliegt = 0;
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))

                                                        }
                                                }
                                                else delay_ausschalten = 0;
                                        }
                }
                                else // gas not at minimum
                                move_safety_switch = 0;
            }
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// neue Werte von der Funke
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Rev 2090	Rev 2146
Line 262...	Line 262...
262	//############################################################################	262	//############################################################################
263	{	263	{
264	unsigned char i;	264	unsigned char i;
265	unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;	265	unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;
266	VersionInfo.HardwareError[0] = 0;	266	VersionInfo.HardwareError[0] = 0;
267	HEF4017R_ON;	267	// HEF4017Reset_ON;
268	NeutralAccX = 0;	268	NeutralAccX = 0;
269	NeutralAccY = 0;	269	NeutralAccY = 0;
270	NeutralAccZ = 0;	270	NeutralAccZ = 0;
Line 271...	Line 271...
271		271
Line 361...	Line 361...
361	Poti[i] = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 127;	361	Poti[i] = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 127;
362	}	362	}
363	SenderOkay = 100;	363	SenderOkay = 100;
364	if(ServoActive)	364	if(ServoActive)
365	{	365	{
366	HEF4017R_ON;	366	// HEF4017Reset_ON;
367	DDRD \|=0x80; // enable J7 -> Servo signal	367	DDRD \|=0x80; // enable J7 -> Servo signal
368	}	368	}
Line 369...	Line 369...
369		369
370	if((AdNeutralNick < 150 * 16) \|\| (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] \|= FC_ERROR0_GYRO_NICK; };	370	if((AdNeutralNick < 150 * 16) \|\| (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] \|= FC_ERROR0_GYRO_NICK; };
Line 683...	Line 683...
683	static long IntegralFehlerNick = 0;	683	static long IntegralFehlerNick = 0;
684	static long IntegralFehlerRoll = 0;	684	static long IntegralFehlerRoll = 0;
685	static unsigned int RcLostTimer;	685	static unsigned int RcLostTimer;
686	static unsigned char delay_neutral = 0;	686	static unsigned char delay_neutral = 0;
687	static unsigned char delay_einschalten = 0,delay_ausschalten = 0;	687	static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
-		688	static signed char move_safety_switch = 0;
688	static long ausgleichNick, ausgleichRoll;	689	static long ausgleichNick, ausgleichRoll;
689	int IntegralNickMalFaktor,IntegralRollMalFaktor;	690	int IntegralNickMalFaktor,IntegralRollMalFaktor;
690	unsigned char i;	691	unsigned char i;
691	Mittelwert();	692	Mittelwert();
692	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	693	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 788...	Line 789...
788	LIBFC_ReceiverInit(EE_Parameter.Receiver);	789	LIBFC_ReceiverInit(EE_Parameter.Receiver);
789	if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?	790	if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
790	{	791	{
791	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();	792	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();
792	}	793	}
793	ServoActive = 0;	794	// ServoActive = 0;
794	SetNeutral(0);	795	SetNeutral(0);
795	CalibrationDone = 1;	796	CalibrationDone = 1;
796	ServoActive = 1;	797	ServoActive = 1;
797	DDRD \|=0x80; // enable J7 -> Servo signal	798	DDRD \|=0x80; // enable J7 -> Servo signal
798	Piep(GetActiveParamSet(),120);	799	Piep(GetActiveParamSet(),120);
Line 815...	Line 816...
815	else delay_neutral = 0;	816	else delay_neutral = 0;
816	}	817	}
817	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	818	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
818	// Gas ist unten	819	// Gas ist unten
819	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	820	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
820		-
821	if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)	821	if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)
822	{	822	{
-		823	if(PPM_diff[EE_Parameter.MotorSafetySwitch & 127] > 5) move_safety_switch = 100;
-		824	else
-		825	if(PPM_diff[EE_Parameter.MotorSafetySwitch & 127] < -5) move_safety_switch = -100;
823	// Motoren Starten	826	// Motoren Starten
824	if(!MotorenEin)	827	if(!MotorenEin)
825	{	828	{
826	if((PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) && (PPM_in[EE_Parameter.MotorSafetySwitch] < -75 \|\| EE_Parameter.MotorSafetySwitch == 0))	829	if((((PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) && ((!(EE_Parameter.GlobalConfig3 & CFG3_MOTOR_SWITCH_MODE) && PPM_in[EE_Parameter.MotorSafetySwitch] < -75) \|\| EE_Parameter.MotorSafetySwitch == 0)))
-		830	\|\| (((EE_Parameter.GlobalConfig3 & CFG3_MOTOR_SWITCH_MODE) && PPM_in[EE_Parameter.MotorSafetySwitch] > -10 && move_safety_switch == 100)))
827	{	831	{
828	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	832	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
829	// Einschalten	833	// Einschalten
830	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	834	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-		835	if(CalibrationDone) FC_StatusFlags \|= FC_STATUS_START;
831	if(++delay_einschalten > 200)	836	if(++delay_einschalten > 253)
832	{	837	{
833	delay_einschalten = 0;	838	delay_einschalten = 0;
834	if(!VersionInfo.HardwareError[0] && CalibrationDone && !NC_ErrorCode)	839	if(!VersionInfo.HardwareError[0] && CalibrationDone && !NC_ErrorCode)
835	{	840	{
836	modell_fliegt = 1;	841	modell_fliegt = 1;
Line 842...	Line 847...
842	Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;	847	Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
843	Mess_IntegralNick2 = IntegralNick;	848	Mess_IntegralNick2 = IntegralNick;
844	Mess_IntegralRoll2 = IntegralRoll;	849	Mess_IntegralRoll2 = IntegralRoll;
845	SummeNick = 0;	850	SummeNick = 0;
846	SummeRoll = 0;	851	SummeRoll = 0;
847	FC_StatusFlags \|= FC_STATUS_START;	-
848	// ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;	852	// ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
849	NeueKompassRichtungMerken = 100; // 2 sekunden	853	NeueKompassRichtungMerken = 100; // 2 sekunden
850	#if (defined(__AVR_ATmega1284__) \|\| defined(__AVR_ATmega1284P__))	854	#if (defined(__AVR_ATmega1284__) \|\| defined(__AVR_ATmega1284P__))
851	SpeakHoTT = SPEAK_STARTING;	855	SpeakHoTT = SPEAK_STARTING;
852	#endif	856	#endif
853	}	857	}
854	else	858	else
855	{	859	{
856	beeptime = 1500; // indicate missing calibration	860	beeptime = 1500; // indicate missing calibration
857	#if (defined(__AVR_ATmega1284__) \|\| defined(__AVR_ATmega1284P__))	861	#if (defined(__AVR_ATmega1284__) \|\| defined(__AVR_ATmega1284P__))
858	SpeakHoTT = SPEAK_ERR_CALIBARTION;	862	if(!CalibrationDone) SpeakHoTT = SPEAK_ERR_CALIBARTION;
859	#endif	863	#endif
860	}	864	}
861	}	865	}
862	}	866	}
863	else delay_einschalten = 0;	867	else delay_einschalten = 0;
Line 865...	Line 869...
865	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	869	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
866	// Auschalten	870	// Auschalten
867	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	871	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
868	else // only if motors are running	872	else // only if motors are running
869	{	873	{
870	if((PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) && (PPM_in[EE_Parameter.MotorSafetySwitch] < -75 \|\| EE_Parameter.MotorSafetySwitch == 0))	874	// if((PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) && (PPM_in[EE_Parameter.MotorSafetySwitch] < -75 \|\| EE_Parameter.MotorSafetySwitch == 0))
-		875	if((((PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) && ((!(EE_Parameter.GlobalConfig3 & CFG3_MOTOR_SWITCH_MODE) && PPM_in[EE_Parameter.MotorSafetySwitch] < -75) \|\| EE_Parameter.MotorSafetySwitch == 0)))
-		876	\|\| (((EE_Parameter.GlobalConfig3 & CFG3_MOTOR_SWITCH_MODE) && PPM_in[EE_Parameter.MotorSafetySwitch] < -50 && move_safety_switch == -100)))
871	{	877	{
872	if(++delay_ausschalten > 200) // nicht sofort	878	if(++delay_ausschalten > 250) // nicht sofort
873	{	879	{
874	MotorenEin = 0;	880	MotorenEin = 0;
875	delay_ausschalten = 0;	881	delay_ausschalten = 0;
876	modell_fliegt = 0;	882	modell_fliegt = 0;
877	#if (defined(__AVR_ATmega1284__) \|\| defined(__AVR_ATmega1284P__))	883	#if (defined(__AVR_ATmega1284__) \|\| defined(__AVR_ATmega1284P__))
Line 880...	Line 886...
880	}	886	}
881	}	887	}
882	else delay_ausschalten = 0;	888	else delay_ausschalten = 0;
883	}	889	}
884	}	890	}
-		891	else // gas not at minimum
-		892	move_safety_switch = 0;
885	}	893	}
886	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	894	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
887	// neue Werte von der Funke	895	// neue Werte von der Funke
888	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	896	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Subversion Repositories FlightCtrl

(root)/tags/V0.88n/fc.c @ 2440 - Rev 2090 → 2146