WebSVN - FlightCtrl - Diff - Rev 1697 and 1698 - /branches/V0.82b-Arthur-P/fc.c

 int  DiffNick,DiffRoll;
 //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;
+char MotorenEin = 0,StartTrigger = 0;
 long HoehenWert = 0;
 long SollHoehe = 0;
 int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0;
 //float Ki =  FAKTOR_I;
 int Ki = 10300 / 33;
   if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen
   GasMischanteil *= STICK_GAIN;
         // if height control is activated
  if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick))  // Höhenregelung
+        {
-                #define HOVER_GAS_AVERAGE 16384L                // 4096 * 2ms = 8.2s averaging
+                #define HOVER_GAS_AVERAGE 16384L                // 16384 * 2ms = 32s averaging
                 #define HC_GAS_AVERAGE 4                        // 4 * 2ms= 8ms averaging
 #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
 #define OPA_OFFSET_STEP 15
+                                        {
                                                 HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN);
                                                 HeightTrimming = 0;
                                                 SollHoehe = HoehenWert; // update setpoint to current height
                                                 if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;
+                                                if(!StartTrigger && HoehenWert > 50)
+                                                {
+                                                 StartTrigger = 1;
+                                                }
+                                        }
                                         VarioCharacter = '=';
+                                }
                                 // Trim height set point
                                 if(abs(HeightTrimming) > 512)
                         else
+                        {
                          SollHoehe = HoehenWert - 400;
                          if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint;
                          else StickGasHover = 120;
+                         HoverGas = GasMischanteil;
+                         }
                         HCGas = HoverGas;      // take hover gas (neutral point)
+                   }
          if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT))
+                 {
                                 // ------------------------- P-Part ----------------------------
                                 tmp_long = (HoehenWert - 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, 255 * STICK_GAIN); // more than the full range makes no sense
+                                LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense
-//                              HCGas -= tmp_long;
 DebugOut.Analog[25] = tmp_long;
-GasReduction = tmp_long;
+                                GasReduction = tmp_long;
                                 // ------------------------- D-Part 1: Vario Meter ----------------------------
                                 tmp_int = VarioMeter / 8;
                                 LIMIT_MIN_MAX(tmp_int, -127, 128);
 DebugOut.Analog[18] = tmp_int;
 //                              tmp_int2 = tmp_int/4;
 //                              LIMIT_MAX(tmp_int2, 64);
 //                              if(tmp_int > 0) tmp_int = tmp_int + (tmp_int2 * tmp_int2);
 //                              else             tmp_int = tmp_int - (tmp_int2 * tmp_int2);
+if(!HeightTrimmingFlag)
+                                {
-                                tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter
+                                 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);
+                                 LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN);
+                                }
-//                              HCGas -= tmp_int;
+else                    {
-DebugOut.Analog[19] = tmp_int;
+                                 tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 16L; // weniger D zum Höhe ändern
+                                 LIMIT_MIN_MAX(tmp_int,-16 * STICK_GAIN, 16 * STICK_GAIN);
+                                }
-GasReduction += tmp_int;
+DebugOut.Analog[19] = tmp_int;
                                 GasReduction += tmp_int;
+                        } // EOF no baro range expanding
+                        // ------------------------ D-Part 2: ACC-Z Integral  ------------------------
-                        } // EOF no baro range expanding
+            if(Parameter_Hoehe_ACC_Wirkung)
-                        // ------------------------ 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);
+                          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;
+                          GasReduction += tmp_long;
 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);
+                        tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L;
-//                      HCGas -= tmp_int;
+            LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN);
-GasReduction += tmp_int;
+                        GasReduction += tmp_int;
 //DebugOut.Analog[16] = GasReduction;
                         // set GasMischanteil to HeightControlGasFilter
             if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)
                         {  // old version
                                 LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas
+                        }
-                        GasMischanteil = FilterHCGas;
+                        GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4;
+                  }
                 }// EOF height control active
                 else // HC not active
+                {
                         //update hoover gas stick value when HC is not active
                         FilterHCGas = GasMischanteil;
+                }
                 // 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((FCFlags & FCFLAG_FLY))// && !(FCFlags & FCFLAG_NOTLANDUNG))
                 {
-                        if(HoverGasFilter == 0)  HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
+                        if(HoverGasFilter == 0 || StartTrigger == 1)  HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
-                        if(abs(VarioMeter) < 100) // only on small vertical speed
                         if(StartTrigger == 1) StartTrigger = 2;
                                 tmp_long2 = (int32_t)GasMischanteil; // take current thrust
                                 tmp_long2 *= CosAttitude;            // apply attitude projection
-                                tmp_long2 /= 8192;
                                 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/(HOVER_GAS_AVERAGE/8L);
-                                        HoverGasFilter += 8L * tmp_long2;
-                                }
+                                // average vertical projected thrust
-                                else if(modell_fliegt < 4000) // the first 8 seconds
+                            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/(HOVER_GAS_AVERAGE/4L);
+                                        HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/16L);
-                                        HoverGasFilter += 4L * tmp_long2;
+                                        HoverGasFilter += 16L * tmp_long2;
                                 }
-                                else if(modell_fliegt < 8000) // the first 16 seconds
+                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/(HOVER_GAS_AVERAGE/2L);
+                                        HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L);
-                                        HoverGasFilter += 2L * tmp_long2;
+                                        HoverGasFilter += 4L * tmp_long2;
                                 }
+                          else //later
-                                else //later
+                          if(abs(VarioMeter) < 100) // only on small vertical speed
+                                {
                                         HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE;
                                         HoverGasFilter += tmp_long2;
+                                }
                                 else
                                 {       // no limit
                                         HoverGasMin = 0;
                                         HoverGasMax = 1023;
+                                }
-                        }
+                }
+                 else
+                  {
+                   StartTrigger = 0;
+                   HoverGasFilter = 0;
+                   HoverGas = 0;
-                }
+                  }
         }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
+DebugOut.Analog[16] = StartTrigger;
         // limit gas to parameter setting
   LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN);
   if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;

Subversion Repositories FlightCtrl

(root)/branches/V0.82b-Arthur-P/fc.c @ 2121 - Rev 1697 → 1698