WebSVN - FlightCtrl - Diff - Rev 2309 and 2316

 int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
 int Mittelwert_AccNick, Mittelwert_AccRoll;
 unsigned int NeutralAccX=0, NeutralAccY=0;
 int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
 int NeutralAccZ = 0;
+signed char NeutralAccZfine = 0;
 unsigned char ControlHeading = 0;// in 2°
 long IntegralNick = 0,IntegralNick2 = 0;
 long IntegralRoll = 0,IntegralRoll2 = 0;
 long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
 long Integral_Gier = 0;
     DebugOut.Analog[27] = KompassSollWert;
         DebugOut.Analog[29] = Capacity.MinOfMaxPWM;
     DebugOut.Analog[30] = GPS_Nick;
     DebugOut.Analog[31] = GPS_Roll;
     if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe;
-DebugOut.Analog[16] = Variance;
+//DebugOut.Analog[16] = Variance;
-DebugOut.Analog[17] = VerticalVelocity;
+//DebugOut.Analog[17] = VarioMeter;
-DebugOut.Analog[18] = HoehenWertF;
+//DebugOut.Analog[18] = HoehenWertF;
-DebugOut.Analog[25] = Parameter_Hoehe_P;
+//DebugOut.Analog[25] = Parameter_Hoehe_P;
-DebugOut.Analog[26] = Parameter_Luftdruck_D;
+//DebugOut.Analog[26] = Parameter_Luftdruck_D;
+}
         Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
+}
 //############################################################################
 //  Nullwerte ermitteln
-void SetNeutral(unsigned char AccAdjustment)
+unsigned char SetNeutral(unsigned char AccAdjustment)  // retuns: "sucess"
 //############################################################################
 {
-        unsigned char i;
+        unsigned char i, sucess = 1;
-        unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;
+        unsigned int gier_neutral = 0, nick_neutral = 0, roll_neutral = 0, acc_z_neutral = 0;
     VersionInfo.HardwareError[0] = 0;
 //    HEF4017Reset_ON;
         NeutralAccX = 0;
         NeutralAccY = 0;
+        NeutralAccZ = 0;
-        NeutralAccZ = 0;
+        NeutralAccZfine = 0;
     AdNeutralNick = 0;
+         {
           Delay_ms_Mess(10);
           gier_neutral += AdWertGier;
           nick_neutral += AdWertNick;
           roll_neutral += AdWertRoll;
+          acc_z_neutral += Aktuell_az;
+         }
-     AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
+     AdNeutralNick = (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
-         AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
+         AdNeutralRoll = (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
-         AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER);
+         AdNeutralGier = (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER);
+     NeutralAccZ = (acc_z_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER);
      StartNeutralRoll = AdNeutralRoll;
      StartNeutralNick = AdNeutralNick;
      if(AccAdjustment)
+     {
-            NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);
-            NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
-            NeutralAccZ = Aktuell_az;
+            NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);
             NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
                 // Save ACC neutral settings to eeprom
                 SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX);
                 SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY);
                 SetParamWord(PID_ACC_TOP,  (uint16_t)NeutralAccZ);
+    }
     else
+    {
                 // restore from eeprom
-                NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK);
-                NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL);
+                NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK);
-                NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP);
+                NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL);
                 // strange settings?
-                if(((unsigned int) NeutralAccX > 2048) || ((unsigned int) NeutralAccY > 2048) || ((unsigned int) NeutralAccZ > 1024))
+                if(((unsigned int) NeutralAccX > 2048) || ((unsigned int) NeutralAccY > 2048)/* || ((unsigned int) NeutralAccZ > 1024)*/)
+                {
                         printf("\n\rACC not calibrated!\r\n");
                         NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);
                         NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
-                NeutralAccZ = Aktuell_az;
+                        sucess = 0;
-                }
+                }
+    }
     MesswertNick = 0;
     MesswertRoll = 0;
     MesswertGier = 0;
     Mess_IntegralNick = IntegralNick;
     Mess_IntegralRoll = IntegralRoll;
     Mess_Integral_Gier = 0;
     StartLuftdruck = Luftdruck;
     VarioMeter = 0;
-        VerticalVelocitySum = 0;
         SummenHoehe = 0;    Mess_Integral_Hoch = 0;
     KompassSollWert = KompassValue;
         KompassSignalSchlecht = 100;
     beeptime = 50;
         Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
 //      if(EE_Parameter.ServoCompInvert & SERVO_NICK_INV) NickServoValue = ((128 + 60) * 4 * 16); // neutral position = upper 1/4
 //        else
           NickServoValue = ((128 - 60) * 4 * 16); // neutral position = lower 1/4
