WebSVN - FlightCtrl - Diff - Rev 499 and 513 - /branches/FC_V070d_ACC-HH

 int StickNick = 0,StickRoll = 0,StickGier = 0;
 char MotorenEin = 0;
 int HoehenWert = 0;
 int SollHoehe = 0;
 int LageKorrekturRoll = 0,LageKorrekturNick = 0;
-float Kp =  FAKTOR_P;
 float Ki =  FAKTOR_I;
 unsigned char Looping_Nick = 0,Looping_Roll = 0;
 unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
      AdNeutralNick= AdWertNick;
          AdNeutralRoll= AdWertRoll;
          AdNeutralGier= AdWertGier;
      StartNeutralRoll = AdNeutralRoll;
      StartNeutralNick = AdNeutralNick;
+    if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
+    {
-    NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
+      NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
-        NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
+          NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
-        NeutralAccZ = Aktuell_az;
+          NeutralAccZ = Aktuell_az;
+    }
+    else
+    {
+      NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);
+          NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);
+          NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);
+    }
         Mess_IntegralNick = 0;
     Mess_IntegralNick2 = 0;
     Mess_IntegralRoll = 0;
         Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
         Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
         Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
     IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
     IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
-    IntegralAccZ    += Aktuell_az - 704;//NeutralAccZ;
+    IntegralAccZ    += Aktuell_az - NeutralAccZ;
 // Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
             Mess_Integral_Gier +=  MesswertGier;
             Mess_Integral_Gier2 += MesswertGier;
 // Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
       if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
+                         }
 //++++++++++++++++++++++++++++++++++++++++++++++++
 // ADC einschalten
     ANALOG_ON;
 //++++++++++++++++++++++++++++++++++++++++++++++++
-DebugOut.Analog[11] = MesswertRoll;
-//DebugOut.Analog[11] = AdWertNick;
     Integral_Gier  = Mess_Integral_Gier;
     IntegralNick = Mess_IntegralNick;
     IntegralRoll = Mess_IntegralRoll;
     GRN_ON;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gaswert ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
+    if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20;
     if(GasMischanteil < 0) GasMischanteil = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Emfang schlecht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(SenderOkay < 100)
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // auf Nullwerte kalibrieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
+                    {
-                    unsigned char setting = 2;
                     if(++delay_neutral > 200)  // nicht sofort
+                        {
                         GRN_OFF;
                         MotorenEin = 0;
                         delay_neutral = 0;
                             ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
                         SetNeutral();
                         Piep(GetActiveParamSetNumber());
+                        }
+                    }
+                 else
+                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)  // ACC Neutralwerte speichern
+                    {
+                    if(++delay_neutral > 200)  // nicht sofort
+                        {
+                        GRN_OFF;
+                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte löschen
+                        MotorenEin = 0;
+                        delay_neutral = 0;
+                        modell_fliegt = 0;
+                        SetNeutral();
+                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern
+                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern
+                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256);
+                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256);
+                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256);
+                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256);
+                        Piep(GetActiveParamSetNumber());
+                        }
+                    }
                  else delay_neutral = 0;
+                }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gas ist unten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+           {
             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
             ausgleichNick = IntegralFehlerNick / 8;
             if(ausgleichNick > 5000) ausgleichNick = 5000;
             LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
-//            Mess_IntegralNick -= ausgleichNick;
+           }
            else last_n_p = 1;
          } else  last_n_p = 0;
         if(IntegralFehlerNick < -FEHLER_LIMIT2)
+         {
+            {
              cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
              ausgleichNick = IntegralFehlerNick / 8;
              if(ausgleichNick < -5000) ausgleichNick = -5000;
              LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
-//             Mess_IntegralNick -= ausgleichNick;
+            }
            else last_n_n = 1;
          } else  last_n_n = 0;
         } else cnt = 0;
         if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
+           {
             cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
             ausgleichRoll = IntegralFehlerRoll / 8;
             if(ausgleichRoll > 5000) ausgleichRoll = 5000;
             LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
-//            Mess_IntegralRoll -= ausgleichRoll;
+           }
            else last_r_p = 1;
          } else  last_r_p = 0;
         if(IntegralFehlerRoll < -FEHLER_LIMIT2)
+         {
            if(last_r_n)
+           {
             cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
             ausgleichRoll = IntegralFehlerRoll / 8;
             if(ausgleichRoll < -5000) ausgleichRoll = -5000;
-//            Mess_IntegralRoll -= ausgleichRoll;
             LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
+           }
            else last_r_n = 1;
          } else  last_r_n = 0;
         } else
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mischer und PI-Regler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   DebugOut.Analog[7] = GasMischanteil;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gier-Anteil
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #define MUL_G  1.0
     GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
 //GierMischanteil = 0;
-    if(GierMischanteil > (MUL_G * GasMischanteil))  GierMischanteil = MUL_G * GasMischanteil;
+    if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
-    if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil);
+    if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
-    if(GierMischanteil > 100)  GierMischanteil = 100;
+    if(GierMischanteil > ((MAX_GAS - GasMischanteil)/2)) GierMischanteil = ((MAX_GAS - GasMischanteil) / 2);
-    if(GierMischanteil < -100) GierMischanteil = -100;
+    if(GierMischanteil < -((MAX_GAS - GasMischanteil) / 2)) GierMischanteil = -((MAX_GAS - GasMischanteil) / 2);
     if(GasMischanteil < 20) GierMischanteil = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
+    DiffNick = MesswertNick - (StickNick - GPS_Nick);   // Differenz bestimmen
     if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung
     else  SummeNick += DiffNick; // I-Anteil bei HH
-    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
+    if(SummeNick > 0) SummeNick-= 2 ; else SummeNick += 2 ;
     if(SummeNick >  16000) SummeNick =  16000;
     if(SummeNick < -16000) SummeNick = -16000;
     pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick
-    // Motor Vorn
-//#define MUL  2
-//    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
-//    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
+    // Motor Vorn
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
     if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
     if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
         Motor_Hinten = motorwert;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
+        DiffRoll = MesswertRoll - (StickRoll  - GPS_Roll);      // Differenz bestimmen
     if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll  - GPS_Roll);// I-Anteil bei Winkelregelung
     else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
-    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
+    if(SummeRoll > 0) SummeRoll-= 2 ; else SummeRoll += 2 ;
     if(SummeRoll >  16000) SummeRoll =  16000;
     if(SummeRoll < -16000) SummeRoll = -16000;
     pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
     if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;

Subversion Repositories FlightCtrl

(root)/branches/FC_V070d_ACC-HH_Umschaltung/fc.c @ 991 - Rev 499 → 513