WebSVN - FlightCtrl - Diff - Rev 2296 and 2309 - /branches/V2.01a_ACC-HH

 signed int KopplungsteilNickRoll,KopplungsteilRollNick;
 signed int tmp_motorwert[MAX_MOTORS];
 char VarioCharacter = ' ';
 unsigned int HooverGasEmergencyPercent = 0; // The gas value for Emergency landing
 unsigned int GasIsZeroCnt = 0; // to detect that the gas-stick is down for a while
+signed int Variance = 0;
+signed int CosAttitude; // for projection of hoover gas
+unsigned char ACC_AltitudeControl = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Debugwerte zuordnen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4);
     DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4);
     DebugOut.Analog[2] = Mittelwert_AccNick / 4;
     DebugOut.Analog[3] = Mittelwert_AccRoll / 4;
     DebugOut.Analog[4] = (signed int) AdNeutralGier - AdWertGier;
-    DebugOut.Analog[5] = HoehenWert/5;
+    DebugOut.Analog[5] = HoehenWert/10;
-    DebugOut.Analog[6] = AdWertAccHoch;//(Mess_Integral_Hoch / 512);// Aktuell_az;
+    DebugOut.Analog[6] = Aktuell_az;//AdWertAccHoch;//(Mess_Integral_Hoch / 512);
     DebugOut.Analog[8] = KompassValue;
     DebugOut.Analog[9] = UBat;
     DebugOut.Analog[10] = SenderOkay;
     DebugOut.Analog[11] = ErsatzKompassInGrad;
     DebugOut.Analog[12] = Motor[0].SetPoint;
     DebugOut.Analog[14] = Motor[2].SetPoint;
     DebugOut.Analog[15] = Motor[3].SetPoint;
     DebugOut.Analog[20] = ServoNickValue;
     DebugOut.Analog[22] = Capacity.ActualCurrent;
     DebugOut.Analog[23] = Capacity.UsedCapacity;
-        DebugOut.Analog[24] = SollHoehe/5;
+        DebugOut.Analog[24] = SollHoehe/10;
-//    DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ;
-//    DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
     DebugOut.Analog[27] = KompassSollWert;
         DebugOut.Analog[29] = Capacity.MinOfMaxPWM;
     DebugOut.Analog[30] = GPS_Nick;
     DebugOut.Analog[31] = GPS_Roll;
     if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe;
-}
+DebugOut.Analog[16] = Variance;
+DebugOut.Analog[17] = VerticalVelocity;
+DebugOut.Analog[18] = HoehenWertF;
+DebugOut.Analog[25] = Parameter_Hoehe_P;
+DebugOut.Analog[26] = Parameter_Luftdruck_D;
+}
     Mess_IntegralNick = IntegralNick;
     Mess_IntegralRoll = IntegralRoll;
     Mess_Integral_Gier = 0;
     StartLuftdruck = Luftdruck;
     VarioMeter = 0;
+        VerticalVelocitySum = 0;
-    Mess_Integral_Hoch = 0;
+        SummenHoehe = 0;    Mess_Integral_Hoch = 0;
     KompassSollWert = KompassValue;
         KompassSignalSchlecht = 100;
     beeptime = 50;
         Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
         Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
