WebSVN - FlightCtrl - Diff - Rev 1320 and 1322

 // +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 // +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 // +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 // +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 // +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 // +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // +  POSSIBILITY OF SUCH DAMAGE.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include "main.h"
 #include "eeprom.c"
 #include "mymath.h"
-unsigned char h,m,s;
+unsigned char h,m,s,BaroExpandActive = 0;
 volatile unsigned int I2CTimeout = 100;
 int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
 int TrimNick, TrimRoll;
 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_Gyro_Gier_P = 150;      // Wert : 10-250
+unsigned char Parameter_Gyro_Gier_I = 150;      // Wert : 10-250
 unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
 unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
 unsigned char Parameter_UserParam1 = 0;
 unsigned char Parameter_UserParam2 = 0;
 unsigned char Parameter_UserParam3 = 0;
 //    MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
     MesswertNick = (signed int) AdWertNickFilter / 8;
     MesswertRoll = (signed int) AdWertRollFilter / 8;
     RohMesswertNick = MesswertNick;
     RohMesswertRoll = MesswertRoll;
-//DebugOut.Analog[21] = MesswertNick;
-//DebugOut.Analog[22] = MesswertRoll;
-//DebugOut.Analog[22] = Mess_Integral_Gier;
-//DebugOut.Analog[21] = MesswertNick;
-//DebugOut.Analog[22] = MesswertRoll;
 // Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
         Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L;
         Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L;
  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_Gyro_Gier_P,EE_Parameter.Gyro_Gier_P,10,255);
+ CHK_POTI(Parameter_Gyro_Gier_I,EE_Parameter.Gyro_Gier_I,0,255);
  CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
  CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
  CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
  CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
  CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
  MAX_GAS = EE_Parameter.Gas_Max;
  MIN_GAS = EE_Parameter.Gas_Min;
+}
 //############################################################################
 //
 void MotorRegler(void)
         StickGas  = (StickGas + PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120) / 2;
     GyroFaktor     = (Parameter_Gyro_P + 10.0);
     IntegralFaktor = Parameter_Gyro_I;
-    GyroFaktorGier     = (Parameter_Gyro_P + 10.0);
+    GyroFaktorGier     = (Parameter_Gyro_Gier_P + 10.0);
-    IntegralFaktorGier = Parameter_Gyro_I;
+    IntegralFaktorGier = Parameter_Gyro_Gier_I;
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     //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[22] = FromNaviCtrl_Value.GpsZ;
 //    DebugOut.Analog[24] = MesswertNick/2;
 //    DebugOut.Analog[25] = MesswertRoll/2;
-    DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
+//    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[30] = GPS_Nick;
     DebugOut.Analog[31] = GPS_Roll;
-//    DebugOut.Analog[19] -= DebugOut.Analog[19]/128;
-//    if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++;
-/*    DebugOut.Analog[16] = motor_rx[0];
-    DebugOut.Analog[17] = motor_rx[1];
-    DebugOut.Analog[18] = motor_rx[2];
-    DebugOut.Analog[19] = motor_rx[3];
-    DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
-    DebugOut.Analog[20] /= 14;
-    DebugOut.Analog[21] = motor_rx[4];
-    DebugOut.Analog[22] = motor_rx[5];
-    DebugOut.Analog[23] = motor_rx[6];
-    DebugOut.Analog[24] = motor_rx[7];
-    DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
-*/
-//    DebugOut.Analog[9] = MesswertNick;
-//    DebugOut.Analog[9] = SollHoehe;
-//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
-//    DebugOut.Analog[11] = KompassStartwert;
-//    DebugOut.Analog[10] = Parameter_Gyro_I;
-//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;
-//    DebugOut.Analog[9] = KompassRichtung;
-//    DebugOut.Analog[10] = GasMischanteil;
-//    DebugOut.Analog[3] = HoeheD * 32;
 //    DebugOut.Analog[4] = hoehenregler;
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // 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;
 DebugOut.Analog[16] = BaroExpandActive;
         // if height control is activated
