WebSVN - FlightCtrl - Diff - Rev 1172 and 1173 - /branches/V0.82b-Arthur-P/fc.c



          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);
         AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER);
     AdNeutralGierBias = AdNeutralGier;
     StartNeutralRoll = AdNeutralRoll;
     StartNeutralNick = AdNeutralNick;
    if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
    {

      NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);
          NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);
          NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);
    }

 
 
 
 
 
 
    MesswertNick = 0;
    MesswertRoll = 0;
    MesswertGier = 0;
    Delay_ms_Mess(100);
        Mittelwert_AccNick = AdWertAccNick;
        Mittelwert_AccRoll = AdWertAccRoll;
    Mittelwert_AccHoch = AdWertAccHoch;
    IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
    IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
    Mess_IntegralNick2 = IntegralNick;
    Mess_IntegralRoll2 = IntegralRoll;
    Mess_Integral_Gier = 0;
    StartLuftdruck = Luftdruck;
    HoeheD = 0;
    Mess_Integral_Hoch = 0;
    KompassStartwert = KompassValue;

    GierGyroFehler = 0;
    SendVersionToNavi = 1;
    LED_Init();
    MikroKopterFlags |= FLAG_CALIBRATE;
    FromNaviCtrl_Value.Kalman_K = -1;
    FromNaviCtrl_Value.Kalman_MaxDrift = 0;
    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;

        else winkel_nick = Mess_IntegralNick;

 
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
   Mess_Integral_Gier += MesswertGier;
 
   ErsatzKompass += MesswertGier;
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
         {
            tmpl3 = (MesswertRoll * winkel_nick) / 2048L;

                        modell_fliegt = 1;
                        MotorenEin = 1;
                        sollGier = 0;
                        Mess_Integral_Gier = 0;
                        Mess_Integral_Gier2 = 0;
                        Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
                        Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
                        Mess_IntegralNick2 = IntegralNick;
                        Mess_IntegralRoll2 = IntegralRoll;
                        SummeNick = 0;
                        SummeRoll = 0;
                        MikroKopterFlags |= FLAG_START;

 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
 {
  static int cnt = 0;
  static char last_n_p,last_n_n,last_r_p,last_r_n;
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
  if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp)
  {
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;

#define FEHLER_LIMIT  (ABGLEICH_ANZAHL * 4)
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
#define BEWEGUNGS_LIMIT 20000
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
        if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8))
        {
        if(IntegralFehlerNick >  FEHLER_LIMIT2)
         {
           if(last_n_p)
           {
            cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8);
            ausgleichNick = IntegralFehlerNick / 8;
            if(ausgleichNick > 5000) ausgleichNick = 5000;
            LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
           }
           else last_n_p = 1;
         } else  last_n_p = 0;
        if(IntegralFehlerNick < -FEHLER_LIMIT2)
         {
           if(last_n_n)
            {
             cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8);
             ausgleichNick = IntegralFehlerNick / 8;
             if(ausgleichNick < -5000) ausgleichNick = -5000;
             LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
            }
           else last_n_n = 1;

        {
         cnt = 0;
         KompassSignalSchlecht = 1000;
        }
        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
                if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;
        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;

 
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++

        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
 
        ausgleichRoll = 0;
        if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8))
        {
        if(IntegralFehlerRoll >  FEHLER_LIMIT2)
         {
           if(last_r_p)
           {
            cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8);
            ausgleichRoll = IntegralFehlerRoll / 8;
            if(ausgleichRoll > 5000) ausgleichRoll = 5000;
            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
           }
           else last_r_p = 1;
         } else  last_r_p = 0;
        if(IntegralFehlerRoll < -FEHLER_LIMIT2)
         {
           if(last_r_n)
           {
            cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8);
            ausgleichRoll = IntegralFehlerRoll / 8;
            if(ausgleichRoll < -5000) ausgleichRoll = -5000;
            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;

        {
         cnt = 0;
         KompassSignalSchlecht = 1000;
        }
        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
                if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;
        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
  }
  else
  {

    MittelIntegralNick = 0;
    MittelIntegralRoll = 0;
    MittelIntegralNick2 = 0;
    MittelIntegralRoll2 = 0;
    ZaehlMessungen = 0;
 } //  ZaehlMessungen >= ABGLEICH_ANZAHL
 
 

 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//  Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
    DebugOut.Analog[19] = WinkelOut.CalcState;
    DebugOut.Analog[20] = ServoValue;
