WebSVN - FlightCtrl - Diff - Rev 1322 and 1328 - /branches/V0.76g

 long GIER_GRAD_FAKTOR = 1291;
 signed int KopplungsteilNickRoll,KopplungsteilRollNick;
 unsigned char RequiredMotors = 4;
 unsigned char Motor[MAX_MOTORS];
 signed int tmp_motorwert[MAX_MOTORS];
+unsigned char LoadHandler = 0;
 #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;}
      static char TimerWerteausgabe = 0;
      static char NeueKompassRichtungMerken = 0;
      static long ausgleichNick, ausgleichRoll;
      int IntegralNickMalFaktor,IntegralRollMalFaktor;
          unsigned char i;
+    if(--LoadHandler == 0) LoadHandler = 5; // verteilt die Prozessorlast
         Mittelwert();
     GRN_ON;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gaswert ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         GasMischanteil = StickGas;
+                        }
+                    }
                 else delay_ausschalten = 0;
+                }
+            }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // neue Werte von der Funke
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     IntegralFehlerNick = tmp_long;
     IntegralFehlerRoll = tmp_long2;
     Mess_IntegralNick2 -= IntegralFehlerNick;
     Mess_IntegralRoll2 -= IntegralFehlerRoll;
-//    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
 //    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
   if(EE_Parameter.Driftkomp)
+   {
     if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }
     if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }
-   }
-//DebugOut.Analog[22] = MittelIntegralRoll / 26;
 //DebugOut.Analog[24] = GierGyroFehler;
-    GierGyroFehler = 0;
-/*DebugOut.Analog[17] = IntegralAccNick / 26;
-DebugOut.Analog[18] = IntegralAccRoll / 26;
-DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
-DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
-*/
-//DebugOut.Analog[21] = MittelIntegralNick / 26;
-//MittelIntegralRoll = MittelIntegralRoll;
-//DebugOut.Analog[28] = ausgleichNick;
-/*
-DebugOut.Analog[29] = ausgleichRoll;
-DebugOut.Analog[30] = LageKorrekturRoll * 10;
+    GierGyroFehler = 0;
 */
 #define FEHLER_LIMIT  (ABGLEICH_ANZAHL / 2)
 #define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4
     DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
     //DebugOut.Analog[16] = Mittelwert_AccHoch;
     //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
     DebugOut.Analog[18] = VarioMeter;
     DebugOut.Analog[19] = WinkelOut.CalcState;
-    DebugOut.Analog[20] = ServoValue;
+    DebugOut.Analog[20] = ServoNickValue;
 //    DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ;
 //    DebugOut.Analog[24] = MesswertNick/2;
 //    DebugOut.Analog[25] = MesswertRoll/2;
 //    DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
 //    DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;
 //    DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
     //DebugOut.Analog[28] = I2CError;
     DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
     DebugOut.Analog[30] = GPS_Nick;
     DebugOut.Analog[31] = GPS_Roll;
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
 // 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;
 DebugOut.Analog[16] = BaroExpandActive;
+        // if height control is activated
-        // if height control is activated
+J4High;
  if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick))  // Höhenregelung
         {
+//              #define HOOVER_GAS_AVERAGE 4096L                // 4096 * 2ms = 8.2s averaging
-                #define HOOVER_GAS_AVERAGE 4096L                // 4096 * 2ms = 8.2s averaging
+                #define HOOVER_GAS_AVERAGE 1024L                // 1024 * 10ms = 10s averaging
-                #define HC_GAS_AVERAGE 4                        // 4 * 2ms= 8 ms averaging
+                #define HC_GAS_AVERAGE 2                        // 2 * 10ms= 20ms averaging
                 #define OPA_OFFSET_STEP 10
                 int HCGas, HeightDeviation;
                 static int HeightTrimming = 0;  // rate for change of height setpoint
                 static int FilterHCGas = 0;
-                static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023;
+                static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023, LastHCGas = 0;
                 static unsigned long HooverGasFilter = 0;
                 static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0;
             int CosAttitude;    // for projection of hoover gas
                 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};
+                // get the current hooverpoint
+    if(LoadHandler == 1)
                 // get the current hooverpoint
                 DebugOut.Analog[21] = 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