+{
  unsigned char i;
     if(!MotorenEin)
+        {
          FC_StatusFlags &= ~(FC_STATUS_MOTOR_RUN | FC_STATUS_FLY);
+                 FC_StatusFlags2 &= ~FC_STATUS2_WAIT_FOR_TAKEOFF;
                  for(i=0;i<MAX_MOTORS;i++)
+                  {
                    if(!PC_MotortestActive)  MotorTest[i] = 0;
                    Motor[i].SetPoint = MotorTest[i];
                    Motor[i].SetPointLowerBits = 0;
             if(GasMischanteil > 40 && MotorenEin)
+                {
                 if(modell_fliegt < 0xffff) modell_fliegt++;
+                }
             if((modell_fliegt < 256))
-                {
+             {
                 SummeNick = 0;
                 SummeRoll = 0;
                 sollGier = 0;
                 Mess_Integral_Gier = 0;
+                                FC_StatusFlags2 |= FC_STATUS2_WAIT_FOR_TAKEOFF;
+             }
+                        else
+                         {
-                } else FC_StatusFlags |= FC_STATUS_FLY;
+               FC_StatusFlags |= FC_STATUS_FLY;
+                           if(FC_StatusFlags2 & FC_STATUS2_WAIT_FOR_TAKEOFF)
+                           {
+/*
+if(Parameter_UserParam3 > 50)
+ {
+   FromNC_AltitudeSpeed = 15;
+   FromNC_AltitudeSetpoint = 500;
+ }
+*/
+                            if(HoehenWert > 150 || HoehenWert < -350 || !(Parameter_GlobalConfig & CFG_HOEHENREGELUNG))
+                                 {
+                                  FC_StatusFlags2 &= ~FC_STATUS2_WAIT_FOR_TAKEOFF;
+#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
+SpeakHoTT = SPEAK_RISING;
+//SollHoehe = 250;
+#endif
+                                  beeptime = 5000;
+                                 }
+                SummeNick = 0;
+                SummeRoll = 0;
+                Mess_Integral_Gier = 0;
+//                              sollGier = 0;
+                                if(modell_fliegt > 1000) modell_fliegt = 1000;  // for the Hooverpoint-Estimation
+                           }
+/*
+else
+if(Parameter_UserParam3 > 150)
+ {
+if(HoehenWert > 2500) FromNC_AltitudeSpeed = 30;
+else
+if(HoehenWert > 1500) FromNC_AltitudeSpeed = 30;
+else
+ {
+  FromNC_AltitudeSpeed = 10;
+  SummeNick -= SummeNick / 4;
+  SummeRoll -= SummeRoll / 4;
+ }
+ FromNC_AltitudeSetpoint = -300;
+ }
+*/
+                         }
             if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
+                {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // auf Nullwerte kalibrieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                            SetNeutral(0);
                            CalibrationDone = 1;
                                                    ServoActive = 1;
                                                    DDRD  |=0x80; // enable J7 -> Servo signal
 #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
+                                                   if(abs(Aktuell_az - NeutralAccZ) > 5 && ACC_AltitudeControl)  { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; };
+                                                   if(VersionInfo.HardwareError[0]) SpeakHoTT = SPEAK_ERR_SENSOR;
-                                                   SpeakHoTT = SPEAK_CALIBRATE;
+                                                   else SpeakHoTT = SPEAK_CALIBRATE;
 #endif
                            Piep(GetActiveParamSet(),120);
+                         }
+                        }
+                    }
+                                                {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Einschalten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                                                         if(CalibrationDone) FC_StatusFlags |= FC_STATUS_START;
+                                                        StartLuftdruck = Luftdruck;
+                                                        HoehenWertF = 0;
+                                                        HoehenWert = 0;
+                                                        SummenHoehe = 0;
                                                         if(++delay_einschalten > 253)
+                                                        {
                                                                 delay_einschalten = 0;
                                                                 if(!VersionInfo.HardwareError[0] && CalibrationDone && !NC_ErrorCode)
+                                                                {
                 if((MaxStickNick > 64) || (MaxStickRoll > 64))
+                {
                 tmp_long  /= 2;
                 tmp_long2 /= 2;
+                }
-/*              if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
-                {
-                tmp_long  /= 3;
-                tmp_long2 /= 3;
-                }
-*/              if(tmp_long >  (long) FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
+                if(tmp_long >  (long) FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
                 if(tmp_long <  (long)-FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
                 if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
                 if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
+     }
      else
          NeueKompassRichtungMerken = 50; // eine Sekunde zum Einloggen
         };
+     }
     tmp_int  = (long) EE_Parameter.StickGier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
     tmp_int += (EE_Parameter.StickGier_P * StickGier) / 4;
+        if(GasIsZeroCnt > 512) tmp_int = 0; // disable Yawing when Gas-Stick is to Zero
         tmp_int += CompassGierSetpoint;
     sollGier = tmp_int;
     Mess_Integral_Gier -= tmp_int;
     if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
     if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
 #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
 #define OPA_OFFSET_STEP 15
 #else
 #define OPA_OFFSET_STEP 10
 #endif
-                int HCGas, HeightDeviation = 0,GasReduction = 0;
+                int HCGas, GasReduction = 0;
                 static int HeightTrimming = 0;  // rate for change of height setpoint
-                static int FilterHCGas = 0;
+                static int HeightDeviation = 0, FilterHCGas = 0;
                 static unsigned long HoverGasFilter = 0;
                 static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0;
-            int CosAttitude;    // for projection of hoover gas
                 // get the current hooverpoint
                 DebugOut.Analog[21] = HoverGas;
+                        }
+                   }
                    else // delay, because of expanding the Baro-Range
+                   {
                     // now clear the D-values
+#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
+                          if(ACC_AltitudeControl) SummenHoehe = HoehenWert * SA_FILTER;
-                          SummenHoehe = HoehenWert * SM_FILTER;
+                          else SummenHoehe = HoehenWert * SM_FILTER;
+#else
+              SummenHoehe = HoehenWert * SM_FILTER;
+#endif
+                          VerticalVelocitySum = 0;
                           VarioMeter = 0;
                           BaroExpandActive--;
+                   }
                 // the setpoint will be fine adjusted with the gas stick position
                         if(FC_StatusFlags & FC_STATUS_FLY) // trim setpoint only when flying
                         {   // gas stick is above hoover point
                                 if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit)
+                                {
+if(HeightDeviation > 20) SollHoehe = HoehenWertF; // update setpoint to current heigth
                                         if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN)
+                                        {
                                                 FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN;
-                                                SollHoehe = HoehenWert; // update setpoint to current heigth
+                                                SollHoehe = HoehenWertF; // update setpoint to current heigth
+                                        }
                                         FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP;
                                         // Limit the maximum Altitude
                                         if(Parameter_MaximumAltitude && (SollHoehe/100 > Parameter_MaximumAltitude)) AltitudeSetpointTrimming = 0;
                                         else
                                 else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit )
+                                {
                                         if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP)
+                                        {
                                                 FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP;
-                                                SollHoehe = HoehenWert; // update setpoint to current heigth
+                                                SollHoehe = HoehenWertF; // update setpoint to current heigth
+                                        }
                                         FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN;
                                         AltitudeSetpointTrimming = -abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD));
 //                                      HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD));
                                         VarioCharacter = '-';
                                                 WaypointTrimming = 10;
                                                 VarioCharacter = '^';
                                                 if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN)  // changed from sinking to rising
