WebSVN - FlightCtrl - Diff - Rev 1660 and 1662 - /branches/MartinW



#include "mymath.h"
#include "isqrt.h"

 
unsigned char h,m,s;
 
unsigned int BaroExpandActive = 0;
int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
int TrimNick, TrimRoll;
int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch;

    Parameter_AchsKopplung1 = 0;
    Parameter_AchsKopplung2 = 0;

 

 
 
    ExpandBaro = 0;
 

    CalibrierMittelwert();

            tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
            tmpl *= Parameter_AchsKopplung1;  // 90
            tmpl /= 4096L;
            tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
            tmpl2 *= Parameter_AchsKopplung1;
            tmpl2 /= 4096L;
            if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
            //MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256;
         }
      else  tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0;
                        TrimRoll = tmpl - tmpl2 / 100L;

    DebugOut.Analog[13] = Motor[1].SetPoint;
    DebugOut.Analog[14] = Motor[2].SetPoint;
    DebugOut.Analog[15] = Motor[3].SetPoint;

 
 
    //Start I2C Interrupt Mode
    motor_write = 0;
    I2C_Start(TWI_STATE_MOTOR_TX);

}

// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability);
 CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl);
 Ki = (10300 / 2) / (Parameter_I_Faktor + 1);
 MAX_GAS = EE_Parameter.Gas_Max;
 MIN_GAS = EE_Parameter.Gas_Min;
