WebSVN - FlightCtrl - Diff - Rev 1588 and 1590 - /branches/V0.84a_var

   GasMischanteil *= STICK_GAIN;
         // if height control is activated
  if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick))  // Höhenregelung
         {
-                #define HOOVER_GAS_AVERAGE 4096L                // 4096 * 2ms = 8.2s averaging
+                #define HOVER_GAS_AVERAGE 4096L         // 4096 * 2ms = 8.2s averaging
                 #define HC_GAS_AVERAGE 4                        // 4 * 2ms= 8ms averaging
                 #define OPA_OFFSET_STEP 10
                 int HCGas, HeightDeviation = 0;
                 static int HeightTrimming = 0;  // rate for change of height setpoint
+                        {
                          SollHoehe = HoehenWert - 400;
                          if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint;
                          else StickGasHover = 120;
+                         }
-                        HCGas = HoverGas;      // take hoover gas (neutral point)
+                        HCGas = HoverGas;      // take hover gas (neutral point)
-          }
+                   }
          if(HoehenWert > SollHoehe || !(EE_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
+                                HeightDeviation = 0;
+                        } // EOF // baro range expanding active
+                        else // valid data from air pressure sensor
+                        {
-            // ------------------------- P-Part ----------------------------
+                                // ------------------------- P-Part ----------------------------
-            tmp_long = (HoehenWert - SollHoehe); // positive when too high
+                                tmp_long = (HoehenWert - SollHoehe); // positive when too high
-                        LIMIT_MIN_MAX(tmp_long, -32767L, 32767L);       // avoid overflov when casting to int16_t
+                                LIMIT_MIN_MAX(tmp_long, -32767L, 32767L);       // avoid overflov when casting to int16_t
-                        HeightDeviation = (int)(tmp_long); // positive when too high
+                                HeightDeviation = (int)(tmp_long); // positive when too high
-                        tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 16L; // p-part
+                                tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 16L; // p-part
-                        LIMIT_MIN_MAX(tmp_long, -255 * STICK_GAIN, 255 * STICK_GAIN); // more than 2 times the full range makes no sense
+                                LIMIT_MIN_MAX(tmp_long, -255 * STICK_GAIN, 255 * STICK_GAIN); // more than 2 times the full range makes no sense
-                        HCGas -= tmp_long;
+                                HCGas -= tmp_long;
-                        // ------------------------- D-Part 1: Vario Meter ----------------------------
+                                // ------------------------- D-Part 1: Vario Meter ----------------------------
-                        tmp_int = VarioMeter / 8;
+                                tmp_int = VarioMeter / 8;
-                        LIMIT_MIN_MAX(tmp_int, -180, 180);      // avoid overflow when squared
+                                LIMIT_MIN_MAX(tmp_int, -181, 181);      // avoid overflow when squared (181^2 = 32761)
-                        tmp_int2 = tmp_int;
+                                tmp_int2 = tmp_int;
-                        LIMIT_MAX(tmp_int2,8); // limit quadratic part on upward movement to avoid to much gas reduction
+                                LIMIT_MAX(tmp_int2, 8); // limit quadratic part on upward movement to avoid to much gas reduction
-                        if(tmp_int2 > 0) tmp_int = tmp_int + (tmp_int2 * tmp_int2) / 4L;
+                                if(tmp_int2 > 0) tmp_int = tmp_int + (tmp_int2 * tmp_int2) / 4;
-                        else             tmp_int = tmp_int - (tmp_int2 * tmp_int2) / 4L;
+                                else             tmp_int = tmp_int - (tmp_int2 * tmp_int2) / 4;
-                        tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 128L; // scale to d-gain parameter
+                                tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 128L; // scale to d-gain parameter
-                        LIMIT_MIN_MAX(tmp_int,-32 * STICK_GAIN, 64 * STICK_GAIN);
+                                LIMIT_MIN_MAX(tmp_int,-32 * STICK_GAIN, 64 * STICK_GAIN);
-                        HCGas -= tmp_int;
+                                HCGas -= tmp_int;
+                        } // EOF no baro range expanding
                         // ------------------------ D-Part 2: ACC-Z Integral  ------------------------
                         tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN);
                         LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN);
                         HCGas -= tmp_long;
-            if(BaroExpandActive) HCGas = HoverGas;
                         // ------------------------ 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);
                         HCGas -= tmp_int;
                 // 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((FCFlags & FCFLAG_FLY) && !(FCFlags & FCFLAG_NOTLANDUNG))
                 {
-                        if(HoverGasFilter == 0)  HoverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
+                        if(HoverGasFilter == 0)  HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
                         if(abs(VarioMeter) < 100) // only on small vertical speed
+                        {
                                 tmp_long2 = (int32_t)GasMischanteil; // take current thrust
                                 tmp_long2 *= CosAttitude;            // apply attitude projection
                                 tmp_long2 /= 8192;
                                 // average vertical projected thrust
                                 if(modell_fliegt < 2000) // the first 4 seconds
                                 {   // reduce the time constant of averaging by factor of 8 to get much faster a stable value
-                                        HoverGasFilter -= HoverGasFilter/(HOOVER_GAS_AVERAGE/8L);
+                                        HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/8L);
                                         HoverGasFilter += 8L * tmp_long2;
+                                }
                                 else if(modell_fliegt < 4000) // the first 8 seconds
                                 {   // reduce the time constant of averaging by factor of 4 to get much faster a stable value
-                                        HoverGasFilter -= HoverGasFilter/(HOOVER_GAS_AVERAGE/4L);
+                                        HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L);
                                         HoverGasFilter += 4L * tmp_long2;
+                                }
                                 else if(modell_fliegt < 8000) // the first 16 seconds
                                 {   // reduce the time constant of averaging by factor of 2 to get much faster a stable value
-                                        HoverGasFilter -= HoverGasFilter/(HOOVER_GAS_AVERAGE/2L);
+                                        HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/2L);
                                         HoverGasFilter += 2L * tmp_long2;
+                                }
                                 else //later
+                                {
-                                        HoverGasFilter -= HoverGasFilter/HOOVER_GAS_AVERAGE;
+                                        HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE;
                                         HoverGasFilter += tmp_long2;
+                                }
-                                HoverGas = (int16_t)(HoverGasFilter/HOOVER_GAS_AVERAGE);
+                                HoverGas = (int16_t)(HoverGasFilter/HOVER_GAS_AVERAGE);
                                 if(EE_Parameter.Hoehe_HoverBand)
+                                {
                                         int16_t band;

Subversion Repositories FlightCtrl

(root)/branches/V0.84a_var_motorsmoothing/fc.c @ 2312 - Rev 1588 → 1590