WebSVN - FlightCtrl - Diff - Rev 1692 and 1693 - /branches/V0.88e_ACC-HH_HR

                                 // ------------------------- 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
-                                LIMIT_MIN_MAX(tmp_long, -255 * STICK_GAIN, 255 * STICK_GAIN); // more than 2 times the full range makes no sense
+                                LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 255 * STICK_GAIN); // more than the full range makes no sense
                                 HCGas -= 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)
                                 tmp_int2 = tmp_int;
                         HCGas -= 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;
                         // limit deviation from hoover point within the target region
                         if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero
+                        {
-                         unsigned int tmp,min,max;
+                         signed int tmp,min,max;
               if(abs(HeightDeviation) < 60)
+                           {
                             LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point
+                           }
                           else
+                           {
                             tmp = (abs(HeightDeviation) - 60)/32;
                                 if(tmp > 15) tmp = 15;
-                    max = (HoverGasMax * (tmp + 16)) / 16;
+                                if(HeightDeviation > 0)
+                                 {
-                    min = (HoverGasMin * (16 - tmp)) / 16;
+                                  min = (HoverGasMin * (16 - tmp)) / 16;
-                            LIMIT_MIN_MAX(HCGas, min, max); // limit gas around the hoover point
+                              LIMIT_MIN_MAX(HCGas, min, HoverGasMax); // limit gas around the hoover point
+                                 }
-DebugOut.Analog[16] = min;
+                                else
+                                {
-DebugOut.Analog[17] = max;
+                                 max = (HoverGasMax * (tmp + 16)) / 16;
-DebugOut.Analog[18] = SollHoehe;
+                              LIMIT_MIN_MAX(HCGas, HoverGasMin, max); // limit gas around the hoover point
+                                }
+                           }
+                        }
                         // strech control output by inverse attitude projection 1/cos
             // + 1/cos(angle)  ++++++++++++++++++++++++++
                         tmp_long2 = (int32_t)HCGas;
     DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
     if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
     else  SummeNick += DiffNick; // I-Anteil bei HH
     if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
     if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
-//if(Parameter_UserParam5 > 50)
         pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 + SummeNick / Ki; // PI-Regler für Nick
-//else
-//      pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick
     // Motor Vorn
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
     if(pd_ergebnis_nick >  tmp_int) pd_ergebnis_nick =  tmp_int;
     if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;
         DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
     if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung
     else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
     if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
     if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
-//if(Parameter_UserParam5 > 50)
     pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8 + SummeRoll / Ki;   // PI-Regler für Roll
-//else
-//    pd_ergebnis_roll = DiffRoll + SummeRoll / Ki;     // PI-Regler für Roll
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
     if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;
     if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
 //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
+ }
-/*
-if(Parameter_UserParam5 > 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 -= Parameter_UserParam6 * (tmp_motorwert[i] - tmp_int);                    // MotorSmoothing
- }
-*/
                         LIMIT_MIN_MAX(tmp_int,MIN_GAS * 4,MAX_GAS * 4);
                         Motor[i].SetPoint = tmp_int / 4;
                         Motor[i].SetPointLowerBits = (tmp_int % 4)<<1; // (3 bits total)
             tmp_motorwert[i] = tmp_int;
+                }

Subversion Repositories FlightCtrl

(root)/branches/V0.88e_ACC-HH_HR_MartinR/fc.c @ 2154 - Rev 1692 → 1693