+                                                {
                                                         FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN;
-                                                        SollHoehe = HoehenWert; // update setpoint to current heigth
+                                                        SollHoehe = HoehenWertF; // update setpoint to current heigth
+                                                }
+                                         }
                                          else
                     if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint < SollHoehe) // von NC gesteuert -> sinken
+                                         {
                                                 WaypointTrimming = -10;
                                                 VarioCharacter = 'v';
                                                 if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) // changed from rising to sinking
+                                                {
                                                         FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP;
-                                                        SollHoehe = HoehenWert; // update setpoint to current heigth
+                                                        SollHoehe = HoehenWertF; // update setpoint to current heigth
+                                                }
+                                         }
                                         else
                                         if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN))
+                                        {
-                                                if(!WaypointTrimming) LIMIT_MIN_MAX(SollHoehe, (HoehenWert-128), (HoehenWert+128)) // max. 1m Unterschied
+                                                if(!WaypointTrimming) LIMIT_MIN_MAX(SollHoehe, (HoehenWertF-200), (HoehenWertF+200)) // max. 2m 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)
                            StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning;
                            if(StickGasHover < 70) StickGasHover = 70;
                            else if(StickGasHover > 150) StickGasHover = 150;
+                       }
+                                }
-              if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active
+              if(BaroExpandActive) SollHoehe = HoehenWertF; // update setpoint to current altitude if Expanding is active
                         } //if FCFlags & MKFCFLAG_FLY
                         else
+                        {
                          SollHoehe = HoehenWert - 400;
                          if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint;
                          HoverGas = GasMischanteil;
                          VarioCharacter = '.';
+                         }
                         HCGas = HoverGas;      // take hover gas (neutral point)