+         }
+#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
+    signed int tilt1, tilt2;
+                tilt1 = (int)(IntegralNick/GIER_GRAD_FAKTOR);  // nick angle in deg
+                tilt2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR);  // roll angle in deg
+                tilt1 = (int16_t)ihypot(tilt1,tilt2);                   // tilt angle over all
+                CosAttitude = c_cos_8192(tilt1);
+                NeutralAccZ = (long)((long) (NeutralAccZ - 512) * 8192 + 4096) / CosAttitude + 512;
+                if(tilt1 > 20) sucess = 0; // calibration must be within 20° Tilt angle
+                if(ACC_AltitudeControl) if((NeutralAccZ < 682 - 25) || (NeutralAccZ > 682 + 25)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; sucess = 0;};
+#else
+        NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP);
+#endif
         if((AdNeutralNick < 150 * 16) || (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; };
         if((AdNeutralRoll < 150 * 16) || (AdNeutralRoll > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_ROLL; };
         if((AdNeutralGier < 150 * 2)  || (AdNeutralGier > 850 * 2))  { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; };
         if((NeutralAccX < 300) || (NeutralAccX > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_NICK; };
         if((NeutralAccY < 300) || (NeutralAccY > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_ROLL; };
+        if((NeutralAccZ < 512) || (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; };
-        if((NeutralAccZ < 512) || (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; };
+    if(VersionInfo.HardwareError[0]) sucess = 0;
     carefree_old = 70;
 #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
+        LIBFC_HoTT_Clear();
-        LIBFC_HoTT_Clear();
+        ACC_AltitudeFusion(2); // initalisation
+#endif
-#endif
+ return(sucess);
+}
                            if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+                            {
                              if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
+                            }
 //                                                 ServoActive = 0;
-                           SetNeutral(0);
-                           CalibrationDone = 1;
+                           CalibrationDone = SetNeutral(0);
                                                    ServoActive = 1;
                                                    DDRD  |=0x80; // enable J7 -> Servo signal
 #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
-                                                   if(abs(Aktuell_az - NeutralAccZ) > 5 && ACC_AltitudeControl)  { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; };
                                                    if(VersionInfo.HardwareError[0]) SpeakHoTT = SPEAK_ERR_SENSOR;
+                                                   else
+                                                   if(!CalibrationDone) SpeakHoTT = SPEAK_ERR_CALIBARTION;
                                                    else SpeakHoTT = SPEAK_CALIBRATE;
 #endif
                            Piep(GetActiveParamSet(),120);
+                         }
+                        }
                     if(++delay_neutral > 200)  // nicht sofort
+                        {
                         MotorenEin = 0;
                         delay_neutral = 0;
                         modell_fliegt = 0;
-                        SetNeutral(1);
-                        CalibrationDone = 1;
+                        CalibrationDone = SetNeutral(1);
 #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
+                                                   if(VersionInfo.HardwareError[0]) SpeakHoTT = SPEAK_ERR_SENSOR;
+                                                   else
+                                                   if(!CalibrationDone) SpeakHoTT = SPEAK_ERR_CALIBARTION;
-                                            SpeakHoTT = SPEAK_CALIBRATE;
+                                                   else SpeakHoTT = SPEAK_CALIBRATE;
 #endif
                         Piep(GetActiveParamSet(),120);
+                        }
+                    }
                  else delay_neutral = 0;
+                        }
+                   }
                    else // delay, because of expanding the Baro-Range
