WebSVN - FlightCtrl - Diff - Rev 1298 and 1309 - /branches/V0.88e_ACC-HH_HR

 long GIER_GRAD_FAKTOR = 1291;
 signed int KopplungsteilNickRoll,KopplungsteilRollNick;
 unsigned char RequiredMotors = 4;
 unsigned char Motor[MAX_MOTORS];
 signed int tmp_motorwert[MAX_MOTORS];
+#define LIMIT_MIN(value, min) {if(value < min) value = min;}
+#define LIMIT_MAX(value, max) {if(value > max) value = max;}
+#define LIMIT_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
 int MotorSmoothing(int neu, int alt)
+{
  int motor;
 //############################################################################
 //
 void MotorRegler(void)
 //############################################################################
+{
-         int pd_ergebnis_nick,pd_ergebnis_roll,h,tmp_int;
+         int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int;
          int GierMischanteil,GasMischanteil;
      static long SummeNick=0,SummeRoll=0;
      static long sollGier = 0,tmp_long,tmp_long2;
      static long IntegralFehlerNick = 0;
      static long IntegralFehlerRoll = 0;
          static unsigned int RcLostTimer;
          static unsigned char delay_neutral = 0;
          static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
-     static int hoehenregler = 0;
      static char TimerWerteausgabe = 0;
      static char NeueKompassRichtungMerken = 0;
      static long ausgleichNick, ausgleichRoll;
      int IntegralNickMalFaktor,IntegralRollMalFaktor;
          unsigned char i;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Empfang schlecht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(SenderOkay < 100)
+        {
-        if(!PcZugriff)
-         {
-           if(BeepMuster == 0xffff)
-            {
-             beeptime = 15000;
-             BeepMuster = 0x0c00;
-            }
-         }
         if(RcLostTimer) RcLostTimer--;
         else
+         {
           MotorenEin = 0;
           MikroKopterFlags &= ~FLAG_NOTLANDUNG;
 // Höhenregelung
 // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(UBat > BattLowVoltageWarning) GasMischanteil = (GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen
   GasMischanteil *= STICK_GAIN;
+        // if height control is activated
  if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
-  {
+        {
+                #define HOOVER_GAS_AVERAGE 4096L                // 4096 * 2ms = 8.2s averaging
+                #define HC_GAS_AVERAGE 4                        // 4 * 2ms= 8 ms averaging
+                #define OPA_OFFSET_STEP 10
-    int tmp_int;
+                int HCGas, HeightDeviation;
+                static int HeightTrimming = 0;  // rate for change of height setpoint
-        long tmp_long_h;
+                static int FilterHCGas = 0;
-    static int hoover = 400, maxhoover,minhoover,gas_old;
+                static int HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023;
+                static unsigned long HooverGasFilter = 0;
-        static char delay = 100;
+                static unsigned char delay = 100;
+                long tmp_long3;
+        const unsigned char GAS_TAB[31] = {128,128,128,129,129,130,131,132,133,135,136,138,140,142,145,148,151,154,158,162,167,172,178,184,191,199,208,218,229,241,255};
-        unsigned char angle_gas = 0;
+                // get the current hooverpoint
+                DebugOut.Analog[25] = HooverGas;
+                DebugOut.Analog[18] = VarioMeter;
+                // if height control is activated by an rc channel
-    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
+        if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
-    {
-      if(Parameter_MaxHoehe < 50)
+                {       // check if parameter is less than activation threshold
+                        if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position
-      {
+                        {   //height control not active
-       if(!delay--)
+                                if(!delay--)
-            {
+                                {
-                 #define OPA_OFFSET_STEP 10
+                                        // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs
-         if((MessLuftdruck > 1000) && (OCR0A < (255 - OPA_OFFSET_STEP)) )
+                                        if( (MessLuftdruck > 1000) && (OCR0A < (255 - OPA_OFFSET_STEP)) )
-                  {
+                                        {   // increase offset
-                    ExpandBaro -= 1;
+                                                ExpandBaro -= 1;
-                    OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro;
+                                                OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down
-                beeptime = 300;
+                                                beeptime = 300;
-            delay = 250;
+                                        delay = 250;
-                  }
+                                        }
+                                        // measurement of air pressure close to lower limit and
-                  else if((MessLuftdruck < 100) && (OCR0A > OPA_OFFSET_STEP))
+                                        else if( (MessLuftdruck < 100) && (OCR0A > OPA_OFFSET_STEP) )
-                  {
+                                        {   // decrease offset
-                    ExpandBaro += 1;
+                                                ExpandBaro += 1;
-                    OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro;
+                                                OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up
-                    beeptime = 300;
+                                                beeptime = 300;
-            delay = 250;
+                                        delay = 250;
-                  }
+                                        }
-          else
+                                        else
-          {
+                                        {
-                SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
+                                                SollHoehe = HoehenWert;  // update SetPoint with current reading
-            HoehenReglerAktiv = 0;
+                                                HoehenReglerAktiv = 0; // disable height control
-                    delay = 1;
+                                                delay = 1;
+                                        }
-                  }
+                                }
+                        }
+                        else
+                        {       //height control is activated
+                                HoehenReglerAktiv = 1; // enable height control
+                                delay = 200;
+                        }
+                }
-      }
-      else
+                else // no switchable height control
-        {
+                {
+                        SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung;
-         HoehenReglerAktiv = 1;
+                        HoehenReglerAktiv = 1;
-                 delay = 200;
+                }
-    }
-    else
-    {
-     SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20;
-     HoehenReglerAktiv = 1;
-    }
+                if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG))
+                {
+                        #define HEIGHT_TRIM_UP          0x01
+                        #define HEIGHT_TRIM_DOWN        0x02
+                        static unsigned char HeightTrimmingFlag = 0x00;
+                        #define HEIGHT_CONTROL_STICKTHRESHOLD 15 * STICK_GAIN
+                // Holger original version
+                // 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.
-    if(MikroKopterFlags & FLAG_NOTLANDUNG) SollHoehe = 0;
+        if((EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) || !(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER))  // Regler wird über Schalter gesteuert)
-    h = HoehenWert;
+              {  // old version
+                        HCGas = GasMischanteil; // take current stick gas as neutral point for the height control
-    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
+                        HeightTrimming = 0;
-     {
+          }
+                  else
-      int vario;
+                  {
-          static char delay = 1;
+                // alternative height control
-// + P-Part ++++++++++++++++++++++++++
+                // PD-Control with respect to hoover point
-      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
+                // the thrust loss out of horizontal attitude is compensated
+                // the setpoint will be fine adjusted with the gas stick position
-      h = GasMischanteil - h;         // vom Gas abziehen
+                        if(MikroKopterFlags & FLAG_FLY) // trim setpoint only when flying
+                        {   // gas stick is above hoover point
-      if(!delay--)
+                                if(GasMischanteil > (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD) )
+                                {
+                                        if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN)
-           {
+                                        {
-        delay = 2;
+                                                HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN;
+                                                SollHoehe = HoehenWert; // update setpoint to current heigth
-        hoover = (15*hoover + h) / 16 + (GasMischanteil - gas_old);
+                                        }
+                                        HeightTrimmingFlag |= HEIGHT_TRIM_UP;
-                gas_old = GasMischanteil;
+                                        HeightTrimming += abs(GasMischanteil - (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD)) / 4;
+                                } // gas stick is below hoover point
+                                else if(GasMischanteil < (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD) )
+                                {
+                                        if(HeightTrimmingFlag & HEIGHT_TRIM_UP)
+                                        {
+                                                HeightTrimmingFlag &= ~HEIGHT_TRIM_UP;
+                                                SollHoehe = HoehenWert; // update setpoint to current heigth
+                                        }
+                                        HeightTrimmingFlag |= HEIGHT_TRIM_DOWN;
+                                        HeightTrimming -= abs(GasMischanteil - (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD)) / 4;
-                if(hoover < h) hoover++; else hoover--;
+                                }
-        if(EE_Parameter.Hoehe_HoverBand)
+                                else // Gas Stick in Hoover Range
+                                {
+                                        if(HeightTrimmingFlag & (HEIGHT_TRIM_UP|HEIGHT_TRIM_DOWN))
+                                        {
-                 {
+                                                HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP|HEIGHT_TRIM_DOWN);
-                  unsigned int band;
+                                                HeightTrimming = 0;
+                                                SollHoehe = HoehenWert; // update setpoint to current height
+                                                if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;
+                                        }
-                  band = hoover / EE_Parameter.Hoehe_HoverBand;
+                                }
-                  maxhoover = hoover + band; // max. 16%
+                                // Trim height set point
-          minhoover = hoover - band; // max. 16%
+                                if(abs(HeightTrimming) > 512)
+                                {
+                                        SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint
+                                        HeightTrimming = 0;
-                 }
+                                        if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75;
+                                }
+                        } //if MikroKopterFlags & MKFLAG_FLY
+                        else   SollHoehe = HoehenWert - 200;
+                        HCGas = HooverGas;      // take hoover gas (neutral point)
+          }
-                 else
+         if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT))
+                 {
+            // ------------------------- P-Part ----------------------------
+            HeightDeviation = (int)(HoehenWert - SollHoehe); // positive when to high
-                 {
+                        tmp_int = (HeightDeviation * (int)Parameter_Hoehe_P) / 16; // p-part
+                        HCGas -= tmp_int;
-                  maxhoover = 1023;
+                        // ------------------------- D-Part 1: Vario Meter ----------------------------
+                        tmp_int = VarioMeter / 8;
+                        if(tmp_int > 8) tmp_int = 8; // limit quadratic part on upward movement to avoid to much gas reduction
+                        if(tmp_int > 0) tmp_int = VarioMeter + (tmp_int * tmp_int) / 4;
+                        else            tmp_int = VarioMeter - (tmp_int * tmp_int) / 4;
+                        tmp_int = (Parameter_Luftdruck_D * (long)(tmp_int)) / 128L; // scale to d-gain parameter
+                        LIMIT_MIN_MAX(tmp_int, -127, 255);
+                        HCGas -= tmp_int;
+                        // ------------------------ D-Part 2: ACC-Z Integral  ------------------------
+                        tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
+                        DebugOut.Analog[23] = -tmp_int;
+                        LIMIT_MIN_MAX(tmp_int, -127, 255);
+                        HCGas -= tmp_int;
+                        // limit deviation from hoover point within the target region
-          minhoover = 0;
+                        if( (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero
+                        {
-                 }
+                                LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point
+                        }
-           }
+                        // ------------------------ D-Part 3: GpsZ  ----------------------------------
-      if(maxhoover < 200) maxhoover = 200;
+                        tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L;
+                        DebugOut.Analog[24] = -tmp_int;
+                        HCGas -= tmp_int;
+                        // strech control output by inverse attitude projection
+// + 1/cos(angle)  ++++++++++++++++++++++++++
-DebugOut.Analog[25] = hoover;
+            tmp_long3 = labs(IntegralNick) + labs(IntegralRoll);
-DebugOut.Analog[26] = maxhoover;
+            tmp_long3 /= 1500;//1024 * 2;
+            if(tmp_long3 > 29) tmp_long3 = 29;
-DebugOut.Analog[28] = minhoover;
+            HCGas = ((long) HCGas * GAS_TAB[tmp_long3]) / 128L;
-// + D1-Part - Vario ++++++++++++++++++++++++++
+                        // update height control gas averaging
-          vario = VarioMeter / 8;
+                        FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE;
-          if(vario > 8) vario = 8;
+                        // limit height control gas pd-control output
-          if(vario > 0) vario = VarioMeter + (vario * vario) / 4;
+                        LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN);
-          else vario = VarioMeter - (vario * vario) / 4;
+                        // set GasMischanteil to HeightControlGasFilter
-      vario = (Parameter_Luftdruck_D * (long)vario)/128L;    // D-Anteil
+            if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)
-DebugOut.Analog[24] = -vario;
+                         {  // old version
+                if(FilterHCGas > GasMischanteil) FilterHCGas = GasMischanteil; // nicht mehr als Gas
-          if(vario > 255) vario = 255;
+                         }
+                        GasMischanteil = FilterHCGas;
-          else if(vario < -127) vario = -127;
+                  }
-      h -= vario;
+                }// EOF height control active
-// + D2-Part - ACC ++++++++++++++++++++++++++
+                // Hoover 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((MikroKopterFlags & FLAG_FLY) && !(MikroKopterFlags & FLAG_NOTLANDUNG))
-//    tmp_int = (((Mess_Integral_Hoch / 128)/*+AdWertAccHoch*/) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (32);
+                {
+                        if(HooverGasFilter == 0)  HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
+                        if(abs(VarioMeter) < 100) // only on small vertical speed
-      tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
+                        {
+                        tmp_long3 = (long)GasMischanteil; // take current thrust
-DebugOut.Analog[23] = -tmp_int;
+            tmp_long3 = labs(IntegralNick) + labs(IntegralRoll);
-      if(tmp_int > 255) tmp_int = 255;
+            tmp_long3 /= 1500;//1024 * 2;
-      else if(tmp_int < -127) tmp_int = -127;
+            if(tmp_long3 > 29) tmp_long3 = 29;
-      h -= tmp_int;
+            tmp_long3 = ((long) GasMischanteil * 128L) / (long) GAS_TAB[tmp_long3];
+                                // 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
+                                        HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/8L);
-      if(h > maxhoover) h = maxhoover;
+                                        HooverGasFilter += 8L * tmp_long3;
+                                }
+                                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
-          else
+                                        HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/4L);
-      if(h < minhoover) h = minhoover;
+                                        HooverGasFilter += 4L * tmp_long3;
+                                }
+                                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
+                                        HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/2L);
+                                        HooverGasFilter += 2L * tmp_long3;
+                                }
+                                else //later
+                                {
+                                        HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE;
+                                        HooverGasFilter += tmp_long3;
+                                }
+                                HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE);
+                                if(EE_Parameter.Hoehe_HoverBand)
+                                {
-// + D3-Part - GPS ++++++++++++++++++++++++++
+                                        int16_t band;
-      vario = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L;    // D-Anteil
+                                        band = HooverGas / EE_Parameter.Hoehe_HoverBand; // the higher the parameter the smaller the range
-DebugOut.Analog[24] = -vario;
+                                        HooverGasMin = HooverGas - band;
+                                        HooverGasMax = HooverGas + band;
-//        if(vario > 127) vario = 127;
+                                }
+                                else
+                                {       // no limit
+                                        HooverGasMin = 0;
+                                        HooverGasMax = 1023;
+                                }
+                        }
-//        else
+                }
+DebugOut.Analog[25] = HooverGas;
-//      if(vario < -63) vario = -63;
+DebugOut.Analog[26] = HooverGasMax;
+DebugOut.Analog[28] = HooverGasMin;
+        }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
-      h -= vario;
-// + 1/cos(angle)  ++++++++++++++++++++++++++
-      const unsigned char GAS_TAB[31] = {128,128,128,129,129,130,131,132,133,135,136,138,140,142,145,148,151,154,158,162,167,172,178,184,191,199,208,218,229,241,255};
-      tmp_long_h = labs(IntegralNick) + labs(IntegralRoll);
-      tmp_long_h /= 1500;//1024 * 2;
-      if(tmp_long_h > 29) tmp_long_h = 29;
-      h = ((long) h * GAS_TAB[tmp_long_h]) / 128L;
-// + Begrenzung  ++++++++++++++++++++++++++
-      hoehenregler = (hoehenregler*3 + h) / 4;
-      if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN
-       {
-         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN;
-         if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil;
-       }
-      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
-      GasMischanteil = hoehenregler;
-     }
-         else
-         {
-          hoover  =  GasMischanteil;
-          gas_old =  GasMischanteil;
+        // 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.88e_ACC-HH_HR_MartinR/fc.c @ 2161 - Rev 1298 → 1309