WebSVN - FlightCtrl - Diff - Rev 1744 and 1760 - /branches/V2.01a_ACC-HH



unsigned char Parameter_NaviWindCorrection;
unsigned char Parameter_NaviSpeedCompensation;
unsigned char Parameter_ExternalControl;
unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;
unsigned char CareFree = 0;
const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8};

 
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
int MaxStickNick = 0,MaxStickRoll = 0;
unsigned int  modell_fliegt = 0;

//    DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
        DebugOut.Analog[29] = Capacity.MinOfMaxPWM;
    DebugOut.Analog[30] = GPS_Nick;
    DebugOut.Analog[31] = GPS_Roll;
    if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe;
 
if(Capacity.MinOfMaxPWM < 250/* && modell_fliegt > 500*/)  { beeptime = 1000; DebugOut.Analog[25]++; }
 
}

 
 
 
void Piep(unsigned char Anzahl, unsigned int dauer)
{
 if(MotorenEin) return; //auf keinen Fall im Flug!
 while(Anzahl--)

                  {
                   if(!PC_MotortestActive)  MotorTest[i] = 0;
                   Motor[i].SetPoint = MotorTest[i];
                   Motor[i].SetPointLowerBits = 0;
/*
 Motor[i].SetPoint = MotorTest[i] / 4;            // testing the high resolution
 Motor[i].SetPointLowerBits = MotorTest[i] % 4;
*/
                  }
          if(PC_MotortestActive) PC_MotortestActive--;
        }
        else FCFlags |= FCFLAG_MOTOR_RUN;

 
    if(I2C_TransferActive)
 
         {
          I2C_TransferActive = 0; // enable for the next time
         }
        else  
    {
 
     motor_write = 0;
     I2C_Start(TWI_STATE_MOTOR_TX); //Start I2C Interrupt Mode
        }

                 else delay_neutral = 0;
                }
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gas ist unten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DebugOut.Analog[16] = EE_Parameter.MotorSafetySwitch;
 
            if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)
                {
                                        // Motoren Starten
                                        if(!MotorenEin)
                        {
                                                if((PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) && (PPM_in[EE_Parameter.MotorSafetySwitch] < -75 || EE_Parameter.MotorSafetySwitch == 0))
                                                {
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Einschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                                                        if(++delay_einschalten > 200)

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 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(++delay_ausschalten > 200)  // nicht sofort
                                                        {
                                                                MotorenEin = 0;
                                                                delay_ausschalten = 0;

    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;

 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// CareFree und freie Wahl der vorderen Richtung
 
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(CareFree)
 {
    signed int nick, roll;
        nick = stick_nick / 4;

        FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle];
    StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8;
    StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8;
 }

 
 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
        if(StickGier > 2) StickGier -= 2;       else

        if(StickGier < -2) StickGier += 2; else StickGier = 0;

                        else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;

 
                        if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2;      // MotorSmoothing

                        else tmp_int = 2 * tmp_int - tmp_motorwert[i];                                                              // MotorSmoothing
 
                        LIMIT_MIN_MAX(tmp_int,(int) MIN_GAS * 4,(int) MAX_GAS * 4);
                        Motor[i].SetPoint = tmp_int / 4;
                        Motor[i].SetPointLowerBits = (tmp_int % 4)<<1; // (3 bits total)
            tmp_motorwert[i] = tmp_int;
                }
                else
                {
                        Motor[i].SetPoint = 0;
                        Motor[i].SetPointLowerBits = 0;
 
                }

Rev 1744	Rev 1760
Line 147...	Line 147...
147	unsigned char Parameter_NaviWindCorrection;	147	unsigned char Parameter_NaviWindCorrection;
148	unsigned char Parameter_NaviSpeedCompensation;	148	unsigned char Parameter_NaviSpeedCompensation;
149	unsigned char Parameter_ExternalControl;	149	unsigned char Parameter_ExternalControl;
150	unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;	150	unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;
151	unsigned char CareFree = 0;	151	unsigned char CareFree = 0;
-		152	const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8};
Line 152...	Line 153...
152		153
153	signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;	154	signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
154	int MaxStickNick = 0,MaxStickRoll = 0;	155	int MaxStickNick = 0,MaxStickRoll = 0;
155	unsigned int modell_fliegt = 0;	156	unsigned int modell_fliegt = 0;
Line 191...	Line 192...
191	// DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;	192	// DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
192	DebugOut.Analog[29] = Capacity.MinOfMaxPWM;	193	DebugOut.Analog[29] = Capacity.MinOfMaxPWM;
193	DebugOut.Analog[30] = GPS_Nick;	194	DebugOut.Analog[30] = GPS_Nick;
194	DebugOut.Analog[31] = GPS_Roll;	195	DebugOut.Analog[31] = GPS_Roll;
195	if(VersionInfo.HardwareError[0] \|\| VersionInfo.HardwareError[1]) DebugOut.Status[1] \|= 1; else DebugOut.Status[1] &= 0xfe;	196	if(VersionInfo.HardwareError[0] \|\| VersionInfo.HardwareError[1]) DebugOut.Status[1] \|= 1; else DebugOut.Status[1] &= 0xfe;
-		197
-		198	if(Capacity.MinOfMaxPWM < 250/* && modell_fliegt > 500*/) { beeptime = 1000; DebugOut.Analog[25]++; }
-		199
196	}	200	}
Line -...	Line 201...
-		201
-		202
197		203
198	void Piep(unsigned char Anzahl, unsigned int dauer)	204	void Piep(unsigned char Anzahl, unsigned int dauer)
199	{	205	{
200	if(MotorenEin) return; //auf keinen Fall im Flug!	206	if(MotorenEin) return; //auf keinen Fall im Flug!
201	while(Anzahl--)	207	while(Anzahl--)
Line 527...	Line 533...
527	{	533	{
528	if(!PC_MotortestActive) MotorTest[i] = 0;	534	if(!PC_MotortestActive) MotorTest[i] = 0;
529	Motor[i].SetPoint = MotorTest[i];	535	Motor[i].SetPoint = MotorTest[i];
530	Motor[i].SetPointLowerBits = 0;	536	Motor[i].SetPointLowerBits = 0;
531	/*	537	/*
532	Motor[i].SetPoint = MotorTest[i] / 4; // testing the high resolution	538	Motor[i].SetPoint = MotorTest[i] / 4; // testing the high resolution
533	Motor[i].SetPointLowerBits = MotorTest[i] % 4;	539	Motor[i].SetPointLowerBits = MotorTest[i] % 4;
534	*/	540	*/
535	}	541	}
536	if(PC_MotortestActive) PC_MotortestActive--;	542	if(PC_MotortestActive) PC_MotortestActive--;
537	}	543	}
538	else FCFlags \|= FCFLAG_MOTOR_RUN;	544	else FCFlags \|= FCFLAG_MOTOR_RUN;
Line 539...	Line 545...
539		545
540	if(I2C_TransferActive)	546	if(I2C_TransferActive)
541	{	-
542	DebugOut.Analog[16]++; // transmission was not finished yet	547	{
543	// I2C_TransferActive = 0; // enable for the next time	548	I2C_TransferActive = 0; // enable for the next time
544	}	549	}
545	//else	550	else
546	{	551	{
547	motor_write = 0;	-
548	//Start I2C Interrupt Mode	552	motor_write = 0;
549	I2C_Start(TWI_STATE_MOTOR_TX);	553	I2C_Start(TWI_STATE_MOTOR_TX); //Start I2C Interrupt Mode
550	}	554	}
Line 759...	Line 763...
759	else delay_neutral = 0;	763	else delay_neutral = 0;
760	}	764	}
761	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	765	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
762	// Gas ist unten	766	// Gas ist unten
763	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	767	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-		768	DebugOut.Analog[16] = EE_Parameter.MotorSafetySwitch;
-		769
764	if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)	770	if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)
765	{	771	{
766	// Motoren Starten	772	// Motoren Starten
767	if(!MotorenEin)	773	if(!MotorenEin)
768	{	774	{
769	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)	775	if((PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) && (PPM_in[EE_Parameter.MotorSafetySwitch] < -75 \|\| EE_Parameter.MotorSafetySwitch == 0))
770	{	776	{
771	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	777	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
772	// Einschalten	778	// Einschalten
773	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	779	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
774	if(++delay_einschalten > 200)	780	if(++delay_einschalten > 200)
Line 801...	Line 807...
801	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	807	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
802	// Auschalten	808	// Auschalten
803	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	809	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
804	else // only if motors are running	810	else // only if motors are running
805	{	811	{
806	if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)	812	if((PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) && (PPM_in[EE_Parameter.MotorSafetySwitch] < -75 \|\| EE_Parameter.MotorSafetySwitch == 0))
807	{	813	{
808	if(++delay_ausschalten > 200) // nicht sofort	814	if(++delay_ausschalten > 200) // nicht sofort
809	{	815	{
810	MotorenEin = 0;	816	MotorenEin = 0;
811	delay_ausschalten = 0;	817	delay_ausschalten = 0;
Line 830...	Line 836...
830	stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;	836	stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
Line 831...	Line 837...
831		837
832	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	838	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
833	// CareFree und freie Wahl der vorderen Richtung	839	// CareFree und freie Wahl der vorderen Richtung
834	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	-
835	signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8};	-
836		840	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
837	if(CareFree)	841	if(CareFree)
838	{	842	{
839	signed int nick, roll;	843	signed int nick, roll;
840	nick = stick_nick / 4;	844	nick = stick_nick / 4;
Line 848...	Line 852...
848	FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle];	852	FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle];
849	StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8;	853	StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8;
850	StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8;	854	StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8;
851	}	855	}
Line 852...	Line -...
852		-
853		856
854	StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];	857	StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
855	if(StickGier > 2) StickGier -= 2; else	858	if(StickGier > 2) StickGier -= 2; else
Line 856...	Line 859...
856	if(StickGier < -2) StickGier += 2; else StickGier = 0;	859	if(StickGier < -2) StickGier += 2; else StickGier = 0;
Line 1701...	Line 1704...
1701	else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;	1704	else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;
Line 1702...	Line 1705...
1702		1705
1703	if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing	1706	if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing
Line 1704...	Line 1707...
1704	else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing	1707	else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing
1705		1708
1706	LIMIT_MIN_MAX(tmp_int,MIN_GAS * 4,MAX_GAS * 4);	1709	LIMIT_MIN_MAX(tmp_int,(int) MIN_GAS * 4,(int) MAX_GAS * 4);
1707	Motor[i].SetPoint = tmp_int / 4;	1710	Motor[i].SetPoint = tmp_int / 4;
1708	Motor[i].SetPointLowerBits = (tmp_int % 4)<<1; // (3 bits total)	1711	Motor[i].SetPointLowerBits = (tmp_int % 4)<<1; // (3 bits total)
1709	tmp_motorwert[i] = tmp_int;	1712	tmp_motorwert[i] = tmp_int;
1710	}	1713	}
1711	else	1714	else
1712	{	1715	{
1713	Motor[i].SetPoint = 0;	1716	Motor[i].SetPoint = 0;
1714	Motor[i].SetPointLowerBits = 0;	1717	Motor[i].SetPointLowerBits = 0;
1715	}	-
1716	}	1718	}

Subversion Repositories FlightCtrl

(root)/branches/V2.01a_ACC-HH_MartinR/fc.c @ 2384 - Rev 1744 → 1760