WebSVN - FlightCtrl - Diff - Rev 1914 and 1916 - /branches/V0.86d_ACC-HH_HR



int   ErsatzKompassInGrad; // Kompasswert in Grad
int   GierGyroFehler = 0;
char GyroFaktor,GyroFaktorGier;
char IntegralFaktor,IntegralFaktorGier;
int  DiffNick,DiffRoll;
int StickGasHover = 120, HoverGasMin = 0, HoverGasMax = 1023;
//int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
unsigned char Poti[9] = {0,0,0,0,0,0,0,0};
volatile unsigned char SenderOkay = 0;
int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
char MotorenEin = 0,StartTrigger = 0;

int Ki = 10300 / 33;
unsigned char Looping_Nick = 0,Looping_Roll = 0;
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;

 
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
unsigned char Parameter_HoehenSchalter     = 251;      // Wert : 0-250
unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
unsigned char Parameter_Hoehe_GPS_Z = 64;        // Wert : 0-250

unsigned char Parameter_NaviGpsACC;
unsigned char Parameter_NaviOperatingRadius;
unsigned char Parameter_NaviWindCorrection;
unsigned char Parameter_NaviSpeedCompensation;
unsigned char Parameter_ExternalControl;
unsigned char Parameter_GlobalConfig;
unsigned char Parameter_ExtraConfig;
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}; // 15° steps

 

 
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
   Mess_Integral_Gier += MesswertGier;
   ErsatzKompass += MesswertGier;
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
      if(!Looping_Nick && !Looping_Roll && (Parameter_GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
         {
            tmpl3 = (MesswertRoll * winkel_nick) / 2048L;
            tmpl3 *= Parameter_AchsKopplung2; //65
            tmpl3 /= 4096L;

 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;
 else                    TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;

 
  if(Parameter_GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
  {
    if(RohMesswertNick > 256)       MesswertNick += 1 * (RohMesswertNick - 256);
    else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256);
    if(RohMesswertRoll > 256)       MesswertRoll += 1 * (RohMesswertRoll - 256);

 CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);
 CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
 CHK_POTI(Parameter_Servo3,EE_Parameter.Servo3);
 CHK_POTI(Parameter_Servo4,EE_Parameter.Servo4);
 CHK_POTI(Parameter_Servo5,EE_Parameter.Servo5);
 
 CHK_POTI(Parameter_HoehenSchalter,EE_Parameter.MaxHoehe);
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung);
 CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z);
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung);
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I);
 CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D);

 CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl);
 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit);
 CHK_POTI(Parameter_AchsKopplung1,EE_Parameter.AchsKopplung1);
 CHK_POTI(Parameter_AchsKopplung2,EE_Parameter.AchsKopplung2);
 CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection);
 Parameter_GlobalConfig = EE_Parameter.GlobalConfig;
 Parameter_ExtraConfig = EE_Parameter.ExtraConfig;