if(Poti4 > 50) HeadFree = 1; else HeadFree = 0;
 if(HeadFree) {if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;}
}

 

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

 
 if(!NewPpmData-- || (FCFlags & FCFLAG_NOTLANDUNG))
  {
        static int stick_nick,stick_roll;
 
unsigned char angle = 180/15;
signed char sintab[31] = { 0, 4, 8, 11, 14, 16, 16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16, 16};
//signed char costab[24] = {16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16};

if(HeadFree)
 {
    StickNick = ((cos_h * stick_nick) + (sin_h * stick_roll))/8;
    StickRoll = ((cos_h * stick_roll) - (sin_h * stick_nick))/8;
 }
 else
 {
    StickNick = stick_nick;
    StickRoll = stick_roll;
 }
 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
        if(StickGier > 2) StickGier -= 2;       else
        if(StickGier < -2) StickGier += 2; else StickGier = 0;

 

       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
       v = abs(IntegralRoll /512);
       if(v > w) w = v; // grösste Neigung ermitteln
       korrektur = w / 8 + 2;
       fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
           fehler += MesswertGier / 12;
 
       if(!KompassSignalSchlecht && w < 25)
        {
        GierGyroFehler += fehler;
        if(NeueKompassRichtungMerken)
         {

Rev 1660	Rev 1662
Line 57...	Line 57...
57	#include "mymath.h"	57	#include "mymath.h"
58	#include "isqrt.h"	58	#include "isqrt.h"
Line 59...	Line 59...
59		59
60	unsigned char h,m,s;	60	unsigned char h,m,s;
61	unsigned int BaroExpandActive = 0;	-
62	volatile unsigned int I2CTimeout = 100;	61	unsigned int BaroExpandActive = 0;
63	int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;	62	int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
64	int TrimNick, TrimRoll;	63	int TrimNick, TrimRoll;
65	int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;	64	int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
66	int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch;	65	int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch;
Line 259...	Line 258...
259	Parameter_AchsKopplung1 = 0;	258	Parameter_AchsKopplung1 = 0;
260	Parameter_AchsKopplung2 = 0;	259	Parameter_AchsKopplung2 = 0;
Line 261...	Line 260...
261		260
Line 262...	Line -...
262	ExpandBaro = 0;	-
263		-
264	I2C_SendBLConfig();	261	ExpandBaro = 0;
265		262
Line 266...	Line 263...
266	CalibrierMittelwert();	263	CalibrierMittelwert();
Line 416...	Line 413...
416	tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;	413	tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
417	tmpl *= Parameter_AchsKopplung1; // 90	414	tmpl *= Parameter_AchsKopplung1; // 90
418	tmpl /= 4096L;	415	tmpl /= 4096L;
419	tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;	416	tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
420	tmpl2 *= Parameter_AchsKopplung1;	417	tmpl2 *= Parameter_AchsKopplung1;
421	tmpl2 /= 4096L;	418	tmpl2 /= 4096L;
422	if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 \|\| labs(tmpl2) > 128) TrichterFlug = 1;	419	if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 \|\| labs(tmpl2) > 128) TrichterFlug = 1;
423	//MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256;	420	//MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256;
424	}	421	}
425	else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0;	422	else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0;
426	TrimRoll = tmpl - tmpl2 / 100L;	423	TrimRoll = tmpl - tmpl2 / 100L;
Line 542...	Line 539...
542	DebugOut.Analog[13] = Motor[1].SetPoint;	539	DebugOut.Analog[13] = Motor[1].SetPoint;
543	DebugOut.Analog[14] = Motor[2].SetPoint;	540	DebugOut.Analog[14] = Motor[2].SetPoint;
544	DebugOut.Analog[15] = Motor[3].SetPoint;	541	DebugOut.Analog[15] = Motor[3].SetPoint;
Line 545...	Line 542...
545		542
546	//Start I2C Interrupt Mode	-
547	twi_state = 0;	543	//Start I2C Interrupt Mode
548	motor = 0;	544	motor_write = 0;
549	I2C_Start();	545	I2C_Start(TWI_STATE_MOTOR_TX);
Line 550...	Line 546...
550	}	546	}
Line 595...	Line 591...
595	// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);	591	// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
596	CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability);	592	CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability);
597	CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl);	593	CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl);
598	Ki = (10300 / 2) / (Parameter_I_Faktor + 1);	594	Ki = (10300 / 2) / (Parameter_I_Faktor + 1);
599	MAX_GAS = EE_Parameter.Gas_Max;	595	MAX_GAS = EE_Parameter.Gas_Max;
600	MIN_GAS = EE_Parameter.Gas_Min;	596	MIN_GAS = EE_Parameter.Gas_Min;
601	if(Poti4 > 50) HeadFree = 1; else HeadFree = 0;	597	if(Poti4 > 50) HeadFree = 1; else HeadFree = 0;
602	if(HeadFree) {if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;}	598	if(HeadFree) {if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;}
603	}	599	}
Line 604...	Line 600...
604		600
Line 804...	Line 800...
804	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	800	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 805...	Line 801...
805		801
806	if(!NewPpmData-- \|\| (FCFlags & FCFLAG_NOTLANDUNG))	802	if(!NewPpmData-- \|\| (FCFlags & FCFLAG_NOTLANDUNG))
807	{	803	{
808	static int stick_nick,stick_roll;	804	static int stick_nick,stick_roll;
809		805
810	unsigned char angle = 180/15;	806	unsigned char angle = 180/15;
811	signed char sintab[31] = { 0, 4, 8, 11, 14, 16, 16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16, 16};	807	signed char sintab[31] = { 0, 4, 8, 11, 14, 16, 16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16, 16};
812	//signed char costab[24] = {16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16};	808	//signed char costab[24] = {16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16};
Line 825...	Line 821...
825	if(HeadFree)	821	if(HeadFree)
826	{	822	{
827	StickNick = ((cos_h * stick_nick) + (sin_h * stick_roll))/8;	823	StickNick = ((cos_h * stick_nick) + (sin_h * stick_roll))/8;
828	StickRoll = ((cos_h * stick_roll) - (sin_h * stick_nick))/8;	824	StickRoll = ((cos_h * stick_roll) - (sin_h * stick_nick))/8;
829	}	825	}
830	else	826	else
831	{	827	{
832	StickNick = stick_nick;	828	StickNick = stick_nick;
833	StickRoll = stick_roll;	829	StickRoll = stick_roll;
834	}	830	}
835		831
836	StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];	832	StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
837	if(StickGier > 2) StickGier -= 2; else	833	if(StickGier > 2) StickGier -= 2; else
838	if(StickGier < -2) StickGier += 2; else StickGier = 0;	834	if(StickGier < -2) StickGier += 2; else StickGier = 0;
Line 839...	Line 835...
839		835
Line 1203...	Line 1199...
1203	w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln	1199	w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
1204	v = abs(IntegralRoll /512);	1200	v = abs(IntegralRoll /512);
1205	if(v > w) w = v; // grösste Neigung ermitteln	1201	if(v > w) w = v; // grösste Neigung ermitteln
1206	korrektur = w / 8 + 2;	1202	korrektur = w / 8 + 2;
1207	fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;	1203	fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
1208	fehler += MesswertGier / 12;	1204	fehler += MesswertGier / 12;
1209		1205
1210	if(!KompassSignalSchlecht && w < 25)	1206	if(!KompassSignalSchlecht && w < 25)
1211	{	1207	{
1212	GierGyroFehler += fehler;	1208	GierGyroFehler += fehler;
1213	if(NeueKompassRichtungMerken)	1209	if(NeueKompassRichtungMerken)
1214	{	1210	{

Subversion Repositories FlightCtrl

(root)/branches/MartinW_Jeti+/fc.c @ 1756 - Rev 1660 → 1662