//    DebugOut.Analog[24] = MesswertNick/2;
//    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[30] = GPS_Nick;
    DebugOut.Analog[31] = GPS_Roll;

Rev 1172	Rev 1173
Line 202...	Line 202...
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;	207	AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
208	AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / NEUTRAL_FILTER;	208	AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
209	AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / NEUTRAL_FILTER;	209	AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER);
210	AdNeutralGierBias = AdNeutralGier;	210	AdNeutralGierBias = AdNeutralGier;
211	StartNeutralRoll = AdNeutralRoll;	211	StartNeutralRoll = AdNeutralRoll;
212	StartNeutralNick = AdNeutralNick;	212	StartNeutralNick = AdNeutralNick;
213	if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)	213	if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
214	{	214	{
Line 221...	Line 221...
221	NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);	221	NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);
222	NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);	222	NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);
223	NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);	223	NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);
224	}	224	}
Line 225...	Line -...
225		-
226	Mess_IntegralNick = 0;	-
227	Mess_IntegralNick2 = 0;	-
228	Mess_IntegralRoll = 0;	-
229	Mess_IntegralRoll2 = 0;	-
230	Mess_Integral_Gier = 0;	225
231	MesswertNick = 0;	226	MesswertNick = 0;
232	MesswertRoll = 0;	227	MesswertRoll = 0;
233	MesswertGier = 0;	228	MesswertGier = 0;
-		229	Delay_ms_Mess(100);
-		230	Mittelwert_AccNick = AdWertAccNick;
-		231	Mittelwert_AccRoll = AdWertAccRoll;
-		232	Mittelwert_AccHoch = AdWertAccHoch;
-		233	IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
-		234	IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
-		235	Mess_IntegralNick2 = IntegralNick;
-		236	Mess_IntegralRoll2 = IntegralRoll;
234	Delay_ms_Mess(100);	237	Mess_Integral_Gier = 0;
235	StartLuftdruck = Luftdruck;	238	StartLuftdruck = Luftdruck;
236	HoeheD = 0;	239	HoeheD = 0;
237	Mess_Integral_Hoch = 0;	240	Mess_Integral_Hoch = 0;
238	KompassStartwert = KompassValue;	241	KompassStartwert = KompassValue;
Line 245...	Line 248...
245	GierGyroFehler = 0;	248	GierGyroFehler = 0;
246	SendVersionToNavi = 1;	249	SendVersionToNavi = 1;
247	LED_Init();	250	LED_Init();
248	MikroKopterFlags \|= FLAG_CALIBRATE;	251	MikroKopterFlags \|= FLAG_CALIBRATE;
249	FromNaviCtrl_Value.Kalman_K = -1;	252	FromNaviCtrl_Value.Kalman_K = -1;
250	FromNaviCtrl_Value.Kalman_MaxDrift = EE_Parameter.Driftkomp * 16;	253	FromNaviCtrl_Value.Kalman_MaxDrift = 0;
251	FromNaviCtrl_Value.Kalman_MaxFusion = 32;	254	FromNaviCtrl_Value.Kalman_MaxFusion = 32;
252	Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110;	255	Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110;
253	Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110;	256	Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110;
254	Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110;	257	Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110;
255	Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110;	258	Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110;
Line 304...	Line 307...
304	else winkel_nick = Mess_IntegralNick;	307	else winkel_nick = Mess_IntegralNick;
Line 305...	Line 308...
305		308
306	// Gier ++++++++++++++++++++++++++++++++++++++++++++++++	309	// Gier ++++++++++++++++++++++++++++++++++++++++++++++++
307	Mess_Integral_Gier += MesswertGier;	310	Mess_Integral_Gier += MesswertGier;
308	ErsatzKompass += MesswertGier;	-
309		311	ErsatzKompass += MesswertGier;
310	// Kopplungsanteil +++++++++++++++++++++++++++++++++++++	312	// Kopplungsanteil +++++++++++++++++++++++++++++++++++++
311	if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))	313	if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
312	{	314	{
313	tmpl3 = (MesswertRoll * winkel_nick) / 2048L;	315	tmpl3 = (MesswertRoll * winkel_nick) / 2048L;
Line 695...	Line 697...
695	modell_fliegt = 1;	697	modell_fliegt = 1;
696	MotorenEin = 1;	698	MotorenEin = 1;
697	sollGier = 0;	699	sollGier = 0;
698	Mess_Integral_Gier = 0;	700	Mess_Integral_Gier = 0;
699	Mess_Integral_Gier2 = 0;	701	Mess_Integral_Gier2 = 0;
700	Mess_IntegralNick = 0;	702	Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
701	Mess_IntegralRoll = 0;	703	Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
702	Mess_IntegralNick2 = IntegralNick;	704	Mess_IntegralNick2 = IntegralNick;
703	Mess_IntegralRoll2 = IntegralRoll;	705	Mess_IntegralRoll2 = IntegralRoll;
704	SummeNick = 0;	706	SummeNick = 0;
705	SummeRoll = 0;	707	SummeRoll = 0;
706	MikroKopterFlags \|= FLAG_START;	708	MikroKopterFlags \|= FLAG_START;
Line 932...	Line 934...
932	if(ZaehlMessungen >= ABGLEICH_ANZAHL)	934	if(ZaehlMessungen >= ABGLEICH_ANZAHL)
933	{	935	{
934	static int cnt = 0;	936	static int cnt = 0;
935	static char last_n_p,last_n_n,last_r_p,last_r_n;	937	static char last_n_p,last_n_n,last_r_p,last_r_n;
936	static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;	938	static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
937	if(!Looping_Nick && !Looping_Roll && !TrichterFlug && (PlatinenVersion < 20))	939	if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp)
938	{	940	{
939	MittelIntegralNick /= ABGLEICH_ANZAHL;	941	MittelIntegralNick /= ABGLEICH_ANZAHL;
940	MittelIntegralRoll /= ABGLEICH_ANZAHL;	942	MittelIntegralRoll /= ABGLEICH_ANZAHL;
941	IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;	943	IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
942	IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;	944	IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
Line 1000...	Line 1002...
1000	#define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4)	1002	#define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4)
1001	#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)	1003	#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
1002	#define BEWEGUNGS_LIMIT 20000	1004	#define BEWEGUNGS_LIMIT 20000
1003	// Nick +++++++++++++++++++++++++++++++++++++++++++++++++	1005	// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
1004	cnt = 1;// + labs(IntegralFehlerNick) / 4096;	1006	cnt = 1;// + labs(IntegralFehlerNick) / 4096;
1005	if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT \|\| (FromNaviCtrl_Value.Kalman_MaxDrift > 3*16))	1007	if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT \|\| (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8))
1006	{	1008	{
1007	if(IntegralFehlerNick > FEHLER_LIMIT2)	1009	if(IntegralFehlerNick > FEHLER_LIMIT2)
1008	{	1010	{
1009	if(last_n_p)	1011	if(last_n_p)
1010	{	1012	{
1011	cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;	1013	cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8);
1012	ausgleichNick = IntegralFehlerNick / 8;	1014	ausgleichNick = IntegralFehlerNick / 8;
1013	if(ausgleichNick > 5000) ausgleichNick = 5000;	1015	if(ausgleichNick > 5000) ausgleichNick = 5000;
1014	LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;	1016	LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
1015	}	1017	}
1016	else last_n_p = 1;	1018	else last_n_p = 1;
1017	} else last_n_p = 0;	1019	} else last_n_p = 0;
1018	if(IntegralFehlerNick < -FEHLER_LIMIT2)	1020	if(IntegralFehlerNick < -FEHLER_LIMIT2)
1019	{	1021	{
1020	if(last_n_n)	1022	if(last_n_n)
1021	{	1023	{
1022	cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;	1024	cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8);
1023	ausgleichNick = IntegralFehlerNick / 8;	1025	ausgleichNick = IntegralFehlerNick / 8;
1024	if(ausgleichNick < -5000) ausgleichNick = -5000;	1026	if(ausgleichNick < -5000) ausgleichNick = -5000;
1025	LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;	1027	LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
1026	}	1028	}
1027	else last_n_n = 1;	1029	else last_n_n = 1;
Line 1031...	Line 1033...
1031	{	1033	{
1032	cnt = 0;	1034	cnt = 0;
1033	KompassSignalSchlecht = 1000;	1035	KompassSignalSchlecht = 1000;
1034	}	1036	}
1035	if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;	1037	if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
1036	if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift/16;	1038	if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;
1037	if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;	1039	if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;
1038	if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;	1040	if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;
Line 1039...	Line 1041...
1039		1041
1040	// Roll +++++++++++++++++++++++++++++++++++++++++++++++++	1042	// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
Line 1041...	Line 1043...
1041	cnt = 1;// + labs(IntegralFehlerNick) / 4096;	1043	cnt = 1;// + labs(IntegralFehlerNick) / 4096;
1042		1044
1043	ausgleichRoll = 0;	1045	ausgleichRoll = 0;
1044	if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT \|\| (FromNaviCtrl_Value.Kalman_MaxDrift > 3*16))	1046	if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT \|\| (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8))
1045	{	1047	{
1046	if(IntegralFehlerRoll > FEHLER_LIMIT2)	1048	if(IntegralFehlerRoll > FEHLER_LIMIT2)
1047	{	1049	{
1048	if(last_r_p)	1050	if(last_r_p)
1049	{	1051	{
1050	cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;	1052	cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8);
1051	ausgleichRoll = IntegralFehlerRoll / 8;	1053	ausgleichRoll = IntegralFehlerRoll / 8;
1052	if(ausgleichRoll > 5000) ausgleichRoll = 5000;	1054	if(ausgleichRoll > 5000) ausgleichRoll = 5000;
1053	LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;	1055	LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
1054	}	1056	}
1055	else last_r_p = 1;	1057	else last_r_p = 1;
1056	} else last_r_p = 0;	1058	} else last_r_p = 0;
1057	if(IntegralFehlerRoll < -FEHLER_LIMIT2)	1059	if(IntegralFehlerRoll < -FEHLER_LIMIT2)
1058	{	1060	{
1059	if(last_r_n)	1061	if(last_r_n)
1060	{	1062	{
1061	cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;	1063	cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8);
1062	ausgleichRoll = IntegralFehlerRoll / 8;	1064	ausgleichRoll = IntegralFehlerRoll / 8;
1063	if(ausgleichRoll < -5000) ausgleichRoll = -5000;	1065	if(ausgleichRoll < -5000) ausgleichRoll = -5000;
1064	LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;	1066	LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
Line 1069...	Line 1071...
1069	{	1071	{
1070	cnt = 0;	1072	cnt = 0;
1071	KompassSignalSchlecht = 1000;	1073	KompassSignalSchlecht = 1000;
1072	}	1074	}
1073	if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;	1075	if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
1074	if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift/16;	1076	if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;
1075	if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;	1077	if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;
1076	if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;	1078	if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;
1077	}	1079	}
1078	else	1080	else
1079	{	1081	{
Line 1093...	Line 1095...
1093	MittelIntegralNick = 0;	1095	MittelIntegralNick = 0;
1094	MittelIntegralRoll = 0;	1096	MittelIntegralRoll = 0;
1095	MittelIntegralNick2 = 0;	1097	MittelIntegralNick2 = 0;
1096	MittelIntegralRoll2 = 0;	1098	MittelIntegralRoll2 = 0;
1097	ZaehlMessungen = 0;	1099	ZaehlMessungen = 0;
-		1100	} // ZaehlMessungen >= ABGLEICH_ANZAHL
1098	}	1101
1099	//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);	-
Line 1100...	Line 1102...
1100		1102
1101	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1103	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1102	// Gieren	1104	// Gieren
1103	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1105	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 1200...	Line 1202...
1200	//DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;	1202	//DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
1201	DebugOut.Analog[19] = WinkelOut.CalcState;	1203	DebugOut.Analog[19] = WinkelOut.CalcState;
1202	DebugOut.Analog[20] = ServoValue;	1204	DebugOut.Analog[20] = ServoValue;
1203	// DebugOut.Analog[24] = MesswertNick/2;	1205	// DebugOut.Analog[24] = MesswertNick/2;
1204	// DebugOut.Analog[25] = MesswertRoll/2;	1206	// DebugOut.Analog[25] = MesswertRoll/2;
1205	// DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;	1207	DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
1206	// DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;	1208	// DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;
1207	// DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;	1209	// DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
1208	DebugOut.Analog[30] = GPS_Nick;	1210	DebugOut.Analog[30] = GPS_Nick;
1209	DebugOut.Analog[31] = GPS_Roll;	1211	DebugOut.Analog[31] = GPS_Roll;

Subversion Repositories FlightCtrl

(root)/branches/V0.82b-Arthur-P/fc.c @ 2121 - Rev 1172 → 1173