WebSVN - FlightCtrl - Diff - Rev 1328 and 1330 - /branches/V0.80f_ACC-HH

 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include "main.h"
 #include "eeprom.c"
+#include "mymath.h"
-#include "mymath.h"
+#include "isqrt.h"
 unsigned char h,m,s,BaroExpandActive = 0;
 volatile unsigned int I2CTimeout = 100;
 //############################################################################
 //
 void MotorRegler(void)
 //############################################################################
+{
-         int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int;
+         int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;
          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;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gaswert ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         GasMischanteil = StickGas;
     if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Empfang schlecht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(SenderOkay < 100)
+        {
                                ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
                            if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+                            {
                              if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
+                            }
                                                    ServoActive = 0;
                            SetNeutral();
                                                    ServoActive = 1;
                                                    DDRD  |=0x80; // enable J7 -> Servo signal
                            Piep(GetActiveParamSetNumber(),120);
+                         }
     StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
         if(StickGier > 2) StickGier -= 2;       else
         if(StickGier < -2) StickGier += 2; else StickGier = 0;
         StickGas  = (StickGas + PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120) / 2;
     GyroFaktor     = (Parameter_Gyro_P + 10.0);
     IntegralFaktor = Parameter_Gyro_I;
     GyroFaktorGier     = (Parameter_Gyro_Gier_P + 10.0);
          StickGier += ExternControl.Gier;
      ExternHoehenValue =  (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
      if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
+    }
     if(StickGas < 0) StickGas = 0;
     if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
-    if(GyroFaktor < 0) GyroFaktor = 0;
+    //if(GyroFaktor < 0) GyroFaktor = 0;
-    if(IntegralFaktor < 0) IntegralFaktor = 0;
+    //if(IntegralFaktor < 0) IntegralFaktor = 0;
     if(abs(StickNick/STICK_GAIN) > MaxStickNick)
+     {
       MaxStickNick = abs(StickNick)/STICK_GAIN;
                 static int FilterHCGas = 0;
                 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)
+     {
                 DebugOut.Analog[21] = HooverGas;
                 DebugOut.Analog[18] = VarioMeter;
                 // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs
           if(!BaroExpandActive)
+                   {
                         if(MessLuftdruck > 920)
                         {   // increase offset
              if(OCR0A < (255 - OPA_OFFSET_STEP))
+                           {
                                 ExpandBaro -= 1;
                                 OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down
                                 beeptime = 300;
                                 BaroExpandActive = 50;
                            }
                            else
+                           {
                             BaroAtLowerLimit = 1;
+               }
+                        }
                         // measurement of air pressure close to lower limit and
                         else
                         if(MessLuftdruck < 100)
                         {   // decrease offset
                           if(OCR0A > OPA_OFFSET_STEP)
+                           {
                                 ExpandBaro += 1;
                                 OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up
                                 beeptime = 300;
                                 BaroExpandActive = 50;
                            }
                            else
+                           {
                             BaroAtUpperLimit = 1;
+               }
+                        }
                         else
+                        {
                             BaroAtUpperLimit = 0;
                                 BaroAtLowerLimit = 0;
+                        }
+                   }
                         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
 J4Low;
-                tmp_long  = IntegralNick/GIER_GRAD_FAKTOR;  // nick angle in deg
-                tmp_long  *= tmp_long;
+                tmp_int  = (int)(IntegralNick/GIER_GRAD_FAKTOR);  // nick angle in deg
-                tmp_long2 = IntegralRoll/GIER_GRAD_FAKTOR;  // roll angle in deg
-                tmp_long2 *= tmp_long2;
+                tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR);  // roll angle in deg
-                CosAttitude = (int16_t)c_sqrt(tmp_long + tmp_long2); // phytagoras gives effective attitude angle in deg
+                CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // 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
                         static unsigned char HeightTrimmingFlag = 0x00;
                         #define HEIGHT_CONTROL_STICKTHRESHOLD 15
                 // Holger original version
                 // start of height control algorithm
                 // the height control is only an attenuation of the actual gas stick.
                                         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(StickGasHoover < 70) StickGasHoover = 70;
 //                  else if(StickGasHoover > 150) StickGasHoover = 150;
+                                }
                         } //if MikroKopterFlags & MKFLAG_FLY
                         else
                         {
                          SollHoehe = HoehenWert - 400;
                          if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint;
                          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
+                        }
             if(BaroExpandActive) HCGas = HooverGas;
                         // ------------------------ D-Part 3: GpsZ  ----------------------------------
                         tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L;
             LIMIT_MIN_MAX(tmp_int, -127, 255);
                         HCGas -= tmp_int;
                         // strech control output by inverse attitude projection 1/cos
             // + 1/cos(angle)  ++++++++++++++++++++++++++
-                        tmp_long3 = (int32_t)HCGas;
+                        tmp_long2 = (int32_t)HCGas;
-                        tmp_long3 *= 8192L;
+                        tmp_long2 *= 8192L;
-                        tmp_long3 /= CosAttitude;
+                        tmp_long2 /= CosAttitude;
-                        HCGas = (int16_t)tmp_long3;
+                        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
+                    {
                          StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value
                          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
                 if((MikroKopterFlags & FLAG_FLY) && !(MikroKopterFlags & FLAG_NOTLANDUNG))
+                {
                         if(HooverGasFilter == 0)  HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
                         if(abs(VarioMeter) < 100) // only on small vertical speed
                         {
-                                tmp_long3 = (int32_t)GasMischanteil; // take current thrust
+                                tmp_long2 = (int32_t)GasMischanteil; // take current thrust
-                                tmp_long3 *= CosAttitude;            // apply attitude projection
+                                tmp_long2 *= CosAttitude;            // apply attitude projection
-                                tmp_long3 /= 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
                                         HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/8L);
-                                        HooverGasFilter += 8L * tmp_long3;
+                                        HooverGasFilter += 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
                                         HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/4L);
-                                        HooverGasFilter += 4L * tmp_long3;
+                                        HooverGasFilter += 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
                                         HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/2L);
-                                        HooverGasFilter += 2L * tmp_long3;
+                                        HooverGasFilter += 2L * tmp_long2;
+                                }
                                 else //later
+                                {
                                         HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE;
-                                        HooverGasFilter += tmp_long3;
+                                        HooverGasFilter += tmp_long2;
+                                }
                                 HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE);
                                 if(EE_Parameter.Hoehe_HoverBand)
+                                {
                                         HooverGasMax = 1023;
+                                }
+                        }
+                }
        LastHCGas = GasMischanteil;
           }
           else
+           {
             GasMischanteil = LastHCGas;
            }
 //DebugOut.Analog[26] = HooverGasMax;
         }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
 J4Low;
   if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // all BL-Ctrl connected?
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(MissingMotor)
   if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0)
+   {
     modell_fliegt = 1;
         GasMischanteil = MIN_GAS;

Subversion Repositories FlightCtrl

(root)/branches/V0.80f_ACC-HH_MartinR/fc.c @ 1728 - Rev 1328 → 1330