- if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
+ if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick))  // 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 StickGasHoover = 110, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023;
+                static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023;
                 static unsigned long HooverGasFilter = 0;
-                static unsigned char delay = 100;
+                static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0;
             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};
                 // get the current hooverpoint
-                DebugOut.Analog[25] = HooverGas;
+                DebugOut.Analog[21] = HooverGas;
                 DebugOut.Analog[18] = VarioMeter;
                 // if height control is activated by an rc channel
         if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
                 {       // check if parameter is less than activation threshold
                         if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position
                         {   //height control not active
-                                if(!delay--)
-                                {
-                                        // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs
-                                        if( (MessLuftdruck > 1000) && (OCR0A < (255 - OPA_OFFSET_STEP)) )
-                                        {   // increase offset
-                                                ExpandBaro -= 1;
-                                                OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down
-                                                beeptime = 300;
-                                        delay = 250;
+                                if(!delay--)
-                                        }
-                                        // measurement of air pressure close to lower limit and
-                                        else if( (MessLuftdruck < 100) && (OCR0A > OPA_OFFSET_STEP) )
-                                        {   // decrease offset
-                                                ExpandBaro += 1;
-                                                OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up
-                                                beeptime = 300;
-                                        delay = 250;
-                                        }
-                                        else
+                                {
-                                        {
-                                                SollHoehe = HoehenWert;  // update SetPoint with current reading
+                                        HoehenReglerAktiv = 0; // disable height control
-                                                HoehenReglerAktiv = 0; // disable height control
-                                                delay = 1;
+                                        SollHoehe = HoehenWert;  // update SetPoint with current reading
                                         delay = 1;
+                                }
+                        }
                         else
                         {       //height control is activated
                                 HoehenReglerAktiv = 1; // enable height control
+                                delay = 200;
+                        }
+        // Expand the measurement
+                // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs
+          if(!BaroExpandActive)
+                   {
+                        if(MessLuftdruck > 920)
+                        {   // increase offset
+             if(OCR0A < (255 - OPA_OFFSET_STEP))
+                           {
+                                ExpandBaro -= 1;
+                                OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down
+                                beeptime = 300;
+                                delay = 250;
+                                BaroExpandActive = 250;
+                           }
+                           else
+                           {
+                            BaroAtLowerLimit = 1;
+               }
+                        }
+                        // measurement of air pressure close to lower limit and
+                        else
+                        if(MessLuftdruck < 100)
+                        {   // decrease offset
+                          if(OCR0A > OPA_OFFSET_STEP)
+                           {
+                                ExpandBaro += 1;
+                                OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up
+                                beeptime = 300;
+                                delay = 250;
+                                BaroExpandActive = 250;
+                           }
+                           else
+                           {
+                            BaroAtUpperLimit = 1;
+               }
+                        }
+                        else
+                        {
+                            BaroAtUpperLimit = 0;
+                                BaroAtLowerLimit = 0;
+                        }
+                   }
+                   else // delay, because of expanding the Baro-Range
+                   {
+                    // now clear the D-values
+                          SummenHoehe = HoehenWert * SM_FILTER;
+                          VarioMeter = 0;
-                                delay = 200;
+                          BaroExpandActive--;
+                   }
+                }
                 else // no switchable height control
+                {
                         SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung;
+                        HoehenReglerAktiv = 1;
                         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;
                 // 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(StickGas > (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD) )
+                                if(StickGas > (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit)
+                                {
                                         if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN)
+                                        {
                                                 HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN;
                                                 SollHoehe = HoehenWert; // update setpoint to current heigth
+                                        }
                                         HeightTrimmingFlag |= HEIGHT_TRIM_UP;
                                         HeightTrimming += abs(StickGas - (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD));
                                 } // gas stick is below hoover point
-                                else if(StickGas < (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD) )
+                                else if(StickGas < (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit )
+                                {
                                         if(HeightTrimmingFlag & HEIGHT_TRIM_UP)
+                                        {
                                                 HeightTrimmingFlag &= ~HEIGHT_TRIM_UP;
                                                 SollHoehe = HoehenWert; // update setpoint to current heigth
+                                {
                                         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 = HooverGas/STICK_GAIN; // rescale back to stick value
-                                        StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;
+//                                      StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;
-                    if(StickGasHoover < 70) StickGasHoover = 70; else
+//                  if(StickGasHoover < 70) StickGasHoover = 70;
-                    if(StickGasHoover > 150) StickGasHoover = 150;
+//                  else if(StickGasHoover > 150) StickGasHoover = 150;
+                                }
                         } //if MikroKopterFlags & MKFLAG_FLY
                         else
+                        {
-                         SollHoehe = HoehenWert - 200;
+                         SollHoehe = HoehenWert - 400;
-                         StickGasHoover = 110;
+                         StickGasHoover = 120;
+                         }
                         HCGas = HooverGas;      // take hoover gas (neutral point)
+          }
                         tmp_int = (Parameter_Luftdruck_D * (long)(tmp_int)) / 128L; // scale to d-gain parameter
                         LIMIT_MIN_MAX(tmp_int, -127, 255);
                         HCGas -= tmp_int;
                         // ------------------------ D-Part 2: ACC-Z Integral  ------------------------
                         tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
-                        DebugOut.Analog[23] = -tmp_int;
                         LIMIT_MIN_MAX(tmp_int, -127, 255);
                         HCGas -= tmp_int;
                         // limit deviation from hoover point within the target region
                         if( (abs(HeightDeviation) < 150)  && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero
+                        {
                                 LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point
+                        }
             if(BaroExpandActive) HCGas = HooverGas;
                         // ------------------------ D-Part 3: GpsZ  ----------------------------------
                         tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L;
+            LIMIT_MIN_MAX(tmp_int, -127, 255);
-                        DebugOut.Analog[24] = -tmp_int;
+                        HCGas -= tmp_int;
-                        HCGas -= tmp_int;
-                        // strech control output by inverse attitude projection
-// + 1/cos(angle)  ++++++++++++++++++++++++++
+                        // strech control output by inverse attitude projection 1/cos
-                                // strech control output by inverse attitude projection 1/cos
+            // + 1/cos(angle)  ++++++++++++++++++++++++++
                         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;
+                        tmp_long3 /= CosAttitude;
-*/
+                        HCGas = (int16_t)tmp_long3;
                         // update height control gas averaging
                         FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE;
                         // limit height control gas pd-control output
+                  }
                 }// EOF height control active
                 else // HC not active
+                {
                         //update hoover gas stick value when HC is not active
+           if(!EE_Parameter.Hoehe_StickNeutralPoint)
+                    {
-                        StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value
+                         StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value
-                        StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;
+                         StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;
+                        }
+                        else StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint;
+            if(StickGasHoover < 70) StickGasHoover = 70;
+            else if(StickGasHoover > 150) StickGasHoover = 150;
+                }
                 // 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 = 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 /= 8192;
                                         HooverGasMax = 1023;
+                                }
+                        }
+                }
 DebugOut.Analog[25] = HooverGas;
-DebugOut.Analog[26] = HooverGasMax;
+//DebugOut.Analog[26] = HooverGasMax;
-DebugOut.Analog[28] = HooverGasMin;
         }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
         // limit gas to parameter setting
   LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN);
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   DebugOut.Analog[7] = GasMischanteil;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gier-Anteil
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-#define MUL_G  1.0
     GierMischanteil = MesswertGier - sollGier * STICK_GAIN;     // Regler für Gier
-// GierMischanteil = 0;
 #define MIN_GIERGAS  (40*STICK_GAIN)  // unter diesem Gaswert trotzdem Gieren
    if(GasMischanteil > MIN_GIERGAS)
+    {
      if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
      if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);

Subversion Repositories FlightCtrl

(root)/tags/V0.80f/fc.c @ 1861 - Rev 1320 → 1322