WebSVN - FlightCtrl - Diff - Rev 2426 and 2427

 int   ErsatzKompassInGrad; // Kompasswert in Grad
 int   GierGyroFehler = 0;
 char GyroFaktor,GyroFaktorGier;
 char IntegralFaktor,IntegralFaktorGier;
 int  DiffNick,DiffRoll;
+unsigned int StickGasHover = 127;
-int StickGasHover = 127, HoverGasMin = 0, HoverGasMax = 1023;
+int HoverGasMin = 0, HoverGasMax = 1023;
 int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
 //int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
 unsigned char Poti[9] = {0,0,0,0,0,0,0,0};
 volatile unsigned char SenderOkay = 0;
 char MotorenEin = 0,StartTrigger = 0;
                 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(AdjustmentMode != 0 && ACC_AltitudeControl) if((NeutralAccZ < 682 - 25) || (NeutralAccZ > 682 + 35)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; sucess = 0;};
+                if(AdjustmentMode != 0 && ACC_AltitudeControl) if((NeutralAccZ < 682 - 30) || (NeutralAccZ > 682 + 35)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; sucess = 0;};
 #else
         NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP);
         EEAR = EE_DUMMY;  // Set the EEPROM Address pointer to an unused space
 #endif
         if((AdNeutralNick < 150 * 16) || (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; };
         else FC_StatusFlags |= FC_STATUS_MOTOR_RUN;
 // ++++++++++++++++++++++++++++++++++++++++++
 // ++++++++++++++++++++++++++++++++++++++++++
-#ifdef REDUNDANT_FC
+#ifdef REDUNDANT_FC_SLAVE
         I2CTimeout = 100;
         return;
+#endif
-#else
-/*
+#ifdef REDUNDANT_FC_MASTER
-if(Parameter_UserParam7 < 100 && RedundanceBlOperation)
+if(Parameter_UserParam7 > 150)
  {
-        I2CTimeout = 100;
+        // Make noise
-   if((BeepMuster == 0xffff))
+        if((BeepMuster == 0xffff)) { beeptime = 25000; BeepMuster = 0x0080; }
-                   {
+        // Do not send I2C-Data
-                    beeptime = 25000;
+        if(RedundanceBlOperation)
-                    BeepMuster = 0x0080;
+         {
+          I2CTimeout = 100;
           return;
-        return;
+         }
 */
 #endif
          CareFree = 0;
+    }
  if(CareFree) { FC_StatusFlags2 |= FC_STATUS2_CAREFREE; /*if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;*/} else FC_StatusFlags2 &= ~FC_STATUS2_CAREFREE;
+}
+void CalcStickGasHover(void)
+{
+ if(!EE_Parameter.Hoehe_StickNeutralPoint)
+        {
+          StickGasHover = HoverGas/STICK_GAIN; // rescale back to stick value
+          StickGasHover = (unsigned int)((unsigned int) StickGasHover * UBat) / BattLowVoltageWarning;
+        }
+  else StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint;
+  LIMIT_MIN_MAX(StickGasHover, 70, 175); // reserve some range for trim up and down
+}
 //############################################################################
 //
 void MotorRegler(void)
 //############################################################################
     GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Auto-Landing
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-//#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
  static unsigned char slower;
  if(!slower--)
