WebSVN - FlightCtrl - Diff - Rev 921 and 927



#include "main.h"
#include "eeprom.c"

 
unsigned char h,m,s;
volatile unsigned int I2CTimeout = 100;
volatile int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias;
int AdNeutralGierBias;  
int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
volatile float NeutralAccZ = 0;
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
long IntegralNick = 0,IntegralNick2 = 0;

        NeutralAccY = 0;
        NeutralAccZ = 0;
    AdNeutralNick = 0; 
        AdNeutralRoll = 0;     
        AdNeutralGier = 0;
    AdNeutralGierBias = 0;
    Parameter_AchsKopplung1 = 0;
    Parameter_AchsGegenKopplung1 = 0;
    CalibrierMittelwert();     
    Delay_ms_Mess(100);
        CalibrierMittelwert();

     }

 
     AdNeutralNick= AdWertNick;
         AdNeutralRoll= AdWertRoll;    
         AdNeutralGier= AdWertGier;
     AdNeutralGierBias = AdWertGier;
     StartNeutralRoll = AdNeutralRoll;
     StartNeutralNick = AdNeutralNick;
    if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
    {

    Mess_IntegralRoll2 = 0;
    Mess_Integral_Gier = 0;    
    MesswertNick = 0;
    MesswertRoll = 0;
    MesswertGier = 0;
 Delay_ms_Mess(100);
    StartLuftdruck = Luftdruck;
    HoeheD = 0;
    Mess_Integral_Hoch = 0;
    KompassStartwert = KompassValue;
    GPS_Neutral();

void Mittelwert(void)
//############################################################################
{      
    static signed long tmpl,tmpl2;     
    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
    MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
    MesswertNick = (signed int) AdWertNick - AdNeutralNick;

 
//DebugOut.Analog[26] = MesswertNick;

    NaviCntAcc++;
    IntegralAccZ    += Aktuell_az - NeutralAccZ;
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
            ErsatzKompass +=  MesswertGier;
            Mess_Integral_Gier +=  MesswertGier;
//            Mess_Integral_Gier2 += MesswertGier;
                    if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR;  // 360° Umschlag
                    if(ErsatzKompass < 0)                          ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
         {
            tmpl = (MesswertGierBias * Mess_IntegralNick) / 2048L;
            tmpl *= Parameter_AchsKopplung1;  //125
            tmpl /= 4096L;
            tmpl2 = (MesswertGierBias * Mess_IntegralRoll) / 2048L;
            tmpl2 *= Parameter_AchsKopplung1;
            tmpl2 /= 4096L;
            if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
         }
      else  tmpl = tmpl2 = 0;

//############################################################################
// Messwerte beim Ermitteln der Nullage
void CalibrierMittelwert(void)
//############################################################################
{                
    if(PlatinenVersion >= 13) SucheGyroOffset();
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
        ANALOG_OFF;
        MesswertNick = AdWertNick;
        MesswertRoll = AdWertRoll;
        MesswertGier = AdWertGier;

                if(modell_fliegt == 250)
                 {
                  NeueKompassRichtungMerken = 1;
                  sollGier = 0;
                  Mess_Integral_Gier = 0;      
//                  Mess_Integral_Gier2 = 0;
                 }
                } else MikroKopterFlags |= FLAG_FLY;

               
            if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)

    Mess_IntegralNick2 -= IntegralFehlerNick;
    Mess_IntegralRoll2 -= IntegralFehlerRoll;

 
//    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
    if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }

    if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }
 
DebugOut.Analog[22] = MittelIntegralRoll / 26;

     {
      KompassSignalSchlecht = 1000;
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))
       {
         NeueKompassRichtungMerken = 1;
 
        };
     }
    tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
    sollGier = tmp_int;

