WebSVN - FlightCtrl - Diff - Rev 1215 and 1216 - /branches/V0.88n_ACC-HH_HR



     }
#define NEUTRAL_FILTER 32
    for(i=0; i<NEUTRAL_FILTER; i++)
         {
          Delay_ms_Mess(10);
          gier_neutral += AdWertGier;
          nick_neutral += AdWertNick;
          roll_neutral += AdWertRoll;
         }
     AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
         AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);

    FromNaviCtrl_Value.Kalman_MaxFusion = 32;
    Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110;
    Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110;
    Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110;
    Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110;
    ServoActive = 1;
    SenderOkay = 100;
}

 
//############################################################################

    RohMesswertNick = MesswertNick;
    RohMesswertRoll = MesswertRoll;
//DebugOut.Analog[21] = MesswertNick;
//DebugOut.Analog[22] = MesswertRoll;
//DebugOut.Analog[22] = Mess_Integral_Gier;
//DebugOut.Analog[21] = MesswertNick;
//DebugOut.Analog[22] = MesswertRoll;

 
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L;
        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L;

// ADC einschalten
    ANALOG_ON;
        AdReady = 0;
//++++++++++++++++++++++++++++++++++++++++++++++++

 
    if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L;
        else if(Mess_IntegralRoll <-93000L) winkel_roll = -93000L;

        else winkel_roll = Mess_IntegralRoll;
 
    if(Mess_IntegralNick > 93000L) winkel_nick = 93000L;

        else if(Mess_IntegralNick <-93000L) winkel_nick = -93000L;
        else winkel_nick = Mess_IntegralNick;
 

         {
            tmpl3 = (MesswertRoll * winkel_nick) / 2048L;
            tmpl3 *= Parameter_AchsKopplung2; //65
            tmpl3 /= 4096L;
            tmpl4 = (MesswertNick * winkel_roll) / 2048L;
            tmpl4 *= Parameter_AchsKopplung2; //65
            tmpl4 /= 4096L;
            KopplungsteilNickRoll = tmpl3;
            KopplungsteilRollNick = tmpl4;
            tmpl4 -= tmpl3;
            ErsatzKompass += tmpl4;

   else  {  if(AdWertNick > 2000) MesswertNick = +1000;  if(AdWertNick > 2015) MesswertNick = +2000; }
   if(AdWertRoll < 15)   MesswertRoll = -1000;  if(AdWertRoll <  7)   MesswertRoll = -2000;
   if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000;  if(AdWertRoll > 1017) MesswertRoll = +2000; }
   else { if(AdWertRoll > 2000) MesswertRoll = +1000;  if(AdWertRoll > 2015) MesswertRoll = +2000;  }

 
  if(Parameter_Gyro_D)
  {
   d2Nick = HiResNick - oldNick;
   oldNick = (oldNick + HiResNick)/2;
   if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;

   if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
   else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;
   MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16;
   HiResNick += (d2Nick * (signed int) Parameter_Gyro_D);
   HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D);
  }

 
 if(RohMesswertRoll > 0) TrimRoll  += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
 else                    TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
 if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;

{
 unsigned char i;
    if(!MotorenEin)
        {
         MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY);
                 for(i=0;i<MAX_MOTORS;i++)
                  {
                   if(!PC_MotortestActive)  MotorTest[i] = 0;
                   Motor[i] = MotorTest[i];
                  }
          if(PC_MotortestActive) PC_MotortestActive--;
        }
        else MikroKopterFlags |= FLAG_MOTOR_RUN;

 
    DebugOut.Analog[12] = Motor[0];
    DebugOut.Analog[13] = Motor[1];

            }

 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// neue Werte von der Funke
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
 if(!NewPpmData-- || Notlandung)
  {
        static int stick_nick,stick_roll;
    ParameterZuordnung();

      tmp_long2 /= 16;
     if((MaxStickNick > 64) || (MaxStickRoll > 64))
      {
      tmp_long  /= 3;
      tmp_long2 /= 3;
      }
     if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
      {
      tmp_long  /= 3;
      tmp_long2 /= 3;
      }

//    DebugOut.Analog[25] = MesswertRoll/2;
    DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
//    DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;
//    DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
    DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
    DebugOut.Analog[30] = GPS_Nick;
    DebugOut.Analog[31] = GPS_Roll;

 
 

  }

 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  if(TrichterFlug)  { SummeRoll = 0; SummeNick = 0;};
 

  if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) /  (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0;

    MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));

 
    // Maximalwerte abfangen
