WebSVN - FlightCtrl - Diff - Rev 1698 and 1701 - /branches/V0.82b-Arthur-P/fc.c

     DebugOut.Analog[10] = SenderOkay;
     DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
     DebugOut.Analog[20] = ServoNickValue;
     DebugOut.Analog[22] = Capacity.ActualCurrent;
     DebugOut.Analog[23] = Capacity.UsedCapacity;
-DebugOut.Analog[24] = AdWertAccHoch;
 //    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[29] = Capacity.MinOfMaxPWM;
     DebugOut.Analog[30] = GPS_Nick;
     DebugOut.Analog[31] = GPS_Roll;
+}
       if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
       if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
       if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
+     }
 //if(Poti2 > 20) { tmp_long = 0; tmp_long2 = 0;}
    Mess_IntegralNick -= tmp_long;
    Mess_IntegralRoll -= tmp_long2;
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  } //  ZaehlMessungen >= ABGLEICH_ANZAHL
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Gieren
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-//    if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(abs(StickGier) > 15) // war 35
+     {
       KompassSignalSchlecht = 1000;
       if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))
     if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-//DebugOut.Analog[18] = KompassSignalSchlecht;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
+     {
        int w,v,r,fehler,korrektur;
        w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
                   ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
           KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
           NeueKompassRichtungMerken = 0;
+         }
+        }
-//DebugOut.Analog[16] = fehler;
-//DebugOut.Analog[17] = korrektur;
        ErsatzKompass += (fehler * 16) / korrektur;
        w = (w * Parameter_KompassWirkung) / 32;           // auf die Wirkung normieren
        w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
        if(w >= 0)
+        {
           if(!KompassSignalSchlecht)
+          {
            v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
            r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
-//           r = KompassRichtung;
            v = (r * w) / v;  // nach Kompass ausrichten
            w = 3 * Parameter_KompassWirkung;
            if(v > w) v = w; // Begrenzen
            else
            if(v < -w) v = -w;
                                 tmp_long = (HoehenWert - SollHoehe); // positive when too high
                                 LIMIT_MIN_MAX(tmp_long, -32767L, 32767L);       // avoid overflov when casting to int16_t
                                 HeightDeviation = (int)(tmp_long); // positive when too high
                                 tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part
                                 LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense
-DebugOut.Analog[25] = tmp_long;
                                 GasReduction = tmp_long;
                                 // ------------------------- D-Part 1: Vario Meter ----------------------------
                                 tmp_int = VarioMeter / 8;
                                 LIMIT_MIN_MAX(tmp_int, -127, 128);
-DebugOut.Analog[18] = tmp_int;
-//                              tmp_int2 = tmp_int/4;
-//                              LIMIT_MAX(tmp_int2, 64);
-//                              if(tmp_int > 0) tmp_int = tmp_int + (tmp_int2 * tmp_int2);
-//                              else             tmp_int = tmp_int - (tmp_int2 * tmp_int2);
-if(!HeightTrimmingFlag)
+                                if(!HeightTrimmingFlag)
+                                {
                                  tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter
                                  LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN);
+                                }
+                                else
-else                    {
+                                {
                                  tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 16L; // weniger D zum Höhe ändern
                                  LIMIT_MIN_MAX(tmp_int,-16 * STICK_GAIN, 16 * STICK_GAIN);
+                                }
-DebugOut.Analog[19] = tmp_int;
                                 GasReduction += tmp_int;
                         } // EOF no baro range expanding
                         // ------------------------ D-Part 2: ACC-Z Integral  ------------------------
             if(Parameter_Hoehe_ACC_Wirkung)
+                         {
+                         }
                         // ------------------------ D-Part 3: GpsZ  ----------------------------------
                         tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L;
             LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN);
                         GasReduction += tmp_int;
+            GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value
-//DebugOut.Analog[16] = GasReduction;
-GasReduction = (long)((long)GasReduction * HoverGas) / 512;
-//GasReduction = (long)((long)GasReduction * Poti3) / 64;
-DebugOut.Analog[17] = GasReduction;
+                        // ------------------------                  ----------------------------------
-HCGas -= GasReduction;
                         HCGas -= GasReduction;
                         // limit deviation from hoover point within the target region
                         if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero
+                        {
                          signed int tmp,min,max;
                    StartTrigger = 0;
                    HoverGasFilter = 0;
                    HoverGas = 0;
+                  }
         }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
-DebugOut.Analog[16] = StartTrigger;
         // 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;
             // Gier
                         if(Mixer.Motor[i][3] == 64) tmp_int += GierMischanteil;
                         else if(Mixer.Motor[i][3] == -64) tmp_int -= GierMischanteil;
                         else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;
-//if(Parameter_UserParam4 < 50)
                         if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2;      // MotorSmoothing
-                        else tmp_int = 2 * tmp_int - tmp_motorwert[i];                                                              // MotorSmoothing
                         else tmp_int = 2 * tmp_int - tmp_motorwert[i];                                                              // MotorSmoothing
                         LIMIT_MIN_MAX(tmp_int,MIN_GAS * 4,MAX_GAS * 4);
                         Motor[i].SetPoint = tmp_int / 4;

Subversion Repositories FlightCtrl

(root)/branches/V0.82b-Arthur-P/fc.c @ 2121 - Rev 1698 → 1701