+  {
    static unsigned int u_filter = 0;
    if(!u_filter) u_filter = UBat;
   if(LowVoltageLandingActive && FromNC_AltitudeSetpoint >= 0)
+     {
                 FromNC_AltitudeSpeed = EE_Parameter.LandingSpeed;
                 FromNC_AltitudeSetpoint = -20000;
+         }
-//#endif
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-//DebugOut.Analog[16] = StickGasMiddle;
+// send SPI pending bytes
-if(BytegapSPI == 0)  SPI_TransmitByte();
+ if(BytegapSPI == 0)  SPI_TransmitByte();
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Höhenregelung
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   GasMischanteil *= STICK_GAIN;
- if(BytegapSPI == 0)  SPI_TransmitByte();
-        // if height control is activated
+// if height control is activated
  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
+                                          }
                                         HeightTrimming = 0;
                                         LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied
                                         if(Parameter_ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100;
                                         //update hoover gas stick value when setpoint is shifted
+                                        if(FromNC_AltitudeSpeed == 0) CalcStickGasHover();
+/*
                        if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0)
+                       {
                            StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value
-                           StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning;
+                           StickGasHover = (unsigned int)((unsigned int) StickGasHover * UBat) / BattLowVoltageWarning;
                            if(StickGasHover < 70) StickGasHover = 70;
-                           else if(StickGasHover > 150) StickGasHover = 150;
+                           else if(StickGasHover > 175) StickGasHover = 175;
+                       }
+*/
+                                }
               if(BaroExpandActive) SollHoehe = HoehenWertF; // update setpoint to current altitude if Expanding is active
                         } //if FCFlags & MKFCFLAG_FLY
                         else
+                        {
                          SollHoehe = HoehenWert - 400;
                          if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint;
-                         else StickGasHover = 120;
+                         else StickGasHover = 127;
                          HoverGas = GasMischanteil;
                          VarioCharacter = '.';
+                         }
                         HCGas = HoverGas;      // take hover gas (neutral point)
+                   }
+                  }
                 }// EOF height control active
                 else // HC not active
+                {
                         //update hoover gas stick value when HC is not active
+                        CalcStickGasHover();
-                        if(!EE_Parameter.Hoehe_StickNeutralPoint)
+/*                      if(!EE_Parameter.Hoehe_StickNeutralPoint)
+                        {
                                 StickGasHover = HoverGas/STICK_GAIN; // rescale back to stick value
-                                StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning;
+                                StickGasHover = (unsigned int)((unsigned int) StickGasHover * UBat) / BattLowVoltageWarning;
+                        }
                         else StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint;
-            LIMIT_MIN_MAX(StickGasHover, 70, 150); // reserve some range for trim up and down
+            LIMIT_MIN_MAX(StickGasHover, 70, 175); // reserve some range for trim up and down
+*/
                         FilterHCGas = GasMischanteil;
                         // set both flags to indicate no vario mode
                         FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN);
                         FC_StatusFlags2 &= ~FC_STATUS2_ALTITUDE_CONTROL;
+                }
 #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
         if(Capacity.MinOfMaxPWM < 40) SpeakHoTT = SPEAK_ERR_MOTOR;
 #endif
+   }
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // send SPI pending bytes
  if(BytegapSPI == 0)  SPI_TransmitByte();
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mischer und PI-Regler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     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(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-if(BytegapSPI == 0)  SPI_TransmitByte();
+// send SPI pending bytes
         if(BytegapSPI == 0)  SPI_TransmitByte();
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Universal Mixer
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                         else tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L;
             // Gier
                         if(Mixer.Motor[i][3] == 64) tmp_int += GierMischanteil;
                         else if(Mixer.Motor[i][3] == -64) tmp_int -= GierMischanteil;
                         else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;
-#ifdef REDUNDANT_FC
+#ifdef REDUNDANT_FC_SLAVE
                         tmp_int = (tmp_motorwert[i] + tmp_int) / 2;
 #else
                         if(Motor[i].Version & MOTOR_STATE_FAST_MODE || tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2;      // Beschleunigen
             else
                          {  // BL-Ctrl 1.0 or 2.0
+                {
                         Motor[i].SetPoint = 0;
                         Motor[i].SetPointLowerBits = 0;
+                }
+        }
+#ifdef REDUNDANT_FC_MASTER
-//if(Parameter_UserParam6 > 200) Motor[0].SetPoint = 0;
+if(Parameter_UserParam6 > 200 && RedundanceBlOperation) Motor[0].SetPoint = 0;
+#endif
+}
 //DebugOut.Analog[16]

Subversion Repositories FlightCtrl

(root)/tags/V2.04a/fc.c @ 2684 - Rev 2426 → 2427