WebSVN - FlightCtrl - Diff - Rev 805 and 819 - /branches/V0.84a_var

 float GyroFaktor;
 float IntegralFaktor;
 volatile int  DiffNick,DiffRoll;
 int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
 volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
-unsigned char MotorWert[5];
 volatile unsigned char SenderOkay = 0;
 int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
 char MotorenEin = 0;
 int HoehenWert = 0;
 int SollHoehe = 0;
  MAX_GAS = EE_Parameter.Gas_Max;
  MIN_GAS = EE_Parameter.Gas_Min;
+}
 //############################################################################
 //
 void MotorRegler(void)
                          if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
                          if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
                          if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
                          eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
+                        }
+                        else
-                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+                         if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 20 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70)
+                          {
+                           WinkelOut.CalcState = 1;
+                           beeptime = 1000;
+                          }
+                          else
+                          {
+                               ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
+                           if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+                            {
                              if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
-                          }
+                            }
-                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
-                        SetNeutral();
+                           SetNeutral();
-                        Piep(GetActiveParamSetNumber());
+                           Piep(GetActiveParamSetNumber());
+                         }
+                        }
+                    }
                  else
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)  // ACC Neutralwerte speichern
+                    {
     Mess_IntegralNick2 -= IntegralFehlerNick;
     Mess_IntegralRoll2 -= IntegralFehlerRoll;
 //    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
-//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
-DebugOut.Analog[10] = GierGyroFehler;
+//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
     if(GierGyroFehler > ABGLEICH_ANZAHL/2) AdNeutralGier++;
     if(GierGyroFehler <-ABGLEICH_ANZAHL/2) AdNeutralGier--;
 //  Gieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //    if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
     if(abs(StickGier) > 20) // war 35
+     {
-      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) { NeueKompassRichtungMerken = 1; KompassSignalSchlecht = 500;};
+      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) { NeueKompassRichtungMerken = 1; KompassSignalSchlecht = 250;};
+     }
     tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
     tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
     sollGier = tmp_int;
     Mess_Integral_Gier -= tmp_int;
     if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll);
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
      {
-       int w,v,fehler,korrektur;
+       int w,v,r,fehler,korrektur;
        static int KompassSignalSchlecht = 0;
        w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
        v = abs(IntegralRoll /512);
 //v /= 4;
 //w /= 4;
+       if(v > w) w = v; // grösste Neigung ermitteln
-       if(v > w) w = v; // grösste Neigung ermitteln
+//       if((MaxStickNick > 32) || (MaxStickRoll > 32)) w *= 2;
        korrektur = w + 4;
        if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht)
+        {
          KompassStartwert = KompassValue;
        if(w > 0)
+        {
           if(!KompassSignalSchlecht)
+          {
            GierGyroFehler += fehler;
+           v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
+           //v = 32;
+           //r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
+           r = KompassRichtung;
-           v = (KompassRichtung * w) / 32;  // nach Kompass ausrichten
+           v = (r * w) / v;  // nach Kompass ausrichten
-           w = Parameter_KompassWirkung;
+           w = 3 * Parameter_KompassWirkung;
            if(v > w) v = w; // Begrenzen
            else
            if(v < -w) v = -w;
            Mess_Integral_Gier += v;
+          }
           if(KompassSignalSchlecht) KompassSignalSchlecht--;
+        }
-        else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
+        else KompassSignalSchlecht = 250; // so lange das Signal taub stellen --> ca. 0,5 sek
+     }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DebugOut.Analog[10] = SenderOkay;
     DebugOut.Analog[16] = Mittelwert_AccHoch;
     DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
+    DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
-    DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
+    DebugOut.Analog[19] = WinkelOut.CalcState;
     DebugOut.Analog[30] = GPS_Nick;

Subversion Repositories FlightCtrl

(root)/branches/V0.84a_var_motorsmoothing/fc.c @ 1903 - Rev 805 → 819