WebSVN - FlightCtrl - Diff - Rev 1279 and 1283

 unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
 unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250
 unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
 unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
 unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
+unsigned char Parameter_Hoehe_GPS_Z = 64;        // Wert : 0-250
 unsigned char Parameter_Gyro_D = 8;             // Wert : 0-250
 unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250
 unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
 unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
 unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
  #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; }
  CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
  CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
  CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
  CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
+ CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z,0,255);
  CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
  CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
  CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
  CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,0,255);
  CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
     DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4);
     DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4);
     DebugOut.Analog[2] = Mittelwert_AccNick / 4;
     DebugOut.Analog[3] = Mittelwert_AccRoll / 4;
     DebugOut.Analog[4] = MesswertGier;
-    DebugOut.Analog[5] = HoehenWert;
+    DebugOut.Analog[5] = HoehenWert/5;
     DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az;
     DebugOut.Analog[8] = KompassValue;
     DebugOut.Analog[9] = UBat;
-    DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
     DebugOut.Analog[10] = SenderOkay;
+    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[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;
  if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
+  {
     int tmp_int;
         long tmp_long_h;
-    static int hoover = 400, maxhoover,minhoover;
+    static int hoover = 400, maxhoover,minhoover,gas_old;
         static char delay = 100;
         unsigned char angle_gas = 0;
     if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
+    {
       h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
       h = GasMischanteil - h;         // vom Gas abziehen
       if(!delay--)
+           {
         delay = 2;
-        hoover = (15*hoover + h) / 16;
+        hoover = (15*hoover + h) / 16 + (GasMischanteil - gas_old);
+                gas_old = GasMischanteil;
                 if(hoover < h) hoover++; else hoover--;
+        if(EE_Parameter.Hoehe_HoverBand)
+                 {
+                  unsigned int band;
+                  band = hoover / EE_Parameter.Hoehe_HoverBand;
-        maxhoover = hoover + hoover/6; // max. 16%
+                  maxhoover = hoover + band; // max. 16%
-        minhoover = hoover - hoover/6; // max. 16%
+          minhoover = hoover - band; // max. 16%
+                 }
+                 else
+                 {
+                  maxhoover = 1023;
+          minhoover = 0;
+                 }
+           }
       if(maxhoover < 200) maxhoover = 200;
 DebugOut.Analog[25] = hoover;
 DebugOut.Analog[26] = maxhoover;
 DebugOut.Analog[28] = minhoover;
           if(vario > 8) vario = 8;
           if(vario > 0) vario = VarioMeter + (vario * vario) / 4;
           else vario = VarioMeter - (vario * vario) / 4;
       vario = (Parameter_Luftdruck_D * (long)vario)/128L;    // D-Anteil
 DebugOut.Analog[24] = -vario;
-          if(vario > 63) vario = 63;
+          if(vario > 255) vario = 255;
-          else if(vario < -63) vario = -63;
+          else if(vario < -127) vario = -127;
       h -= vario;
 // + D2-Part - ACC ++++++++++++++++++++++++++
 //    tmp_int = (((Mess_Integral_Hoch / 128)/*+AdWertAccHoch*/) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (32);
       tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
 DebugOut.Analog[23] = -tmp_int;
-      if(tmp_int > 127) tmp_int = 127;
+      if(tmp_int > 255) tmp_int = 255;
-      else if(tmp_int < -63) tmp_int = -63;
+      else if(tmp_int < -127) tmp_int = -127;
       h -= tmp_int;
       if(h > maxhoover) h = maxhoover;
+          else
+      if(h < minhoover) h = minhoover;
+// + D3-Part - GPS ++++++++++++++++++++++++++
+      vario = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L;    // D-Anteil
+DebugOut.Analog[24] = -vario;
+//        if(vario > 127) vario = 127;
+//        else
+//      if(vario < -63) vario = -63;
+      h -= vario;
           else
       if(h < minhoover) h = minhoover;
 // + 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);
          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;
+         }
+  }
   if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Subversion Repositories FlightCtrl

(root)/tags/V0.80f/fc.c @ 2167 - Rev 1279 → 1283