WebSVN - FlightCtrl - Diff - Rev 1316 and 1320 - /branches/V0.76g

 // +  POSSIBILITY OF SUCH DAMAGE.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include "main.h"
+#include "eeprom.c"
-#include "eeprom.c"
+#include "mymath.h"
 unsigned char h,m,s;
 volatile unsigned int I2CTimeout = 100;
 void SetNeutral(void)
 //############################################################################
+{
  unsigned char i;
  unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;
-    ServoActive = 0; HEF4017R_ON;
+    HEF4017R_ON;
         NeutralAccX = 0;
         NeutralAccY = 0;
         NeutralAccZ = 0;
     AdNeutralNick = 0;
         AdNeutralRoll = 0;
     FromNaviCtrl_Value.Kalman_MaxFusion = 32;
     Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110;
     Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110;
     Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110;
     Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110;
-//    ServoActive = 1;
     SenderOkay = 100;
+    if(ServoActive)
+         {
+                HEF4017R_ON;
+                DDRD  |=0x80; // enable J7 -> Servo signal
+     }
+}
 //############################################################################
 // Bearbeitet die Messwerte
                                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);
+                         }
     if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
     if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
     if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// all BL-Ctrl connected?
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  if(MissingMotor) if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0)
-   {
-    modell_fliegt = 1;
-        GasMischanteil = MIN_GAS;
-   }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // 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;
   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 HCGas, HeightDeviation;
                 static int HeightTrimming = 0;  // rate for change of height setpoint
                 static int FilterHCGas = 0;
-                static int HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023;
+                static int StickGasHoover = 110, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023;
+                static unsigned long HooverGasFilter = 0;
-                static unsigned long HooverGasFilter = 0;
+                static unsigned char delay = 100;
-                static unsigned char delay = 100;
+            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};
+//        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
                 DebugOut.Analog[25] = HooverGas;
                 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
+                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;
+                CosAttitude = (int16_t)c_sqrt(tmp_long + tmp_long2); // phytagoras gives effective attitude angle in deg
+                LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle
+                CosAttitude = c_cos_8192(CosAttitude);  // cos of actual attitude
                 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
+                        #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.
                 // I.e. it will work only if the gas stick is higher than the hover gas
                 // PD-Control with respect to hoover point
                 // the thrust loss out of horizontal attitude is compensated
                 // the setpoint will be fine adjusted with the gas stick position
                         if(MikroKopterFlags & FLAG_FLY) // trim setpoint only when flying
                         {   // gas stick is above hoover point
-                                if(GasMischanteil > (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD) )
+                                if(StickGas > (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD) )
+                                {
                                         if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN)
+                                        {
                                                 HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN;
                                                 SollHoehe = HoehenWert; // update setpoint to current heigth
+                                        }
                                         HeightTrimmingFlag |= HEIGHT_TRIM_UP;
-                                        HeightTrimming += abs(GasMischanteil - (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD)) / 4;
+                                        HeightTrimming += abs(StickGas - (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD));
                                 } // gas stick is below hoover point
-                                else if(GasMischanteil < (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD) )
+                                else if(StickGas < (StickGasHoover - 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;
+                                        HeightTrimming -= abs(StickGas - (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD));
+                                }
                                 else // Gas Stick in Hoover Range
+                                {
                                         if(HeightTrimmingFlag & (HEIGHT_TRIM_UP|HEIGHT_TRIM_DOWN))
+                                        {
                                 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;
+                                        //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
+                        {
-                        else   SollHoehe = HoehenWert - 200;
+                         SollHoehe = HoehenWert - 200;
+                         StickGasHoover = 110;
+                         }
                         HCGas = HooverGas;      // take hoover gas (neutral point)
+          }
          if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT))
                         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)  ++++++++++++++++++++++++++
+                                // strech control output by inverse attitude projection 1/cos
+                        tmp_long3 = (int32_t)HCGas;
+                        tmp_long3 *= 8192L;
+                        tmp_long3 /= CosAttitude;
+                        HCGas = (int16_t)tmp_long3;
+/*
             tmp_long3 = labs(IntegralNick) + labs(IntegralRoll);
             tmp_long3 /= 1500;//1024 * 2;
             if(tmp_long3 > 29) tmp_long3 = 29;
             HCGas = ((long) HCGas * GAS_TAB[tmp_long3]) / 128L;
+*/
                         // 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
                 if(FilterHCGas > GasMischanteil) FilterHCGas = GasMischanteil; // nicht mehr als Gas
+                         }
                         GasMischanteil = FilterHCGas;
+                  }
                 }// EOF height control active
+                else // HC not active
+                {
+                        //update hoover gas stick value when HC is not active
+                        StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value
+                        StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;
+                }
                 // 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 = (long)GasMischanteil; // take current thrust
+/*                      tmp_long3 = (long)GasMischanteil; // take current thrust
             tmp_long3 = labs(IntegralNick) + labs(IntegralRoll);
             tmp_long3 /= 1500;//1024 * 2;
             if(tmp_long3 > 29) tmp_long3 = 29;
+            tmp_long3 = ((long) GasMischanteil * 128L) / (long) GAS_TAB[tmp_long3];
+*/
+                                tmp_long3 = (int32_t)GasMischanteil; // take current thrust
+                                tmp_long3 *= CosAttitude;            // apply attitude projection
-            tmp_long3 = ((long) GasMischanteil * 128L) / (long) GAS_TAB[tmp_long3];
+                                tmp_long3 /= 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);
 DebugOut.Analog[28] = HooverGasMin;
         }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
-        // limit gas to parameter setting
-   LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN);
+        // 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;
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// all BL-Ctrl connected?
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+  if(MissingMotor)
+  if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0)
+   {
+    modell_fliegt = 1;
-  if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
+        GasMischanteil = MIN_GAS;
+   }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mischer und PI-Regler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Subversion Repositories FlightCtrl

(root)/branches/V0.76g_VarMotorSmoothing/fc.c @ 1673 - Rev 1316 → 1320