WebSVN - FlightCtrl - Diff - Rev 2366 and 2367

 unsigned int GasIsZeroCnt = 0; // to detect that the gas-stick is down for a while
 signed int Variance = 0;
 signed int CosAttitude; // for projection of hoover gas
 unsigned char ACC_AltitudeControl = 0;
+#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
+#define OPA_OFFSET_STEP 5
+#else
+#define OPA_OFFSET_STEP 10
+#endif
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Debugwerte zuordnen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 void CopyDebugValues(void)
+{
     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] = (signed int) AdNeutralGier - AdWertGier;
-    DebugOut.Analog[5] = HoehenWert/10;
+        DebugOut.Analog[5] = HoehenWert/10;
     DebugOut.Analog[6] = Aktuell_az;//AdWertAccHoch;//(Mess_Integral_Hoch / 512);
     DebugOut.Analog[8] = KompassValue;
     DebugOut.Analog[9] = UBat;
     DebugOut.Analog[10] = SenderOkay;
 //  Parameter: 2 -> calibrate and store ACC
 unsigned char SetNeutral(unsigned char AdjustmentMode)  // retuns: "sucess"
 //############################################################################
+{
         unsigned char i, sucess = 1;
-        unsigned int gier_neutral = 0, nick_neutral = 0, roll_neutral = 0, acc_z_neutral = 0;
+        unsigned int gier_neutral = 0, nick_neutral = 0, roll_neutral = 0, acc_z_neutral = 0, barotest;
     VersionInfo.HardwareError[0] = 0;
 //    HEF4017Reset_ON;
         NeutralAccX = 0;
         NeutralAccY = 0;
         NeutralAccZ = 0;
         CalibrierMittelwert();
     if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+     {
+      if((MessLuftdruck > 950) || (MessLuftdruck < 750) || ExpandBaro) SucheLuftruckOffset();
+     }
       if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
+        barotest = MessLuftdruck;
+#define NEUTRAL_FILTER 32
         OCR0A += OPA_OFFSET_STEP;
-#define NEUTRAL_FILTER 32
+        OCR0B = 255 - OCR0A;
     for(i=0; i<NEUTRAL_FILTER; i++)
+         {
           Delay_ms_Mess(10);
           gier_neutral += AdWertGier;
           nick_neutral += AdWertNick;
           roll_neutral += AdWertRoll;
+          acc_z_neutral += Aktuell_az;
+         }
+        if(MessLuftdruck < 1010 && MessLuftdruck > 20) BaroStep = barotest - MessLuftdruck;
-          acc_z_neutral += Aktuell_az;
+        OCR0A -= OPA_OFFSET_STEP;
         OCR0B = 255 - OCR0A;
      AdNeutralNick = (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
          AdNeutralRoll = (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
+    }
         EEAR = EE_DUMMY;  // Set the EEPROM Address pointer to an unused space
         MesswertNick = 0;
     MesswertRoll = 0;
     MesswertGier = 0;
-    Delay_ms_Mess(100);
+    Delay_ms_Mess(200);
     Mittelwert_AccNick = ACC_AMPLIFY * AdWertAccNick;
     Mittelwert_AccRoll = ACC_AMPLIFY * AdWertAccRoll;
     IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
     IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
     Mess_IntegralNick = IntegralNick;
     Mess_IntegralRoll = IntegralRoll;
     Mess_Integral_Gier = 0;
-    StartLuftdruck = Luftdruck;
-    VarioMeter = 0;
-        SummenHoehe = 0;    Mess_Integral_Hoch = 0;
     KompassSollWert = KompassValue;
         KompassSignalSchlecht = 100;
     beeptime = 50;
         Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
         Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
     carefree_old = 70;
 #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
         LIBFC_HoTT_Clear();
         ACC_AltitudeFusion(2); // initalisation
 #endif
+    StartLuftdruck = Luftdruck;
+    VarioMeter = 0;
+        SummenHoehe = 0;    Mess_Integral_Hoch = 0;
+        CalcExpandBaroStep();
  return(sucess);
+}
+                                                {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Einschalten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                                                         if(CalibrationDone) FC_StatusFlags |= FC_STATUS_START;
                                                         StartLuftdruck = Luftdruck;
                                                         HoehenWertF = 0;
                                                         HoehenWert = 0;
                                                         SummenHoehe = 0;
-if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -100 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 100) && EE_Parameter.MotorSafetySwitch == 0) delay_einschalten = 0;
+                                                        if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -100 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 100) && EE_Parameter.MotorSafetySwitch == 0) delay_einschalten = 0;
                                                         if(++delay_einschalten > 253)
+                                                        {
+if((abs(MesswertGier) > 32 || abs(MesswertNick) > 20) || abs(MesswertRoll) > 20)  CalibrationDone = 0; // dann ist der Gyro defekt, schlecht kalibriert oder der MK dreht sich
                                                                 delay_einschalten = 0;
                                                                 if(!VersionInfo.HardwareError[0] && CalibrationDone && !NC_ErrorCode)
+                                                                {
                                                                         modell_fliegt = 1;
                                                                         MotorenEin = 1;
  if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick) && !(VersionInfo.HardwareError[0] & 0x7F))  // Höhenregelung
+        {
                 #define HOVER_GAS_AVERAGE 16384L                // 16384 * 2ms = 32s averaging
                 #define HC_GAS_AVERAGE 4                        // 4 * 2ms= 8ms averaging
-#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
-#define OPA_OFFSET_STEP 15
-#else
-#define OPA_OFFSET_STEP 10
 #endif
                 int HCGas, GasReduction = 0;
                 static int HeightTrimming = 0;  // rate for change of height setpoint
                 static int HeightDeviation = 0, FilterHCGas = 0;
                 static unsigned long HoverGasFilter = 0;
-                static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0;
                 static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0;
         // 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
-                                OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down
+                                OCR0B = 255 - OCR0A;
                                 beeptime = 300;
+                                BaroExpandActive = 350;
-                                BaroExpandActive = 350;
+                                CalcExpandBaroStep();
+                           }
                            else
+                           {
                             BaroAtLowerLimit = 1;
                         {   // decrease offset
                           if(OCR0A > OPA_OFFSET_STEP)
+                           {
                                 ExpandBaro += 1;
                                 OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up
+                                OCR0B = 255 - OCR0A;
                                 beeptime = 300;
                                 BaroExpandActive = 350;
+                                CalcExpandBaroStep();
+                           }
                            else
+                           {
                             BaroAtUpperLimit = 1;
+               }

Subversion Repositories FlightCtrl

(root)/tags/V2.02b/fc.c @ 2549 - Rev 2366 → 2367