Rev 921	Rev 927
Line 55...	Line 55...
55	#include "main.h"	55	#include "main.h"
56	#include "eeprom.c"	56	#include "eeprom.c"
Line 57...	Line 57...
57		57
58	unsigned char h,m,s;	58	unsigned char h,m,s;
59	volatile unsigned int I2CTimeout = 100;	59	volatile unsigned int I2CTimeout = 100;
-		60	volatile int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias;
60	volatile int MesswertNick,MesswertRoll,MesswertGier;	61	int AdNeutralGierBias;
61	volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;	62	int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
62	volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;	63	int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
63	int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;	64	int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
64	volatile float NeutralAccZ = 0;	65	volatile float NeutralAccZ = 0;
65	unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;	66	unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
66	long IntegralNick = 0,IntegralNick2 = 0;	67	long IntegralNick = 0,IntegralNick2 = 0;
Line 157...	Line 158...
157	NeutralAccY = 0;	158	NeutralAccY = 0;
158	NeutralAccZ = 0;	159	NeutralAccZ = 0;
159	AdNeutralNick = 0;	160	AdNeutralNick = 0;
160	AdNeutralRoll = 0;	161	AdNeutralRoll = 0;
161	AdNeutralGier = 0;	162	AdNeutralGier = 0;
-		163	AdNeutralGierBias = 0;
162	Parameter_AchsKopplung1 = 0;	164	Parameter_AchsKopplung1 = 0;
163	Parameter_AchsGegenKopplung1 = 0;	165	Parameter_AchsGegenKopplung1 = 0;
164	CalibrierMittelwert();	166	CalibrierMittelwert();
165	Delay_ms_Mess(100);	167	Delay_ms_Mess(100);
166	CalibrierMittelwert();	168	CalibrierMittelwert();
Line 170...	Line 172...
170	}	172	}
Line 171...	Line 173...
171		173
172	AdNeutralNick= AdWertNick;	174	AdNeutralNick= AdWertNick;
173	AdNeutralRoll= AdWertRoll;	175	AdNeutralRoll= AdWertRoll;
-		176	AdNeutralGier= AdWertGier;
174	AdNeutralGier= AdWertGier;	177	AdNeutralGierBias = AdWertGier;
175	StartNeutralRoll = AdNeutralRoll;	178	StartNeutralRoll = AdNeutralRoll;
176	StartNeutralNick = AdNeutralNick;	179	StartNeutralNick = AdNeutralNick;
177	if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)	180	if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
178	{	181	{
Line 193...	Line 196...
193	Mess_IntegralRoll2 = 0;	196	Mess_IntegralRoll2 = 0;
194	Mess_Integral_Gier = 0;	197	Mess_Integral_Gier = 0;
195	MesswertNick = 0;	198	MesswertNick = 0;
196	MesswertRoll = 0;	199	MesswertRoll = 0;
197	MesswertGier = 0;	200	MesswertGier = 0;
-		201	Delay_ms_Mess(100);
198	StartLuftdruck = Luftdruck;	202	StartLuftdruck = Luftdruck;
199	HoeheD = 0;	203	HoeheD = 0;
200	Mess_Integral_Hoch = 0;	204	Mess_Integral_Hoch = 0;
201	KompassStartwert = KompassValue;	205	KompassStartwert = KompassValue;
202	GPS_Neutral();	206	GPS_Neutral();
Line 216...	Line 220...
216	void Mittelwert(void)	220	void Mittelwert(void)
217	//############################################################################	221	//############################################################################
218	{	222	{
219	static signed long tmpl,tmpl2;	223	static signed long tmpl,tmpl2;
220	MesswertGier = (signed int) AdNeutralGier - AdWertGier;	224	MesswertGier = (signed int) AdNeutralGier - AdWertGier;
-		225	MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
221	MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;	226	MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
222	MesswertNick = (signed int) AdWertNick - AdNeutralNick;	227	MesswertNick = (signed int) AdWertNick - AdNeutralNick;
Line 223...	Line 228...
223		228
224	//DebugOut.Analog[26] = MesswertNick;	229	//DebugOut.Analog[26] = MesswertNick;
Line 235...	Line 240...
235	NaviCntAcc++;	240	NaviCntAcc++;
236	IntegralAccZ += Aktuell_az - NeutralAccZ;	241	IntegralAccZ += Aktuell_az - NeutralAccZ;
237	// Gier ++++++++++++++++++++++++++++++++++++++++++++++++	242	// Gier ++++++++++++++++++++++++++++++++++++++++++++++++
238	ErsatzKompass += MesswertGier;	243	ErsatzKompass += MesswertGier;
239	Mess_Integral_Gier += MesswertGier;	244	Mess_Integral_Gier += MesswertGier;
240	Mess_Integral_Gier2 += MesswertGier;	245	// Mess_Integral_Gier2 += MesswertGier;
241	if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag	246	if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag
242	if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR;	247	if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
243	// Kopplungsanteil +++++++++++++++++++++++++++++++++++++	248	// Kopplungsanteil +++++++++++++++++++++++++++++++++++++
244	if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))	249	if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
245	{	250	{
246	tmpl = (MesswertGier * Mess_IntegralNick) / 2048L;	251	tmpl = (MesswertGierBias * Mess_IntegralNick) / 2048L;
247	tmpl *= Parameter_AchsKopplung1; //125	252	tmpl *= Parameter_AchsKopplung1; //125
248	tmpl /= 4096L;	253	tmpl /= 4096L;
249	tmpl2 = (MesswertGier * Mess_IntegralRoll) / 2048L;	254	tmpl2 = (MesswertGierBias * Mess_IntegralRoll) / 2048L;
250	tmpl2 *= Parameter_AchsKopplung1;	255	tmpl2 *= Parameter_AchsKopplung1;
251	tmpl2 /= 4096L;	256	tmpl2 /= 4096L;
252	if(labs(tmpl) > 128 \|\| labs(tmpl2) > 128) TrichterFlug = 1;	257	if(labs(tmpl) > 128 \|\| labs(tmpl2) > 128) TrichterFlug = 1;
253	}	258	}
254	else tmpl = tmpl2 = 0;	259	else tmpl = tmpl2 = 0;
Line 338...	Line 343...
338	//############################################################################	343	//############################################################################
339	// Messwerte beim Ermitteln der Nullage	344	// Messwerte beim Ermitteln der Nullage
340	void CalibrierMittelwert(void)	345	void CalibrierMittelwert(void)
341	//############################################################################	346	//############################################################################
342	{	347	{
343	if(PlatinenVersion >= 12) SucheGyroOffset();	348	if(PlatinenVersion >= 13) SucheGyroOffset();
344	// ADC auschalten, damit die Werte sich nicht während der Berechnung ändern	349	// ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
345	ANALOG_OFF;	350	ANALOG_OFF;
346	MesswertNick = AdWertNick;	351	MesswertNick = AdWertNick;
347	MesswertRoll = AdWertRoll;	352	MesswertRoll = AdWertRoll;
348	MesswertGier = AdWertGier;	353	MesswertGier = AdWertGier;
Line 520...	Line 525...
520	if(modell_fliegt == 250)	525	if(modell_fliegt == 250)
521	{	526	{
522	NeueKompassRichtungMerken = 1;	527	NeueKompassRichtungMerken = 1;
523	sollGier = 0;	528	sollGier = 0;
524	Mess_Integral_Gier = 0;	529	Mess_Integral_Gier = 0;
525	Mess_Integral_Gier2 = 0;	530	// Mess_Integral_Gier2 = 0;
526	}	531	}
527	} else MikroKopterFlags \|= FLAG_FLY;	532	} else MikroKopterFlags \|= FLAG_FLY;
Line 528...	Line 533...
528		533
529	if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)	534	if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
Line 865...	Line 870...
865	Mess_IntegralNick2 -= IntegralFehlerNick;	870	Mess_IntegralNick2 -= IntegralFehlerNick;
866	Mess_IntegralRoll2 -= IntegralFehlerRoll;	871	Mess_IntegralRoll2 -= IntegralFehlerRoll;
Line 867...	Line 872...
867		872
868	// IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;	873	// IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
869	// IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;	874	// IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
870	if(GierGyroFehler > ABGLEICH_ANZAHL/2) AdNeutralGier++;	875	if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }
Line 871...	Line 876...
871	if(GierGyroFehler <-ABGLEICH_ANZAHL/2) AdNeutralGier--;	876	if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }
872		877
873	DebugOut.Analog[22] = MittelIntegralRoll / 26;	878	DebugOut.Analog[22] = MittelIntegralRoll / 26;
Line 996...	Line 1001...
996	{	1001	{
997	KompassSignalSchlecht = 1000;	1002	KompassSignalSchlecht = 1000;
998	if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))	1003	if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))
999	{	1004	{
1000	NeueKompassRichtungMerken = 1;	1005	NeueKompassRichtungMerken = 1;
1001	KompassStartwert = ErsatzKompass;	-
1002	};	1006	};
1003	}	1007	}
1004	tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx²	1008	tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx²
1005	tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;	1009	tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
1006	sollGier = tmp_int;	1010	sollGier = tmp_int;

Subversion Repositories FlightCtrl

(root)/tags/V0.72p/fc.c @ 1340 - Rev 921 → 927