//    #define MAX_SENSOR  (4096*STICK_GAIN)
    #define MAX_SENSOR  (4096*4)
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;

    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// all BL-Ctrl connected?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(MissingMotor) if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0)
   {
    modell_fliegt = 1;

        GasMischanteil = MIN_GAS;
   }
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  {
    int tmp_int;
        static char delay = 100;
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
    {
 
      if(Parameter_MaxHoehe < 50)
      {
       if(!delay--)
            {
         if(MessLuftdruck > 1000)
                  {

    if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;

 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Universal Mixer
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 for(i=0; i<MAX_MOTORS; i++)
 {
  signed int tmp_int;
  if(Mixer.Motor[i][0] > 0)
   {
    tmp_int =  ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L;
    tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L;
    tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L;
    tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;
    tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]);  // Filter
        tmp_int = tmp_motorwert[i] / STICK_GAIN;
        CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS);
    Motor[i] = tmp_int;
   }
   else Motor[i] = 0;
 }
/*

Rev 1215	Rev 1216
Line 198...	Line 198...
198	}	198	}
199	#define NEUTRAL_FILTER 32	199	#define NEUTRAL_FILTER 32
200	for(i=0; i<NEUTRAL_FILTER; i++)	200	for(i=0; i<NEUTRAL_FILTER; i++)
201	{	201	{
202	Delay_ms_Mess(10);	202	Delay_ms_Mess(10);
203	gier_neutral += AdWertGier;	203	gier_neutral += AdWertGier;
204	nick_neutral += AdWertNick;	204	nick_neutral += AdWertNick;
205	roll_neutral += AdWertRoll;	205	roll_neutral += AdWertRoll;
206	}	206	}
207	AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);	207	AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
208	AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);	208	AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
Line 253...	Line 253...
253	FromNaviCtrl_Value.Kalman_MaxFusion = 32;	253	FromNaviCtrl_Value.Kalman_MaxFusion = 32;
254	Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110;	254	Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110;
255	Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110;	255	Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110;
256	Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110;	256	Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110;
257	Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110;	257	Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110;
258	ServoActive = 1;	258	ServoActive = 1;
259	SenderOkay = 100;	259	SenderOkay = 100;
260	}	260	}
Line 261...	Line 261...
261		261
262	//############################################################################	262	//############################################################################
Line 275...	Line 275...
275	RohMesswertNick = MesswertNick;	275	RohMesswertNick = MesswertNick;
276	RohMesswertRoll = MesswertRoll;	276	RohMesswertRoll = MesswertRoll;
277	//DebugOut.Analog[21] = MesswertNick;	277	//DebugOut.Analog[21] = MesswertNick;
278	//DebugOut.Analog[22] = MesswertRoll;	278	//DebugOut.Analog[22] = MesswertRoll;
279	//DebugOut.Analog[22] = Mess_Integral_Gier;	279	//DebugOut.Analog[22] = Mess_Integral_Gier;
280	//DebugOut.Analog[21] = MesswertNick;	280	//DebugOut.Analog[21] = MesswertNick;
281	//DebugOut.Analog[22] = MesswertRoll;	281	//DebugOut.Analog[22] = MesswertRoll;
Line 282...	Line 282...
282		282
283	// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++	283	// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++
284	Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L;	284	Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L;
285	Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L;	285	Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L;
Line 295...	Line 295...
295	// ADC einschalten	295	// ADC einschalten
296	ANALOG_ON;	296	ANALOG_ON;
297	AdReady = 0;	297	AdReady = 0;
298	//++++++++++++++++++++++++++++++++++++++++++++++++	298	//++++++++++++++++++++++++++++++++++++++++++++++++
Line 299...	Line 299...
299		299
300	if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L;	300	if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L;
301	else if(Mess_IntegralRoll <-93000L) winkel_roll = -93000L;	301	else if(Mess_IntegralRoll <-93000L) winkel_roll = -93000L;
Line 302...	Line 302...
302	else winkel_roll = Mess_IntegralRoll;	302	else winkel_roll = Mess_IntegralRoll;
303		303
304	if(Mess_IntegralNick > 93000L) winkel_nick = 93000L;	304	if(Mess_IntegralNick > 93000L) winkel_nick = 93000L;
Line 305...	Line 305...
305	else if(Mess_IntegralNick <-93000L) winkel_nick = -93000L;	305	else if(Mess_IntegralNick <-93000L) winkel_nick = -93000L;
306	else winkel_nick = Mess_IntegralNick;	306	else winkel_nick = Mess_IntegralNick;
307		307
Line 313...	Line 313...
313	{	313	{
314	tmpl3 = (MesswertRoll * winkel_nick) / 2048L;	314	tmpl3 = (MesswertRoll * winkel_nick) / 2048L;
315	tmpl3 *= Parameter_AchsKopplung2; //65	315	tmpl3 *= Parameter_AchsKopplung2; //65
316	tmpl3 /= 4096L;	316	tmpl3 /= 4096L;
317	tmpl4 = (MesswertNick * winkel_roll) / 2048L;	317	tmpl4 = (MesswertNick * winkel_roll) / 2048L;
318	tmpl4 *= Parameter_AchsKopplung2; //65	318	tmpl4 *= Parameter_AchsKopplung2; //65
319	tmpl4 /= 4096L;	319	tmpl4 /= 4096L;
320	KopplungsteilNickRoll = tmpl3;	320	KopplungsteilNickRoll = tmpl3;
321	KopplungsteilRollNick = tmpl4;	321	KopplungsteilRollNick = tmpl4;
322	tmpl4 -= tmpl3;	322	tmpl4 -= tmpl3;
323	ErsatzKompass += tmpl4;	323	ErsatzKompass += tmpl4;
Line 383...	Line 383...
383	else { if(AdWertNick > 2000) MesswertNick = +1000; if(AdWertNick > 2015) MesswertNick = +2000; }	383	else { if(AdWertNick > 2000) MesswertNick = +1000; if(AdWertNick > 2015) MesswertNick = +2000; }
384	if(AdWertRoll < 15) MesswertRoll = -1000; if(AdWertRoll < 7) MesswertRoll = -2000;	384	if(AdWertRoll < 15) MesswertRoll = -1000; if(AdWertRoll < 7) MesswertRoll = -2000;
385	if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000; if(AdWertRoll > 1017) MesswertRoll = +2000; }	385	if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000; if(AdWertRoll > 1017) MesswertRoll = +2000; }
386	else { if(AdWertRoll > 2000) MesswertRoll = +1000; if(AdWertRoll > 2015) MesswertRoll = +2000; }	386	else { if(AdWertRoll > 2000) MesswertRoll = +1000; if(AdWertRoll > 2015) MesswertRoll = +2000; }
Line 387...	Line 387...
387		387
388	if(Parameter_Gyro_D)	388	if(Parameter_Gyro_D)
389	{	389	{
390	d2Nick = HiResNick - oldNick;	390	d2Nick = HiResNick - oldNick;
391	oldNick = (oldNick + HiResNick)/2;	391	oldNick = (oldNick + HiResNick)/2;
392	if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;	392	if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;
Line 397...	Line 397...
397	if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;	397	if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
398	else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;	398	else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;
399	MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16;	399	MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16;
400	HiResNick += (d2Nick * (signed int) Parameter_Gyro_D);	400	HiResNick += (d2Nick * (signed int) Parameter_Gyro_D);
401	HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D);	401	HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D);
402	}	402	}
Line 403...	Line 403...
403		403
404	if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;	404	if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
405	else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;	405	else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
406	if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;	406	if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
Line 460...	Line 460...
460	{	460	{
461	unsigned char i;	461	unsigned char i;
462	if(!MotorenEin)	462	if(!MotorenEin)
463	{	463	{
464	MikroKopterFlags &= ~(FLAG_MOTOR_RUN \| FLAG_FLY);	464	MikroKopterFlags &= ~(FLAG_MOTOR_RUN \| FLAG_FLY);
465	for(i=0;i<MAX_MOTORS;i++)	465	for(i=0;i<MAX_MOTORS;i++)
466	{	466	{
467	if(!PC_MotortestActive) MotorTest[i] = 0;	467	if(!PC_MotortestActive) MotorTest[i] = 0;
468	Motor[i] = MotorTest[i];	468	Motor[i] = MotorTest[i];
469	}	469	}
470	if(PC_MotortestActive) PC_MotortestActive--;	470	if(PC_MotortestActive) PC_MotortestActive--;
471	}	471	}
472	else MikroKopterFlags \|= FLAG_MOTOR_RUN;	472	else MikroKopterFlags \|= FLAG_MOTOR_RUN;
Line 473...	Line 473...
473		473
474	DebugOut.Analog[12] = Motor[0];	474	DebugOut.Analog[12] = Motor[0];
475	DebugOut.Analog[13] = Motor[1];	475	DebugOut.Analog[13] = Motor[1];
Line 720...	Line 720...
720	}	720	}
Line 721...	Line 721...
721		721
722	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	722	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
723	// neue Werte von der Funke	723	// neue Werte von der Funke
724	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	724	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
725		725
726	if(!NewPpmData-- \|\| Notlandung)	726	if(!NewPpmData-- \|\| Notlandung)
727	{	727	{
728	static int stick_nick,stick_roll;	728	static int stick_nick,stick_roll;
729	ParameterZuordnung();	729	ParameterZuordnung();
Line 898...	Line 898...
898	tmp_long2 /= 16;	898	tmp_long2 /= 16;
899	if((MaxStickNick > 64) \|\| (MaxStickRoll > 64))	899	if((MaxStickNick > 64) \|\| (MaxStickRoll > 64))
900	{	900	{
901	tmp_long /= 3;	901	tmp_long /= 3;
902	tmp_long2 /= 3;	902	tmp_long2 /= 3;
903	}	903	}
904	if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)	904	if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
905	{	905	{
906	tmp_long /= 3;	906	tmp_long /= 3;
907	tmp_long2 /= 3;	907	tmp_long2 /= 3;
908	}	908	}
Line 1186...	Line 1186...
1186	// DebugOut.Analog[25] = MesswertRoll/2;	1186	// DebugOut.Analog[25] = MesswertRoll/2;
1187	DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;	1187	DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
1188	// DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;	1188	// DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;
1189	// DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;	1189	// DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
1190	DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;	1190	DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
1191	DebugOut.Analog[30] = GPS_Nick;	1191	DebugOut.Analog[30] = GPS_Nick;
1192	DebugOut.Analog[31] = GPS_Roll;	1192	DebugOut.Analog[31] = GPS_Roll;
Line 1193...	Line 1193...
1193		1193
1194		1194
Line 1220...	Line 1220...
1220	}	1220	}
Line 1221...	Line 1221...
1221		1221
1222	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1222	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1223	// Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen	1223	// Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
1224	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1224	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 1225...	Line 1225...
1225	if(TrichterFlug) { SummeRoll = 0; SummeNick = 0;};	1225	if(TrichterFlug) { SummeRoll = 0; SummeNick = 0;};
1226		1226
Line 1227...	Line 1227...
1227	if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0;	1227	if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0;
Line 1236...	Line 1236...
1236	MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));	1236	MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
Line 1237...	Line 1237...
1237		1237
1238	// Maximalwerte abfangen	1238	// Maximalwerte abfangen
1239	// #define MAX_SENSOR (4096*STICK_GAIN)	1239	// #define MAX_SENSOR (4096*STICK_GAIN)
1240	#define MAX_SENSOR (4096*4)	1240	#define MAX_SENSOR (4096*4)
1241	if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR;	1241	if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR;
1242	if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;	1242	if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
1243	if(MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR;	1243	if(MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR;
1244	if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;	1244	if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
1245	if(MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR;	1245	if(MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR;
Line 1246...	Line 1246...
1246	if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;	1246	if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
1247		1247
1248	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1248	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1249	// all BL-Ctrl connected?	1249	// all BL-Ctrl connected?
1250	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1250	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1251	if(MissingMotor) if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0)	1251	if(MissingMotor) if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0)
1252	{	1252	{
1253	modell_fliegt = 1;	1253	modell_fliegt = 1;
Line 1254...	Line 1254...
1254	GasMischanteil = MIN_GAS;	1254	GasMischanteil = MIN_GAS;
1255	}	1255	}
1256		1256
1257	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1257	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 1263...	Line 1263...
1263	{	1263	{
1264	int tmp_int;	1264	int tmp_int;
1265	static char delay = 100;	1265	static char delay = 100;
1266	if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert	1266	if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert
1267	{	1267	{
1268	if(((EE_Parameter.BitConfig & CFG_HIGHT_3SWITCH) && ((Parameter_MaxHoehe > 80) && (Parameter_MaxHoehe < 140))) \|\|	-
1269	(!(EE_Parameter.BitConfig & CFG_HIGHT_3SWITCH) && (Parameter_MaxHoehe < 50)))	1268	if(Parameter_MaxHoehe < 50)
1270	{	1269	{
1271	if(!delay--)	1270	if(!delay--)
1272	{	1271	{
1273	if(MessLuftdruck > 1000)	1272	if(MessLuftdruck > 1000)
1274	{	1273	{
Line 1381...	Line 1380...
1381	if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;	1380	if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
Line 1382...	Line 1381...
1382		1381
1383	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1382	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1384	// Universal Mixer	1383	// Universal Mixer
1385	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1384	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1386	for(i=0; i<MAX_MOTORS; i++)	1385	for(i=0; i<MAX_MOTORS; i++)
1387	{	1386	{
1388	signed int tmp_int;	1387	signed int tmp_int;
1389	if(Mixer.Motor[i][0] > 0)	1388	if(Mixer.Motor[i][0] > 0)
1390	{	1389	{
1391	tmp_int = ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L;	1390	tmp_int = ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L;
1392	tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L;	1391	tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L;
1393	tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L;	1392	tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L;
1394	tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;	1393	tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;
1395	tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]); // Filter	1394	tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]); // Filter
1396	tmp_int = tmp_motorwert[i] / STICK_GAIN;	1395	tmp_int = tmp_motorwert[i] / STICK_GAIN;
1397	CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS);	1396	CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS);
1398	Motor[i] = tmp_int;	1397	Motor[i] = tmp_int;
1399	}	1398	}
1400	else Motor[i] = 0;	1399	else Motor[i] = 0;
1401	}	1400	}
1402	/*	1401	/*

Subversion Repositories FlightCtrl

(root)/branches/V0.88n_ACC-HH_HR_MartinR/fc.c @ 2177 - Rev 1215 → 1216