-                {       // 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--)
-                                {
-                                        HoehenReglerAktiv = 0; // disable height control
-                                        SollHoehe = HoehenWert;  // update SetPoint with current reading
-                                        delay = 1;
-                                }
-                        }
-                        else
-                        {       //height control is activated
-                                HoehenReglerAktiv = 1; // enable height control
-                                delay = 200;
+                DebugOut.Analog[18] = VarioMeter;
         // Expand the measurement
                 // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs
           if(!BaroExpandActive)
              if(OCR0A < (255 - OPA_OFFSET_STEP))
+                           {
                                 ExpandBaro -= 1;
                                 OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down
                                 beeptime = 300;
-                                delay = 250;
-                                BaroExpandActive = 250;
+                                BaroExpandActive = 50;
+                           }
                            else
+                           {
                             BaroAtLowerLimit = 1;
+               }
                           if(OCR0A > OPA_OFFSET_STEP)
+                           {
                                 ExpandBaro += 1;
                                 OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up
                                 beeptime = 300;
-                                delay = 250;
-                                BaroExpandActive = 250;
+                                BaroExpandActive = 50;
+                           }
                            else
+                           {
                             BaroAtUpperLimit = 1;
+               }
                     // now clear the D-values
                           SummenHoehe = HoehenWert * SM_FILTER;
                           VarioMeter = 0;
                           BaroExpandActive--;
+                   }
+                // if height control is activated by an rc channel
+        if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
+                {       // 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--)
+                                {
+                                        HoehenReglerAktiv = 0; // disable height control
+                                        SollHoehe = HoehenWert;  // update SetPoint with current reading
+                                        delay = 1;
+                                }
+                        }
+                        else
+                        {       //height control is activated
+                                HoehenReglerAktiv = 1; // enable height control
+                                delay = 40;
+                        }
+                }
                 else // no switchable height control
+                {
                         SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung;
                         HoehenReglerAktiv = 1;
+                }
+                // calculate cos of nick and roll angle used for projection of the vertical hoover gas
-                // calculate cos of nick and roll angle used for projection of the vertical hoover gas
+J4Low;
                 tmp_long  = IntegralNick/GIER_GRAD_FAKTOR;  // nick angle in deg
                 tmp_long  *= tmp_long;
                 tmp_long2 = IntegralRoll/GIER_GRAD_FAKTOR;  // roll angle in deg
-                tmp_long2 *= tmp_long2;
                 tmp_long2 *= tmp_long2;
+                CosAttitude = (int16_t)c_sqrt(tmp_long + tmp_long2); // phytagoras gives effective attitude angle in deg
-                CosAttitude = (int16_t)c_sqrt(tmp_long + tmp_long2); // phytagoras gives effective attitude angle in deg
+DebugOut.Analog[16] = CosAttitude;
                 LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle
                 CosAttitude = c_cos_8192(CosAttitude);  // cos of actual attitude
 J4High;
                 if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG))
+                {
                         #define HEIGHT_TRIM_UP          0x01
                         #define HEIGHT_TRIM_DOWN        0x02
                                                 SollHoehe = HoehenWert; // update setpoint to current height
                                                 if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;
+                                        }
+                                }
                                 // Trim height set point
-                                if(abs(HeightTrimming) > 512)
+                                if(abs(HeightTrimming) > 256)
+                                {
-                                        SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint
+                                        SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(512 / 2); // move setpoint
                                         HeightTrimming = 0;
                                         if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75;
                                         //update hoover gas stick value when setpoint is shifted
 //                                      StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value
 //                                      StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;
+                                }
                         } //if MikroKopterFlags & MKFLAG_FLY
                         else
+                        {
                          SollHoehe = HoehenWert - 400;
+                         if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint;
-                         StickGasHoover = 120;
+                         else StickGasHoover = 120;
+                         }
                         HCGas = HooverGas;      // take hoover gas (neutral point)
+          }
                         HCGas -= tmp_int;
                         // ------------------------ D-Part 2: ACC-Z Integral  ------------------------
                         tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
                         LIMIT_MIN_MAX(tmp_int, -127, 255);
                         HCGas -= tmp_int;
                         // limit deviation from hoover point within the target region
                         if( (abs(HeightDeviation) < 150)  && (!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
+                        }
                          StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;
+                        }
                         else StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint;
             if(StickGasHoover < 70) StickGasHoover = 70;
             else if(StickGasHoover > 150) StickGasHoover = 150;
+                        FilterHCGas = GasMischanteil;
+                }
                 // 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
                                         HooverGasMin = 0;
                                         HooverGasMax = 1023;
+                                }
+                        }
+                }
+       LastHCGas = GasMischanteil;
+          }
+          else
+           {
-DebugOut.Analog[25] = HooverGas;
+            GasMischanteil = LastHCGas;
+           }
 //DebugOut.Analog[26] = HooverGasMax;
         }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
+J4Low;
         // limit gas to parameter setting
   LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN);

Subversion Repositories FlightCtrl

(root)/branches/V0.76g_WKN-hef/fc.c @ 1516 - Rev 1322 → 1328