WebSVN - FlightCtrl - Diff - Rev 1693 and 1695 - /branches/metro/v0.82a metro

     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;
                 if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;
         if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
         if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
 // Roll +++++++++++++++++++++++++++++++++++++++++++++++++
-        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
+        cnt = 1;// + labs(IntegralFehlerRoll) / 4096;
-        if(labs(IntegralFehlerRoll) > FEHLER_LIMIT1) cnt = 4;
-        ausgleichRoll = 0;
+        if(labs(IntegralFehlerRoll) > FEHLER_LIMIT1) cnt = 4;
         if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8))
+        {
         if(IntegralFehlerRoll >  FEHLER_LIMIT2)
+         {
 #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
 #define OPA_OFFSET_STEP 15
 #else
 #define OPA_OFFSET_STEP 10
 #endif
-                int HCGas, HeightDeviation = 0;
+                int HCGas, HeightDeviation = 0,GasReduction;
                 static int HeightTrimming = 0;  // rate for change of height setpoint
                 static int FilterHCGas = 0;
                 static int StickGasHover = 120, HoverGasMin = 0, HoverGasMax = 1023;
                 static unsigned long HoverGasFilter = 0;
                 static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0;
+                        {
                                 // ------------------------- P-Part ----------------------------
                                 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) / 16L; // p-part
+                                tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part
                                 LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 255 * STICK_GAIN); // more than the full range makes no sense
-                                HCGas -= tmp_long;
+//                              HCGas -= tmp_long;
+DebugOut.Analog[25] = tmp_long;
+GasReduction = tmp_long;
                                 // ------------------------- D-Part 1: Vario Meter ----------------------------
                                 tmp_int = VarioMeter / 8;
-                                LIMIT_MIN_MAX(tmp_int, -181, 181);      // avoid overflow when squared (181^2 = 32761)
+                                LIMIT_MIN_MAX(tmp_int, -127, 128);
+DebugOut.Analog[18] = tmp_int;
-                                tmp_int2 = tmp_int;
+//                              tmp_int2 = tmp_int/4;
-                                LIMIT_MAX(tmp_int2, 8); // limit quadratic part on upward movement to avoid to much gas reduction
+//                              LIMIT_MAX(tmp_int2, 64);
-                                if(tmp_int2 > 0) tmp_int = tmp_int + (tmp_int2 * tmp_int2) / 4;
+//                              if(tmp_int > 0) tmp_int = tmp_int + (tmp_int2 * tmp_int2);
-                                else             tmp_int = tmp_int - (tmp_int2 * tmp_int2) / 4;
+//                              else             tmp_int = tmp_int - (tmp_int2 * tmp_int2);
-                                tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 128L; // scale to d-gain parameter
+                                tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 64L; // scale to d-gain parameter
-                                LIMIT_MIN_MAX(tmp_int,-32 * STICK_GAIN, 64 * STICK_GAIN);
+                                LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN);
-                                HCGas -= tmp_int;
+//                              HCGas -= tmp_int;
+DebugOut.Analog[19] = tmp_int;
+GasReduction += tmp_int;
                         } // EOF no baro range expanding
                         // ------------------------ D-Part 2: ACC-Z Integral  ------------------------
                         tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN);
                         LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN);
-                        HCGas -= tmp_long;
+//                      HCGas -= tmp_long;
+GasReduction += tmp_long;
                         // ------------------------ 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);
-                        HCGas -= tmp_int;
+//                      HCGas -= tmp_int;
+GasReduction += tmp_int;
+//DebugOut.Analog[16] = GasReduction;
+GasReduction = (long)((long)GasReduction * HoverGas) / 512;
+//GasReduction = (long)((long)GasReduction * Poti3) / 64;
+DebugOut.Analog[17] = 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;
               if(abs(HeightDeviation) < 60)

Subversion Repositories FlightCtrl

(root)/branches/metro/v0.82a metro_Akku+/fc.c @ 1833 - Rev 1693 → 1695