// 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 / (Parameter_I_Faktor + 1);
 MAX_GAS = EE_Parameter.Gas_Max;

    if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;

 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Empfang schlecht
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(SenderOkay < 100 && !(FC_StatusFlags2 & FC_STATUS2_RC_FAILSAVE_ACTIVE))
        {
        if(RcLostTimer) RcLostTimer--;
        else
         {

                          else
                          {
                               ParamSet_ReadFromEEProm(GetActiveParamSet());
                               LipoDetection(0);
                                                   LIBFC_ReceiverInit(EE_Parameter.Receiver);
                           if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
                            {
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
                            }
                                                   ServoActive = 0;
                           SetNeutral(0);

     ExternHoehenValue =  (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
     if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
    }
    if(StickGas < 0) StickGas = 0;

 

    if(Parameter_GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
 
    if(abs(StickNick/STICK_GAIN) > MaxStickNick)
     {

 
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Bei Empfangsausfall im Flug
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(FC_StatusFlags2 & FC_STATUS2_RC_FAILSAVE_ACTIVE)
   {
    StickNick = -GPS_Nick;
    StickRoll = -GPS_Roll;
        StickGas  = StickGasHover;
        Parameter_GlobalConfig &= ~(CFG_HEADING_HOLD | CFG_DREHRATEN_BEGRENZER);
        Parameter_GlobalConfig |= CFG_HOEHENREGELUNG | CFG_ACHSENKOPPLUNG_AKTIV | CFG_KOMPASS_AKTIV | CFG_GPS_AKTIV | CFG_HOEHEN_SCHALTER | CFG_GPS_AKTIV;
        Parameter_ExtraConfig &= ~(CFG2_HEIGHT_LIMIT | CFG_LEARNABLE_CAREFREE | CFG2_VARIO_BEEP);
        Parameter_HoehenSchalter = 200; // switch on
   }
   else
   if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)
    {
     StickGier = 0;

//  Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(abs(StickGier) > 3) // war 15
     {
//      KompassSignalSchlecht = 1000;
      if(!(Parameter_GlobalConfig & CFG_KOMPASS_FIX))
       {
         NeueKompassRichtungMerken = 50; // eine Sekunde zum Einloggen
        };
     }
    tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²

    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;

 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//  Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(KompassValue >= 0 && (Parameter_GlobalConfig & CFG_KOMPASS_AKTIV))
     {
      if(CalculateCompassTimer-- == 1)
          {
       int w,v,r,fehler,korrektur; // wird von der SPI-Routine auf 1 gesetzt

// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen
  GasMischanteil *= STICK_GAIN;
        // if height control is activated
 if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick))  // Höhenregelung
        {
                #define HOVER_GAS_AVERAGE 16384L                // 16384 * 2ms = 32s averaging
                #define HC_GAS_AVERAGE 4                        // 4 * 2ms= 8ms averaging

 

#define OPA_OFFSET_STEP 10
#endif
                int HCGas, HeightDeviation = 0,GasReduction = 0;
                static int HeightTrimming = 0;  // rate for change of height setpoint
                static int FilterHCGas = 0;
 
                static unsigned long HoverGasFilter = 0;
                static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0;
                static signed char WaypointTrimming = 0;
            int CosAttitude;    // for projection of hoover gas

                          VarioMeter = 0;
                          BaroExpandActive--;
                   }

 
                // if height control is activated by an rc channel
        if(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
                {       // check if parameter is less than activation threshold
                        if(Parameter_HoehenSchalter < 50) // for 3 or 2-state switch height control is disabled in lowest position
                        {   //height control not active
                                if(!delay--)
                                {
                                        HoehenReglerAktiv = 0; // disable height control

                                delay = 200;
                        }
                }
                else // no switchable height control
                {
                        SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_HoehenSchalter) * (int)EE_Parameter.Hoehe_Verstaerkung;
                        HoehenReglerAktiv = 1;
                }

 
                // calculate cos of nick and roll angle used for projection of the vertical hoover gas

                // start of height control algorithm
                // the height control is only an attenuation of the actual gas stick.
                // I.e. it will work only if the gas stick is higher than the hover gas
                // and the hover height will be allways larger than height setpoint.
                FC_StatusFlags2 |= FC_STATUS2_ALTITUDE_CONTROL;
        if((Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) || !(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER))  // Regler wird über Schalter gesteuert)
              {  // old version
                        HCGas = GasMischanteil; // take current stick gas as neutral point for the height control
                        HeightTrimming = 0;
                        // set both flags to indicate no vario mode
                        FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN);

//else 
if(!WaypointTrimming) LIMIT_MIN_MAX(SollHoehe, (HoehenWert-128), (HoehenWert+128)) // max. 1m Unterschied
else                                    WaypointTrimming = 0;
                                                FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN);
                                                HeightTrimming = 0;
                                                if(Parameter_ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;
                                                if(!StartTrigger && HoehenWert > 50)
                                                {
                                                 StartTrigger = 1;
                                                }
                                        }

                                          else SollHoehe += WaypointTrimming;
                                          }
                                        else
                                        SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint
                                        HeightTrimming = 0;
LIMIT_MIN_MAX(SollHoehe, (HoehenWert-512), (HoehenWert+512)); // max. 5m Unterschied
                                        if(Parameter_ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100;
                                        //update hoover gas stick value when setpoint is shifted
                       if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0)
                       {
                           StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value
                           StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning;

                         else StickGasHover = 120;
                         HoverGas = GasMischanteil;
                         }
                        HCGas = HoverGas;      // take hover gas (neutral point)
                   }
         if(HoehenWert > SollHoehe || !(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT))
                 {
                        // from this point the Heigth Control Algorithm is identical for both versions
                        if(BaroExpandActive) // baro range expanding active
                        {
                                HCGas = HoverGas; // hover while expanding baro adc range

                                LIMIT_MIN_MAX(tmp_int, -127, 128);
                                tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter
                                LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN);
                                if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint
                                else
                                if(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode
                                GasReduction += tmp_int;
                        } // EOF no baro range expanding
                        // ------------------------ D-Part 2: ACC-Z Integral  ------------------------
            if(Parameter_Hoehe_ACC_Wirkung)
                         {

                        // update height control gas averaging
                        FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE;
                        // limit height control gas pd-control output
                        LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN);
                        // set GasMischanteil to HeightControlGasFilter
            if(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT)
                        {  // old version
                                LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas
                                GasMischanteil = FilterHCGas;
                        }
                        else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode

                  {
                   StartTrigger = 0;
                   HoverGasFilter = 0;
                   HoverGas = 0;
                  }
        }// EOF Parameter_GlobalConfig & CFG_HEIGHT_CONTROL
        else
        {
                // set undefined state to indicate vario off
                FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN);
        } // EOF no height control

Rev 1914	Rev 1916
Line 91...	Line 91...
91	int ErsatzKompassInGrad; // Kompasswert in Grad	91	int ErsatzKompassInGrad; // Kompasswert in Grad
92	int GierGyroFehler = 0;	92	int GierGyroFehler = 0;
93	char GyroFaktor,GyroFaktorGier;	93	char GyroFaktor,GyroFaktorGier;
94	char IntegralFaktor,IntegralFaktorGier;	94	char IntegralFaktor,IntegralFaktorGier;
95	int DiffNick,DiffRoll;	95	int DiffNick,DiffRoll;
-		96	int StickGasHover = 120, HoverGasMin = 0, HoverGasMax = 1023;
96	//int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;	97	//int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
97	unsigned char Poti[9] = {0,0,0,0,0,0,0,0};	98	unsigned char Poti[9] = {0,0,0,0,0,0,0,0};
98	volatile unsigned char SenderOkay = 0;	99	volatile unsigned char SenderOkay = 0;
99	int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;	100	int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
100	char MotorenEin = 0,StartTrigger = 0;	101	char MotorenEin = 0,StartTrigger = 0;
Line 110...	Line 111...
110	int Ki = 10300 / 33;	111	int Ki = 10300 / 33;
111	unsigned char Looping_Nick = 0,Looping_Roll = 0;	112	unsigned char Looping_Nick = 0,Looping_Roll = 0;
112	unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;	113	unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
Line 113...	Line 114...
113		114
114	unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250	115	unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250
115	unsigned char Parameter_MaxHoehe = 251; // Wert : 0-250	116	unsigned char Parameter_HoehenSchalter = 251; // Wert : 0-250
116	unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32	117	unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32
117	unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250	118	unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
118	unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250	119	unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250
119	unsigned char Parameter_Hoehe_GPS_Z = 64; // Wert : 0-250	120	unsigned char Parameter_Hoehe_GPS_Z = 64; // Wert : 0-250
Line 152...	Line 153...
152	unsigned char Parameter_NaviGpsACC;	153	unsigned char Parameter_NaviGpsACC;
153	unsigned char Parameter_NaviOperatingRadius;	154	unsigned char Parameter_NaviOperatingRadius;
154	unsigned char Parameter_NaviWindCorrection;	155	unsigned char Parameter_NaviWindCorrection;
155	unsigned char Parameter_NaviSpeedCompensation;	156	unsigned char Parameter_NaviSpeedCompensation;
156	unsigned char Parameter_ExternalControl;	157	unsigned char Parameter_ExternalControl;
-		158	unsigned char Parameter_GlobalConfig;
-		159	unsigned char Parameter_ExtraConfig;
157	unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;	160	unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;
158	unsigned char CareFree = 0;	161	unsigned char CareFree = 0;
159	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}; // 15° steps	162	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}; // 15° steps
Line 160...	Line 163...
160		163
Line 407...	Line 410...
407		410
408	// Gier ++++++++++++++++++++++++++++++++++++++++++++++++	411	// Gier ++++++++++++++++++++++++++++++++++++++++++++++++
409	Mess_Integral_Gier += MesswertGier;	412	Mess_Integral_Gier += MesswertGier;
410	ErsatzKompass += MesswertGier;	413	ErsatzKompass += MesswertGier;
411	// Kopplungsanteil +++++++++++++++++++++++++++++++++++++	414	// Kopplungsanteil +++++++++++++++++++++++++++++++++++++
412	if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))	415	if(!Looping_Nick && !Looping_Roll && (Parameter_GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
413	{	416	{
414	tmpl3 = (MesswertRoll * winkel_nick) / 2048L;	417	tmpl3 = (MesswertRoll * winkel_nick) / 2048L;
415	tmpl3 *= Parameter_AchsKopplung2; //65	418	tmpl3 *= Parameter_AchsKopplung2; //65
416	tmpl3 /= 4096L;	419	tmpl3 /= 4096L;
Line 504...	Line 507...
504	if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;	507	if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
505	else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;	508	else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
506	if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;	509	if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
507	else TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;	510	else TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
Line 508...	Line 511...
508		511
509	if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)	512	if(Parameter_GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
510	{	513	{
511	if(RohMesswertNick > 256) MesswertNick += 1 * (RohMesswertNick - 256);	514	if(RohMesswertNick > 256) MesswertNick += 1 * (RohMesswertNick - 256);
512	else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256);	515	else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256);
513	if(RohMesswertRoll > 256) MesswertRoll += 1 * (RohMesswertRoll - 256);	516	if(RohMesswertRoll > 256) MesswertRoll += 1 * (RohMesswertRoll - 256);
Line 577...	Line 580...
577	CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);	580	CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);
578	CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);	581	CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
579	CHK_POTI(Parameter_Servo3,EE_Parameter.Servo3);	582	CHK_POTI(Parameter_Servo3,EE_Parameter.Servo3);
580	CHK_POTI(Parameter_Servo4,EE_Parameter.Servo4);	583	CHK_POTI(Parameter_Servo4,EE_Parameter.Servo4);
581	CHK_POTI(Parameter_Servo5,EE_Parameter.Servo5);	584	CHK_POTI(Parameter_Servo5,EE_Parameter.Servo5);
582	CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe);	-
583	CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe);	585	CHK_POTI(Parameter_HoehenSchalter,EE_Parameter.MaxHoehe);
584	CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung);	586	CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung);
585	CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z);	587	CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z);
586	CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung);	588	CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung);
587	CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I);	589	CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I);
588	CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D);	590	CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D);
Line 601...	Line 603...
601	CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl);	603	CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl);
602	CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit);	604	CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit);
603	CHK_POTI(Parameter_AchsKopplung1,EE_Parameter.AchsKopplung1);	605	CHK_POTI(Parameter_AchsKopplung1,EE_Parameter.AchsKopplung1);
604	CHK_POTI(Parameter_AchsKopplung2,EE_Parameter.AchsKopplung2);	606	CHK_POTI(Parameter_AchsKopplung2,EE_Parameter.AchsKopplung2);
605	CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection);	607	CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection);
-		608	Parameter_GlobalConfig = EE_Parameter.GlobalConfig;
-		609	Parameter_ExtraConfig = EE_Parameter.ExtraConfig;
606	// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);	610	// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
607	CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability);	611	CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability);
608	CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl);	612	CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl);
609	Ki = 10300 / (Parameter_I_Faktor + 1);	613	Ki = 10300 / (Parameter_I_Faktor + 1);
610	MAX_GAS = EE_Parameter.Gas_Max;	614	MAX_GAS = EE_Parameter.Gas_Max;
Line 671...	Line 675...
671	if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;	675	if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;
Line 672...	Line 676...
672		676
673	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	677	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
674	// Empfang schlecht	678	// Empfang schlecht
675	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	679	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
676	if(SenderOkay < 100)	680	if(SenderOkay < 100 && !(FC_StatusFlags2 & FC_STATUS2_RC_FAILSAVE_ACTIVE))
677	{	681	{
678	if(RcLostTimer) RcLostTimer--;	682	if(RcLostTimer) RcLostTimer--;
679	else	683	else
680	{	684	{
Line 745...	Line 749...
745	else	749	else
746	{	750	{
747	ParamSet_ReadFromEEProm(GetActiveParamSet());	751	ParamSet_ReadFromEEProm(GetActiveParamSet());
748	LipoDetection(0);	752	LipoDetection(0);
749	LIBFC_ReceiverInit(EE_Parameter.Receiver);	753	LIBFC_ReceiverInit(EE_Parameter.Receiver);
750	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?	754	if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
751	{	755	{
752	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();	756	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();
753	}	757	}
754	ServoActive = 0;	758	ServoActive = 0;
755	SetNeutral(0);	759	SetNeutral(0);
Line 897...	Line 901...
897	ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;	901	ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
898	if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;	902	if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
899	}	903	}
900	if(StickGas < 0) StickGas = 0;	904	if(StickGas < 0) StickGas = 0;
Line 901...	Line 905...
901		905
Line 902...	Line 906...
902	if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0;	906	if(Parameter_GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0;
903		907
904	if(abs(StickNick/STICK_GAIN) > MaxStickNick)	908	if(abs(StickNick/STICK_GAIN) > MaxStickNick)
905	{	909	{
Line 963...	Line 967...
963		967
964		968
965	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	969	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-		970	// Bei Empfangsausfall im Flug
-		971	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-		972	if(FC_StatusFlags2 & FC_STATUS2_RC_FAILSAVE_ACTIVE)
-		973	{
-		974	StickNick = -GPS_Nick;
-		975	StickRoll = -GPS_Roll;
-		976	StickGas = StickGasHover;
-		977	Parameter_GlobalConfig &= ~(CFG_HEADING_HOLD \| CFG_DREHRATEN_BEGRENZER);
-		978	Parameter_GlobalConfig \|= CFG_HOEHENREGELUNG \| CFG_ACHSENKOPPLUNG_AKTIV \| CFG_KOMPASS_AKTIV \| CFG_GPS_AKTIV \| CFG_HOEHEN_SCHALTER \| CFG_GPS_AKTIV;
-		979	Parameter_ExtraConfig &= ~(CFG2_HEIGHT_LIMIT \| CFG_LEARNABLE_CAREFREE \| CFG2_VARIO_BEEP);
-		980	Parameter_HoehenSchalter = 200; // switch on
966	// Bei Empfangsausfall im Flug	981	}
967	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	982	else
968	if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)	983	if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)
969	{	984	{
970	StickGier = 0;	985	StickGier = 0;
Line 1212...	Line 1227...
1212	// Gieren	1227	// Gieren
1213	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1228	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1214	if(abs(StickGier) > 3) // war 15	1229	if(abs(StickGier) > 3) // war 15
1215	{	1230	{
1216	// KompassSignalSchlecht = 1000;	1231	// KompassSignalSchlecht = 1000;
1217	if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))	1232	if(!(Parameter_GlobalConfig & CFG_KOMPASS_FIX))
1218	{	1233	{
1219	NeueKompassRichtungMerken = 50; // eine Sekunde zum Einloggen	1234	NeueKompassRichtungMerken = 50; // eine Sekunde zum Einloggen
1220	};	1235	};
1221	}	1236	}
1222	tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx²	1237	tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx²
Line 1228...	Line 1243...
1228	if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;	1243	if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
Line 1229...	Line 1244...
1229		1244
1230	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1245	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1231	// Kompass	1246	// Kompass
1232	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1247	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1233	if(KompassValue >= 0 && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))	1248	if(KompassValue >= 0 && (Parameter_GlobalConfig & CFG_KOMPASS_AKTIV))
1234	{	1249	{
1235	if(CalculateCompassTimer-- == 1)	1250	if(CalculateCompassTimer-- == 1)
1236	{	1251	{
1237	int w,v,r,fehler,korrektur; // wird von der SPI-Routine auf 1 gesetzt	1252	int w,v,r,fehler,korrektur; // wird von der SPI-Routine auf 1 gesetzt
Line 1305...	Line 1320...
1305	// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht	1320	// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
1306	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1321	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1307	if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen	1322	if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen
1308	GasMischanteil *= STICK_GAIN;	1323	GasMischanteil *= STICK_GAIN;
1309	// if height control is activated	1324	// if height control is activated
1310	if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll \|\| Looping_Nick)) // Höhenregelung	1325	if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll \|\| Looping_Nick)) // Höhenregelung
1311	{	1326	{
1312	#define HOVER_GAS_AVERAGE 16384L // 16384 * 2ms = 32s averaging	1327	#define HOVER_GAS_AVERAGE 16384L // 16384 * 2ms = 32s averaging
1313	#define HC_GAS_AVERAGE 4 // 4 * 2ms= 8ms averaging	1328	#define HC_GAS_AVERAGE 4 // 4 * 2ms= 8ms averaging
Line 1314...	Line 1329...
1314		1329
Line 1318...	Line 1333...
1318	#define OPA_OFFSET_STEP 10	1333	#define OPA_OFFSET_STEP 10
1319	#endif	1334	#endif
1320	int HCGas, HeightDeviation = 0,GasReduction = 0;	1335	int HCGas, HeightDeviation = 0,GasReduction = 0;
1321	static int HeightTrimming = 0; // rate for change of height setpoint	1336	static int HeightTrimming = 0; // rate for change of height setpoint
1322	static int FilterHCGas = 0;	1337	static int FilterHCGas = 0;
1323	static int StickGasHover = 120, HoverGasMin = 0, HoverGasMax = 1023;	-
1324	static unsigned long HoverGasFilter = 0;	1338	static unsigned long HoverGasFilter = 0;
1325	static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0;	1339	static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0;
1326	static signed char WaypointTrimming = 0;	1340	static signed char WaypointTrimming = 0;
1327	int CosAttitude; // for projection of hoover gas	1341	int CosAttitude; // for projection of hoover gas
Line 1376...	Line 1390...
1376	VarioMeter = 0;	1390	VarioMeter = 0;
1377	BaroExpandActive--;	1391	BaroExpandActive--;
1378	}	1392	}
Line 1379...	Line 1393...
1379		1393
1380	// if height control is activated by an rc channel	1394	// if height control is activated by an rc channel
1381	if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert	1395	if(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert
1382	{ // check if parameter is less than activation threshold	1396	{ // check if parameter is less than activation threshold
1383	if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position	1397	if(Parameter_HoehenSchalter < 50) // for 3 or 2-state switch height control is disabled in lowest position
1384	{ //height control not active	1398	{ //height control not active
1385	if(!delay--)	1399	if(!delay--)
1386	{	1400	{
1387	HoehenReglerAktiv = 0; // disable height control	1401	HoehenReglerAktiv = 0; // disable height control
Line 1395...	Line 1409...
1395	delay = 200;	1409	delay = 200;
1396	}	1410	}
1397	}	1411	}
1398	else // no switchable height control	1412	else // no switchable height control
1399	{	1413	{
1400	SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung;	1414	SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_HoehenSchalter) * (int)EE_Parameter.Hoehe_Verstaerkung;
1401	HoehenReglerAktiv = 1;	1415	HoehenReglerAktiv = 1;
1402	}	1416	}
Line 1403...	Line 1417...
1403		1417
1404	// calculate cos of nick and roll angle used for projection of the vertical hoover gas	1418	// calculate cos of nick and roll angle used for projection of the vertical hoover gas
Line 1415...	Line 1429...
1415	// start of height control algorithm	1429	// start of height control algorithm
1416	// the height control is only an attenuation of the actual gas stick.	1430	// the height control is only an attenuation of the actual gas stick.
1417	// I.e. it will work only if the gas stick is higher than the hover gas	1431	// I.e. it will work only if the gas stick is higher than the hover gas
1418	// and the hover height will be allways larger than height setpoint.	1432	// and the hover height will be allways larger than height setpoint.
1419	FC_StatusFlags2 \|= FC_STATUS2_ALTITUDE_CONTROL;	1433	FC_StatusFlags2 \|= FC_STATUS2_ALTITUDE_CONTROL;
1420	if((EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) \|\| !(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)) // Regler wird über Schalter gesteuert)	1434	if((Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) \|\| !(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER)) // Regler wird über Schalter gesteuert)
1421	{ // old version	1435	{ // old version
1422	HCGas = GasMischanteil; // take current stick gas as neutral point for the height control	1436	HCGas = GasMischanteil; // take current stick gas as neutral point for the height control
1423	HeightTrimming = 0;	1437	HeightTrimming = 0;
1424	// set both flags to indicate no vario mode	1438	// set both flags to indicate no vario mode
1425	FC_StatusFlags \|= (FC_STATUS_VARIO_TRIM_UP\|FC_STATUS_VARIO_TRIM_DOWN);	1439	FC_StatusFlags \|= (FC_STATUS_VARIO_TRIM_UP\|FC_STATUS_VARIO_TRIM_DOWN);
Line 1491...	Line 1505...
1491	//else	1505	//else
1492	if(!WaypointTrimming) LIMIT_MIN_MAX(SollHoehe, (HoehenWert-128), (HoehenWert+128)) // max. 1m Unterschied	1506	if(!WaypointTrimming) LIMIT_MIN_MAX(SollHoehe, (HoehenWert-128), (HoehenWert+128)) // max. 1m Unterschied
1493	else WaypointTrimming = 0;	1507	else WaypointTrimming = 0;
1494	FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP\|FC_STATUS_VARIO_TRIM_DOWN);	1508	FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP\|FC_STATUS_VARIO_TRIM_DOWN);
1495	HeightTrimming = 0;	1509	HeightTrimming = 0;
1496	if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;	1510	if(Parameter_ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;
1497	if(!StartTrigger && HoehenWert > 50)	1511	if(!StartTrigger && HoehenWert > 50)
1498	{	1512	{
1499	StartTrigger = 1;	1513	StartTrigger = 1;
1500	}	1514	}
1501	}	1515	}
Line 1509...	Line 1523...
1509	else SollHoehe += WaypointTrimming;	1523	else SollHoehe += WaypointTrimming;
1510	}	1524	}
1511	else	1525	else
1512	SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint	1526	SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint
1513	HeightTrimming = 0;	1527	HeightTrimming = 0;
1514	LIMIT_MIN_MAX(SollHoehe, (HoehenWert-256), (HoehenWert+256)); // max. 2m Unterschied	1528	LIMIT_MIN_MAX(SollHoehe, (HoehenWert-512), (HoehenWert+512)); // max. 5m Unterschied
1515	if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100;	1529	if(Parameter_ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100;
1516	//update hoover gas stick value when setpoint is shifted	1530	//update hoover gas stick value when setpoint is shifted
1517	if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0)	1531	if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0)
1518	{	1532	{
1519	StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value	1533	StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value
1520	StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning;	1534	StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning;
Line 1531...	Line 1545...
1531	else StickGasHover = 120;	1545	else StickGasHover = 120;
1532	HoverGas = GasMischanteil;	1546	HoverGas = GasMischanteil;
1533	}	1547	}
1534	HCGas = HoverGas; // take hover gas (neutral point)	1548	HCGas = HoverGas; // take hover gas (neutral point)
1535	}	1549	}
1536	if(HoehenWert > SollHoehe \|\| !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT))	1550	if(HoehenWert > SollHoehe \|\| !(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT))
1537	{	1551	{
1538	// from this point the Heigth Control Algorithm is identical for both versions	1552	// from this point the Heigth Control Algorithm is identical for both versions
1539	if(BaroExpandActive) // baro range expanding active	1553	if(BaroExpandActive) // baro range expanding active
1540	{	1554	{
1541	HCGas = HoverGas; // hover while expanding baro adc range	1555	HCGas = HoverGas; // hover while expanding baro adc range
Line 1555...	Line 1569...
1555	LIMIT_MIN_MAX(tmp_int, -127, 128);	1569	LIMIT_MIN_MAX(tmp_int, -127, 128);
1556	tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter	1570	tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter
1557	LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN);	1571	LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN);
1558	if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP\|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint	1572	if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP\|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint
1559	else	1573	else
1560	if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode	1574	if(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode
1561	GasReduction += tmp_int;	1575	GasReduction += tmp_int;
1562	} // EOF no baro range expanding	1576	} // EOF no baro range expanding
1563	// ------------------------ D-Part 2: ACC-Z Integral ------------------------	1577	// ------------------------ D-Part 2: ACC-Z Integral ------------------------
1564	if(Parameter_Hoehe_ACC_Wirkung)	1578	if(Parameter_Hoehe_ACC_Wirkung)
1565	{	1579	{
Line 1608...	Line 1622...
1608	// update height control gas averaging	1622	// update height control gas averaging
1609	FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE;	1623	FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE;
1610	// limit height control gas pd-control output	1624	// limit height control gas pd-control output
1611	LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN);	1625	LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN);
1612	// set GasMischanteil to HeightControlGasFilter	1626	// set GasMischanteil to HeightControlGasFilter
1613	if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)	1627	if(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT)
1614	{ // old version	1628	{ // old version
1615	LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas	1629	LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas
1616	GasMischanteil = FilterHCGas;	1630	GasMischanteil = FilterHCGas;
1617	}	1631	}
1618	else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode	1632	else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode
Line 1677...	Line 1691...
1677	{	1691	{
1678	StartTrigger = 0;	1692	StartTrigger = 0;
1679	HoverGasFilter = 0;	1693	HoverGasFilter = 0;
1680	HoverGas = 0;	1694	HoverGas = 0;
1681	}	1695	}
1682	}// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL	1696	}// EOF Parameter_GlobalConfig & CFG_HEIGHT_CONTROL
1683	else	1697	else
1684	{	1698	{
1685	// set undefined state to indicate vario off	1699	// set undefined state to indicate vario off
1686	FC_StatusFlags \|= (FC_STATUS_VARIO_TRIM_UP\|FC_STATUS_VARIO_TRIM_DOWN);	1700	FC_StatusFlags \|= (FC_STATUS_VARIO_TRIM_UP\|FC_STATUS_VARIO_TRIM_DOWN);
1687	} // EOF no height control	1701	} // EOF no height control

Subversion Repositories FlightCtrl

(root)/branches/V0.86d_ACC-HH_HR_MartinR/fc.c @ 2705 - Rev 1914 → 1916