+                   }
-         if(HoehenWert > SollHoehe || !(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT))
+         if(HoehenWertF > SollHoehe || !(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT))
+                 {
+ if(!ACC_AltitudeControl)
+  {
                         // 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
                                 HeightDeviation = 0;
                         } // EOF // baro range expanding active
                         else // valid data from air pressure sensor
+                        {
                                 // ------------------------- P-Part ----------------------------
-                                tmp_long = (HoehenWert - SollHoehe); // positive when too high
+                                tmp_long = (HoehenWertF - SollHoehe); // positive when too high
                                 LIMIT_MIN_MAX(tmp_long, -32767L, 32767L);       // avoid overflov when casting to int16_t
                                 HeightDeviation = (int)(tmp_long); // positive when too high
                                 tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part
                                 LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense
                                 GasReduction = tmp_long;
                         // ------------------------ D-Part 3: GpsZ  ----------------------------------
                         tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L;
             LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN);
                         GasReduction += tmp_int;
             GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value
                         // ------------------------                  ----------------------------------
                         HCGas -= GasReduction;
                         // limit deviation from hoover point within the target region
                         if(!AltitudeSetpointTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero
+                        {
                         {  // old version
                                 LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas
                                 GasMischanteil = FilterHCGas;
+                        }
                         else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode
+ }
+ else
+ {
+                        // 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
+                                HeightDeviation = 0;
+                        } // EOF // baro range expanding active
+                        else // valid data from air pressure sensor
+                        {
+                                // ------------------------- P-Part ----------------------------
+                                tmp_long = (HoehenWertF - SollHoehe); // positive when too high
+                                LIMIT_MIN_MAX(tmp_long, -32767L, 32767L);       // avoid overflov when casting to int16_t
+                                HeightDeviation = (int)(tmp_long); // positive when too high
+                                tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part
+                                LIMIT_MIN_MAX(tmp_long, -511 * STICK_GAIN, 512 * STICK_GAIN); // more than full range makes sense
+                                GasReduction = tmp_long;
+                                // ------------------------ D-Part: ACC-Z Integral  ------------------------
+                                tmp_long = VerticalVelocity + AdWertAccHoch * Parameter_Hoehe_ACC_Wirkung/256;
+                                // ------------------------- D-Part: Vario Meter ----------------------------
+                                if(WaypointTrimming) {
+                                        Variance = AltitudeSetpointTrimming * 8;
+                                } else {
+                                        Variance = AltitudeSetpointTrimming * EE_Parameter.Hoehe_Verstaerkung*9/32;
+                                }
+                                tmp_long -= (long)Variance;
+                                tmp_long = (tmp_long * (long)Parameter_Luftdruck_D) / 32; // scale to d-gain parameter
+                                LIMIT_MIN_MAX(tmp_long,-511 * STICK_GAIN, 512 * STICK_GAIN);
+                                GasReduction += tmp_long;
+                        } // EOF no baro range expanding
+DebugOut.Analog[19] = -GasReduction;
+                        HCGas -= GasReduction;
+                        LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limits gas around hover point
+                        // strech control output by inverse attitude projection 1/cos
+            // + 1/cos(angle)  ++++++++++++++++++++++++++
+                        tmp_long2 = (int32_t)HCGas;
+                        tmp_long2 *= 8192L;
+                        tmp_long2 /= CosAttitude;
+                        HCGas = (int16_t)tmp_long2;
+                        // 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; // Qopter: geändert
+ }
+                  }
                 }// EOF height control active
                 else // HC not active
+                {
                         //update hoover gas stick value when HC is not active
+                }
                 // Hover gas estimation by averaging gas control output on small z-velocities
                 // this is done only if height contol option is selected in global config and aircraft is flying
                 if((FC_StatusFlags & FC_STATUS_FLY))// && !(FC_SatusFlags & FC_STATUS_EMERGENCY_LANDING))
+                {
-                        if(HoverGasFilter == 0 || StartTrigger == 1)  HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
+//if(HoverGasFilter == 0 || StartTrigger == 1)  HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
+if(HoverGasFilter == 0 || StartTrigger == 1)  HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(HoverGas); // Qopter: geändert
                         if(StartTrigger == 1) StartTrigger = 2;
                                 tmp_long2 = (int32_t)GasMischanteil; // take current thrust
                                 tmp_long2 *= CosAttitude;            // apply attitude projection
                                 tmp_long2 /= 8192;
                                 // average vertical projected thrust
                                 {   // reduce the time constant of averaging by factor of 2 to get much faster a stable value
                                         HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L);
                                         HoverGasFilter += 4L * tmp_long2;
+                                }
                           else //later
+//if(abs(VerticalVelocity) < 50 && abs(HoehenWertF - SollHoehe) < 256) // Qopter: geändert, Anpassung nötig?
                           if(abs(VarioMeter) < 100 && abs(HoehenWert - SollHoehe) < 256) // only on small vertical speed & difference is small (only descending)
+                                {
                                         HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE;
                                         HoverGasFilter += tmp_long2;
+                                }
+        {
                 // set undefined state to indicate vario off
                 FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN);
         } // EOF no height control
-   // Linits the maximum gas in case of "Out of Range emergency landing"
+   // Limits the maximum gas in case of "Out of Range emergency landing"
    if(NC_To_FC_Flags & NC_TO_FC_EMERGENCY_LANDING)
+        {
          if(GasMischanteil/STICK_GAIN > HooverGasEmergencyPercent && HoverGas) GasMischanteil = HooverGasEmergencyPercent * STICK_GAIN;
-         SollHoehe = HoehenWert; // update setpoint to current heigth
+         SollHoehe = HoehenWertF; // update setpoint to current heigth
           beeptime = 15000;
           BeepMuster = 0x0E00;
+        }
     // limit gas to parameter setting

Subversion Repositories FlightCtrl

(root)/branches/V2.01a_ACC-HH_MartinR/fc.c @ 2384 - Rev 2296 → 2309