+                   {
                     // now clear the D-values
+                          VarioMeter = 0;
 #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
-                          if(ACC_AltitudeControl) SummenHoehe = HoehenWert * SA_FILTER;
+                          if(ACC_AltitudeControl) ACC_AltitudeFusion(1); // init
                           else SummenHoehe = HoehenWert * SM_FILTER;
 #else
               SummenHoehe = HoehenWert * SM_FILTER;
 #endif
-                          VerticalVelocitySum = 0;
-                          VarioMeter = 0;
                           BaroExpandActive--;
+                   }
                 // if height control is activated by an rc channel
         if(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
                 {       // check if parameter is less than activation threshold
                         if(Parameter_HoehenSchalter < 50) // for 3 or 2-state switch height control is disabled in lowest position
                         {   //height control not active
+                         }
                         HCGas = HoverGas;      // take hover gas (neutral point)
+                   }
          if(HoehenWertF > SollHoehe || !(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT))
+                 {
- if(!ACC_AltitudeControl)
+                  if(!ACC_AltitudeControl)
-  {
+                  {
                         // from this point the Heigth Control Algorithm is identical for both versions
                         if(BaroExpandActive) // baro range expanding active
+                        {
                                 HCGas = HoverGas; // hover while expanding baro adc range
                                 HeightDeviation = 0;
                         {  // old version
                                 LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas
                                 GasMischanteil = FilterHCGas;
+                        }
                         else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode
- }
+                   }
- else
+          else // ACC-Altitude control
- {
+                   {
                         // from this point the Heigth Control Algorithm is identical for both versions
                         if(BaroExpandActive) // baro range expanding active
+                        {
                                 HCGas = HoverGas; // hover while expanding baro adc range
                                 HeightDeviation = 0;
                                 HeightDeviation = (int)(tmp_long); // positive when too high
                                 tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part
                                 LIMIT_MIN_MAX(tmp_long, -511 * STICK_GAIN, 512 * STICK_GAIN); // more than full range makes sense
                                 GasReduction = tmp_long;
                                 // ------------------------ D-Part: ACC-Z Integral  ------------------------
-                                tmp_long = VerticalVelocity + AdWertAccHoch * Parameter_Hoehe_ACC_Wirkung/256;
+                                tmp_long = VarioMeter + (AdWertAccHoch * Parameter_Hoehe_ACC_Wirkung)/256;
                                 // ------------------------- D-Part: Vario Meter ----------------------------
                                 if(WaypointTrimming) {
                                         Variance = AltitudeSetpointTrimming * 8;
                                 } else {
                                         Variance = AltitudeSetpointTrimming * EE_Parameter.Hoehe_Verstaerkung*9/32;
                                 tmp_long -= (long)Variance;
                                 tmp_long = (tmp_long * (long)Parameter_Luftdruck_D) / 32; // scale to d-gain parameter
                                 LIMIT_MIN_MAX(tmp_long,-511 * STICK_GAIN, 512 * STICK_GAIN);
                                 GasReduction += tmp_long;
                         } // EOF no baro range expanding
-DebugOut.Analog[19] = -GasReduction;
                         HCGas -= GasReduction;
                         LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limits gas around hover point
                         // strech control output by inverse attitude projection 1/cos
             // + 1/cos(angle)  ++++++++++++++++++++++++++
                         tmp_long2 = (int32_t)HCGas;
             if(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT)
                         {  // old version
                                 LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas
                                 GasMischanteil = FilterHCGas;
+                        }
-                        else GasMischanteil = FilterHCGas;// + (GasMischanteil - HoverGas) / 4; // Qopter: geändert
+                        else GasMischanteil = FilterHCGas;
- }
+           } // end of ACC-Altitude control
+                  }
                 }// EOF height control active
                 else // HC not active
+                {
                         //update hoover gas stick value when HC is not active
+                }
                 // Hover 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((FC_StatusFlags & FC_STATUS_FLY))// && !(FC_SatusFlags & FC_STATUS_EMERGENCY_LANDING))
+                {
-//if(HoverGasFilter == 0 || StartTrigger == 1)  HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
+                        //if(HoverGasFilter == 0 || StartTrigger == 1)  HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
-if(HoverGasFilter == 0 || StartTrigger == 1)  HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(HoverGas); // Qopter: geändert
+                        if(HoverGasFilter == 0 || StartTrigger == 1)  HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(HoverGas); // Qopter: geändert
                         if(StartTrigger == 1) StartTrigger = 2;
                                 tmp_long2 = (int32_t)GasMischanteil; // take current thrust
                                 tmp_long2 *= CosAttitude;            // apply attitude projection
                                 tmp_long2 /= 8192;
                                 // average vertical projected thrust
                                 {   // reduce the time constant of averaging by factor of 2 to get much faster a stable value
                                         HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L);
                                         HoverGasFilter += 4L * tmp_long2;
+                                }
                           else //later
-//if(abs(VerticalVelocity) < 50 && abs(HoehenWertF - SollHoehe) < 256) // Qopter: geändert, Anpassung nötig?
-                          if(abs(VarioMeter) < 100 && abs(HoehenWert - SollHoehe) < 256) // only on small vertical speed & difference is small (only descending)
+                          if(abs(VarioMeter) < 100 && abs(HoehenWertF - SollHoehe) < 256) // only on small vertical speed & difference is small (only descending)
+                                {
                                         HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE;
                                         HoverGasFilter += tmp_long2;
+                                }
                                 HoverGas = (int16_t)(HoverGasFilter/HOVER_GAS_AVERAGE);

Subversion Repositories FlightCtrl

(root)/tags/V2.06a/fc.c @ 2753 - Rev 2309 → 2316