WebSVN - FlightCtrl - Diff - Rev 1232 and 1247 - /branches/FC_075b_ACC-HH

 int TrimNick, TrimRoll;
 int AdNeutralGierBias;
 int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
 int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
 int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
-volatile float NeutralAccZ = 0;
+//volatile float NeutralAccZ = 0; // MartinR  : so war es
+volatile int NeutralAccZ = 0; // MartinR geändert
+volatile int NeutralAccZ2 = 0; // MartinR: für HP-Filter
+signed int h_p,h_d,h_delta,Ziel_d;  // MartinR: P,D Anteil beim eigenen Höhenregler
+signed int SchwebeGas; // MartinR: für neuen Höhenregler
 unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
 long IntegralNick = 0,IntegralNick2 = 0;
 long IntegralRoll = 0,IntegralRoll2 = 0;
 long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
 long Integral_Gier = 0;
 unsigned int  KompassSignalSchlecht = 500;
 unsigned char  MAX_GAS,MIN_GAS;
 unsigned char Notlandung = 0;
 unsigned char HoehenReglerAktiv = 0;
 unsigned char TrichterFlug = 0;
+unsigned char delay_Hoehenregler = 0; // MartinR: zur Begrenzung der Sinkgeschwindigkeit
+signed int delay_SchwebeGas = 0; // MartinR: zur Begrenzung der Sinkgeschwindigkeit
 long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
 long  ErsatzKompass;
 int   ErsatzKompassInGrad; // Kompasswert in Grad
 int   GierGyroFehler = 0;
 char GyroFaktor,GyroFaktorGier;
 int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
 volatile unsigned char SenderOkay = 0;
 int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
 char MotorenEin = 0;
 int HoehenWert = 0;
+int HoehenWertalt = 0; //MartinR: Test
+unsigned char iHoehe; // MartinR: Zähler für HoeheD
+signed long HoehenWertSumme = 0; // MartinR: für neuen Höhenregler D-Anteil
 int SollHoehe = 0;
 int LageKorrekturRoll = 0,LageKorrekturNick = 0;
 //float Ki =  FAKTOR_I;
 int Ki = 10300 / 33;
+int KiHH = 10300 / 33; // MartinR : für Ki bei HH über Schalter
 unsigned char Looping_Nick = 0,Looping_Roll = 0;
 unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
 unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
 unsigned char Parameter_NaviWindCorrection;
 unsigned char Parameter_NaviSpeedCompensation;
 unsigned char Parameter_ExternalControl;
 struct mk_param_struct EE_Parameter;
 signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
-int MaxStickNick = 0,MaxStickRoll = 0;
+//int MaxStickNick = 0,MaxStickRoll = 0;MartinR: so war es
+int MaxStickNick = 0,MaxStickRoll = 0,stick_nick_neutral = 0,stick_roll_neutral = 0;  // MartinR: stick_.._neutral hinzugefügt
 unsigned int  modell_fliegt = 0;
 volatile unsigned char MikroKopterFlags = 0;
 long GIER_GRAD_FAKTOR = 1291;
 signed int KopplungsteilNickRoll,KopplungsteilRollNick;
 unsigned char RequiredMotors = 4;
 int MotorSmoothing(int neu, int alt)
+{
  int motor;
  if(neu > alt) motor = (1*(int)alt + neu) / 2;
+ //else   motor = neu - (alt - neu)*1; // MartinR: so war es
- else   motor = neu - (alt - neu)*1;
+ else   motor = neu; // MartinR: Entsprechen Vorschlag von MartinW geändert
 //if(Poti2 < 20)  return(neu);
  return(motor);
+}
  unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;
     ServoActive = 0; HEF4017R_ON;
         NeutralAccX = 0;
         NeutralAccY = 0;
         NeutralAccZ = 0;
+        NeutralAccZ2 = 0; // MartinR
     AdNeutralNick = 0;
         AdNeutralRoll = 0;
         AdNeutralGier = 0;
     AdNeutralGierBias = 0;
     Parameter_AchsKopplung1 = 0;
     if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
+    {
       NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
           NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);
           NeutralAccZ = Aktuell_az;
+          NeutralAccZ2 = NeutralAccZ; // MartinR
+    }
     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]);
 void Mittelwert(void)
 //############################################################################
+{
     static signed long tmpl,tmpl2,tmpl3,tmpl4;
         static signed int oldNick, oldRoll, d2Roll, d2Nick;
+        static signed int oldNick2, oldRoll2 ; //MartinR : für geänderte d2* Ermittlung
         signed long winkel_nick, winkel_roll;
         MesswertGier = (signed int) AdNeutralGier - AdWertGier;
 //    MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
     NaviCntAcc++;
     IntegralAccZ  += Aktuell_az - NeutralAccZ;
 //++++++++++++++++++++++++++++++++++++++++++++++++
 // ADC einschalten
+        //ANALOG_ON; // MartinR : so war es
-    ANALOG_ON;
+        // ANALOG_START; // MartinR verschoben zu timer0
         AdReady = 0;
 //++++++++++++++++++++++++++++++++++++++++++++++++
 // Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
    Mess_Integral_Gier += MesswertGier;
    ErsatzKompass += MesswertGier;
+// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
+if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 140)) IntegralFaktor = 0;  // MartinR: zusätzlich
-// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
+      //if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))  // MartinR : so war es
-      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
+          if(!Looping_Nick && !Looping_Roll && IntegralFaktor && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) // MartinR: zusätzlich "&& IntegralFaktor"
+         {
             tmpl3 = (MesswertRoll * winkel_nick) / 2048L;
             tmpl3 *= Parameter_AchsKopplung2; //65
             tmpl3 /= 4096L;
     IntegralNick2 = Mess_IntegralNick2;
     IntegralRoll2 = Mess_IntegralRoll2;
 #define D_LIMIT 128
+   //MesswertNick = HiResNick / 8;// MartinR : so war es
+   //MesswertRoll = HiResRoll / 8;// MartinR : so war es
+   MesswertNick = AdWertNickFilter / 8;// MartinR : anstelle HiResNick : AdWertNickFilter
+   MesswertRoll = AdWertRollFilter / 8;// MartinR : ist im vergleich zu HiRes.. eimal mehr gemittelt
    MesswertNick = HiResNick / 8;
-   MesswertRoll = HiResRoll / 8;
+ // MartinR : so war es Anfang
         /*
    if(AdWertNick < 15)   MesswertNick = -1000;  if(AdWertNick <  7)   MesswertNick = -2000;
    if(PlatinenVersion == 10)  { if(AdWertNick > 1010) MesswertNick = +1000;  if(AdWertNick > 1017) MesswertNick = +2000; }
    else  {  if(AdWertNick > 2000) MesswertNick = +1000;  if(AdWertNick > 2015) MesswertNick = +2000; }
+   if(AdWertRoll < 15)   MesswertRoll = -1000;  if(AdWertRoll <  7)   MesswertRoll = -2000;
+   if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000;  if(AdWertRoll > 1017) MesswertRoll = +2000; }
+   else { if(AdWertRoll > 2000) MesswertRoll = +1000;  if(AdWertRoll > 2015) MesswertRoll = +2000;  }
+        // MartinR : FC 1.0: Sprung von 500 auf 2000 !! FC-ME: Sprung von 1000 auf 2000
+        */
+        // MartinR : so war es Ende
+         // MartinR : Neu Anfang
+        if(PlatinenVersion == 10)
+        {
+        if(AdWertNick > 1010) MesswertNick = +600;
+        if(AdWertNick > 1017) MesswertNick = +800;
+        if(AdWertNick < 15)   MesswertNick = -600;
+        if(AdWertNick <  7)   MesswertNick = -800;
+        if(AdWertRoll > 1010) MesswertRoll = +600;
+        if(AdWertRoll > 1017) MesswertRoll = +800;
+        if(AdWertRoll < 15)   MesswertRoll = -600;
+        if(AdWertRoll <  7)   MesswertRoll = -800;
+        }
+        else
+        {
+        if(AdWertNick > 2000) MesswertNick = +1200;
+        if(AdWertNick > 2015) MesswertNick = +1600;
+        if(AdWertNick < 15)   MesswertNick = -1200;
+        if(AdWertNick <  7)   MesswertNick = -1600;
+        if(AdWertRoll > 2000) MesswertRoll = +1200;
+        if(AdWertRoll > 2015) MesswertRoll = +1600;
+        if(AdWertRoll < 15)   MesswertRoll = -1200;
-   if(AdWertRoll < 15)   MesswertRoll = -1000;  if(AdWertRoll <  7)   MesswertRoll = -2000;
+        if(AdWertRoll <  7)   MesswertRoll = -1600;
    if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000;  if(AdWertRoll > 1017) MesswertRoll = +2000; }
+ // MartinR : Neu Ende
-   else { if(AdWertRoll > 2000) MesswertRoll = +1000;  if(AdWertRoll > 2015) MesswertRoll = +2000;  }
+  if(Parameter_Gyro_D)
+  {
-  if(Parameter_Gyro_D)
+  /* MartinR: so war es Anfang
    d2Nick = HiResNick - oldNick;
    if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
    else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;
    MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16;
    HiResNick += (d2Nick * (signed int) Parameter_Gyro_D);
    HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D);
+   */ //MartinR: so war es Ende
+   // MartinR :neu Anfang
+   d2Nick = MesswertNick - oldNick2;
+   oldNick2 = oldNick;
+   oldNick = MesswertNick;
+   if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;
+   else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT;
+   if(d2Nick > 0) d2Nick --;
+   if(d2Nick < 0) d2Nick ++;
+   MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16;
+   d2Roll = MesswertRoll - oldRoll2;
+   oldRoll2 = oldRoll;
+   oldRoll = MesswertRoll;
+   if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
+   else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;
+   if(d2Roll > 0) d2Roll --;
+   if(d2Roll < 0) d2Roll ++;
+   MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16;
+   // MartinR :neu Ende
+  }
  if(RohMesswertRoll > 0) TrimRoll  += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
  else                    TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
  CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
  CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);
  CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
  CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255);
  Ki = 10300 / (Parameter_I_Faktor + 1);
+ if(Parameter_UserParam1 > 140) KiHH = 10300 / (Parameter_UserParam2 + 1); else  KiHH = Ki; // MartinR : für HH über Schalter
+ Parameter_NaviGpsModeControl = EE_Parameter.NaviGpsModeControl; //MartinR: Standard: EE_Parameter.NaviGpsModeControl wird übertragen
+ if(!IntegralFaktor) Parameter_NaviGpsModeControl= 0; // MartinR: wenn HH dann GPS auf free- Mode
+  // 0 = AID; 100 = free; 200 = coming home //so war es
+  // 0 = free; 100 = AID; 200 = coming home //neu
  MAX_GAS = EE_Parameter.Gas_Max;
  MIN_GAS = EE_Parameter.Gas_Min;
+}
 //############################################################################
+{
          int pd_ergebnis_nick,pd_ergebnis_roll,h,tmp_int;
          int GierMischanteil,GasMischanteil;
      static long SummeNick=0,SummeRoll=0;
+         static long SummeNickHH=0,SummeRollHH=0; // MartinR: Für ACC-HH Umschaltung
      static long sollGier = 0,tmp_long,tmp_long2;
      static long IntegralFehlerNick = 0;
      static long IntegralFehlerRoll = 0;
          static unsigned int RcLostTimer;
          static unsigned char delay_neutral = 0;
                         Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
                         Mess_IntegralNick2 = IntegralNick;
                         Mess_IntegralRoll2 = IntegralRoll;
                         SummeNick = 0;
                         SummeRoll = 0;
+                                                SchwebeGas = 0; // MartinR: für neuen Höhenregler
                         MikroKopterFlags |= FLAG_START;
+                        }
+                    }
                     else delay_einschalten = 0;
                 //Auf Neutralwerte setzen
  if(!NewPpmData-- || Notlandung)
+  {
         static int stick_nick,stick_roll;
+    ParameterZuordnung();
+        // MartinR: original:
-    ParameterZuordnung();
+        /*
     stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
     stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
     StickNick = stick_nick - (GPS_Nick + GPS_Nick2);
     stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
+    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
+    StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
+        */
+// MartinR: geändert Anfang
+        if(Parameter_UserParam1 > 140)  // MartinR: zweiter Stick_P Wert nur, wenn HH über Schalter aktiv ist
+                {
+                stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * Parameter_UserParam3 - stick_nick_neutral) / 4;
+                stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * Parameter_UserParam3 - stick_roll_neutral) / 4 ;
+                }
+         else
+                stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
+                stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
+         if(IntegralFaktor)
+                {
+                stick_nick_neutral = stick_nick; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
+                stick_roll_neutral = stick_roll; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
+                stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
+                stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
+                StickNick = stick_nick - (GPS_Nick + GPS_Nick2); // MartinR: GPS nur im ACC-Mode wirksam
+                StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); // MartinR: GPS nur im ACC-Mode wirksam
+                }
+        else            // wenn HH , MartinR
+                {
+                stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben
+                stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben
+                StickNick = stick_nick; // MartinR: GPS nur im ACC-Mode wirksam
+                StickRoll = stick_roll; // MartinR: GPS nur im ACC-Mode wirksam
+                }
-    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
+// MartinR: geändert Ende
     StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
     IntegralFaktorGier = Parameter_Gyro_I;
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //+ Analoge Steuerung per Seriell
+//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// MartinR: ToDo: eventuell die Kombination HH und Steuerung per Seriell nicht zulassen??
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128)
+    {
          StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
          StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
          StickGier += ExternControl.Gier;
      ExternHoehenValue =  (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
      if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
+    }
+    if(StickGas < 0) StickGas = 0;
+        //if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0; // MartinR: Original
-    if(StickGas < 0) StickGas = 0;
         if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 140)) IntegralFaktor = 0;  // MartinR
     if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
     if(GyroFaktor < 0) GyroFaktor = 0;
     if(IntegralFaktor < 0) IntegralFaktor = 0;
  MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
  MittelIntegralRoll  += IntegralRoll;
  MittelIntegralNick2 += IntegralNick2;
  MittelIntegralRoll2 += IntegralRoll2;
+ //if(Looping_Nick || Looping_Roll) // MartinR: so war es
- if(Looping_Nick || Looping_Roll)
+ if(Looping_Nick || Looping_Roll || !IntegralFaktor)  // MartinR: "|| !IntegralFaktor" hinzugefügt
+  {
     IntegralAccNick = 0;
     IntegralAccRoll = 0;
     MittelIntegralNick = 0;
     MittelIntegralRoll = 0;
     MittelIntegralNick2 = 0;
+    MittelIntegralRoll2 = 0;
+        IntegralNick = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
+    IntegralRoll = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
+    Mess_IntegralNick = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
+    Mess_IntegralRoll = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
+    Mess_Integral_Gier = 0;     // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
+    Mess_Integral_Gier2 = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
     MittelIntegralRoll2 = 0;
     Mess_IntegralNick2 = Mess_IntegralNick;
     Mess_IntegralRoll2 = Mess_IntegralRoll;
     ZaehlMessungen = 0;
     LageKorrekturNick = 0;
     LageKorrekturRoll = 0;
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin))
+  if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin) && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt
+  {
    long tmp_long, tmp_long2;
    if(FromNaviCtrl_Value.Kalman_K != -1 /*&& !TrichterFlug*/)
  if(ZaehlMessungen >= ABGLEICH_ANZAHL)
+ {
   static int cnt = 0;
   static char last_n_p,last_n_n,last_r_p,last_r_n;
   static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
-  if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp)
+  if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt
+  {
     MittelIntegralNick  /= ABGLEICH_ANZAHL;
     MittelIntegralRoll  /= ABGLEICH_ANZAHL;
         IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
         IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
     DebugOut.Analog[10] = SenderOkay;
     //DebugOut.Analog[16] = Mittelwert_AccHoch;
     //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
     //DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
     DebugOut.Analog[19] = WinkelOut.CalcState;
-    DebugOut.Analog[20] = ServoValue;
+//    DebugOut.Analog[20] = ServoValue; // MartinR: DebugOut.Analog[20] wird in timer0.c ausgegeben!!
 //    DebugOut.Analog[24] = MesswertNick/2;
 //    DebugOut.Analog[25] = MesswertRoll/2;
     DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
 //    DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;
 //    DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
 #define TRIM_MAX 200
  if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else  if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;
+ if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else  if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
+ if(!IntegralFaktor) // MartinR : hinzugefügt
+        {
+        MesswertNick = (long) ((long)MesswertNick * GyroFaktor) / (256L / STICK_GAIN)  ; // MartinR : hinzugefügt
+        MesswertRoll = (long) ((long)MesswertRoll * GyroFaktor) / (256L / STICK_GAIN) ;  // MartinR : hinzugefügt
+        }
- if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else  if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
+        else // MartinR so war es
+        {
+    MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
+    MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
     MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
     MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
+    MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
-    MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
+    // Maximalwerte abfangen
         // MartinR : Prüfen ob Unterscheidung nach Platinenversion erforderlich?
     // Maximalwerte abfangen
 //    #define MAX_SENSOR  (4096*STICK_GAIN)
     #define MAX_SENSOR  (4096*4)
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Höhenregelung
 // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   GasMischanteil *= STICK_GAIN;
+        Mess_Integral_Hoch += AdWertAccHoch;      // Integrieren // MartinR: zu Höhenregler in fc.c verschoben
+    Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen // MartinR: zu Höhenregler in fc.c verschoben
+        // MartinR: Änderungen HoheD Anfang :
+                HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
+                HoehenWertSumme += HoehenWert ;
+                if (++ iHoehe >= 40) // MartinR: Wartezeit um auch kleine Änderungsgeschwindigkeiten erkennen zu können
+                {
+                 HoehenWertSumme = HoehenWertSumme / iHoehe ; // MartinR: Mittelwert während der Wartezeit
+                 HoeheD = (int) Parameter_Luftdruck_D * (HoehenWert - HoehenWertalt) ;
+                 if (HoeheD < 0) HoeheD += Parameter_Luftdruck_D ; // MartinR: Digitalisierungsrauschen abmindern
+                 if (HoeheD > 0) HoeheD -= Parameter_Luftdruck_D ; // MartinR: entspricht - 1 Bit vom Höhenwert
+                 iHoehe = 0 ;
+                 HoehenWertalt = HoehenWertSumme ;
+                 HoehenWertSumme = 0 ;
+                }
+        // MartinR: Änderungen HoheD Ende
+  // MartinR: bisheriger Höhenregler
+  /*
  if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
+  {
     int tmp_int;
         static char delay = 100;
     if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
       if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
       GasMischanteil = hoehenregler;
+     }
+  }
   if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
+  */
+// MartinR: Ende bisheriger Höhenregler
+// MartinR: Neuer Höhenregler von MartinR: Anfang
+   if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
+  {
+    int tmp_int;
+        static char delay = 100;
+    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
+    {
+      //if(Parameter_MaxHoehe < 50)  // MartinR :so war es
+          if(Parameter_MaxHoehe < 50  || (Parameter_UserParam1 > 140) )   // MartinR: Schalter aus oder HH über UsererParam1 an
+      {
+       if(!delay--)
+            {
+         if((MessLuftdruck > 1000) && OCR0A < 254)
+                  {
+                    if(OCR0A < 244)
+                     {
+                      ExpandBaro -= 10;
+                      OCR0A = DruckOffsetSetting - ExpandBaro;
+                         }
+                         else OCR0A = 254;
+                beeptime = 300;
+            delay = 250;
+                  }
+                  else
+         if((MessLuftdruck < 100) && OCR0A > 1)
+                  {
+                   if(OCR0A > 10)
+                    {
+                     ExpandBaro += 10;
+                     OCR0A = DruckOffsetSetting - ExpandBaro;
+                        }
+                         else OCR0A = 1;
+                   beeptime = 300;
+           delay = 250;
+                  }
+          else
+          {
+               //SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters // MartinR : so war es
+                   SollHoehe = HoehenWert; // MartinR : geändert
+           HoehenReglerAktiv = 0;
+                   delay = 1;
+                  }
+                }
+                SollHoehe = HoehenWert;  // MartinR: SollHoehe wird nachgeführt bis HoehenreglerAktiv = 1
+                SchwebeGas = GasMischanteil;  // MartinR: SchwebeGas wird nachgeführt bis HoehenreglerAktiv = 1
+                HoehenReglerAktiv = 0; // MartinR: zur Sicherheit
+      }
+      else // Schalter an und kein HH über Schalter
+        {
+         HoehenReglerAktiv = 1;
+                 // Schwebegas ist der letzte GasMischanteil vor dem Aktivieren der Höhenregelung
+                 delay = 200;
+                }
+    }
+    else // Sollhöhe über Poti
+    {
+     //SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20; // MartinR : so war es
+         //HoehenReglerAktiv = 1; // MartinR : so war es
+         if(Parameter_UserParam1 > 140) // HH über Schalter: Höhenregler abgeschaltet, Nachführen von Parametern
+         {
+           SollHoehe = HoehenWert;  // SollHoehe wird nachgeführt bis HoehenreglerAktiv = 1
+       HoehenReglerAktiv = 0;
+         }
+         else  // Höhenregler mit Sollhöhe über Poti aktiv
+         {
+          SchwebeGas = GasMischanteil;  // SchwebeGas zunächst = Gasmischanteil bis bessere Lösung
+      HoehenReglerAktiv = 1;
+          // MartinR: Versuch Höhenregler zu verlangsamen.  Steig- / Sinkrate im MK-Tool über UserParam4 einstellbar
+                if (((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20 > SollHoehe)
+                {
+                        if(++delay_Hoehenregler > (Parameter_UserParam4 / 4))
+                        {
+                        delay_Hoehenregler = 0;
+                        SollHoehe ++ ; //MartinR: Höhenänderung begrenzen.
+                        }
+                }
+                else
+                {
+                        if (((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20 < SollHoehe)
+                        {
+                                if(++delay_Hoehenregler > (Parameter_UserParam4 / 4))
+                                {
+                                delay_Hoehenregler = 0;
+                                SollHoehe -- ; //MartinR: Höhenänderung begrenzen.
+                                }
+                        }
+                }
+         }
+    }
+    if(Notlandung) SollHoehe = 0;
+        // MartinR: so war es Anfang
+        /*
+    h = HoehenWert;
+    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
+     {
+      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / (16 / STICK_GAIN); // Differenz bestimmen --> P-Anteil
+      h = GasMischanteil - h;         // vom Gas abziehen
+      h -= (HoeheD)/(8/STICK_GAIN);    // D-Anteil
+      tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
+      if(tmp_int > 70*STICK_GAIN) tmp_int = 70*STICK_GAIN;
+      else if(tmp_int < -(70*STICK_GAIN)) tmp_int = -(70*STICK_GAIN);
+      h -= tmp_int;
+      hoehenregler = (hoehenregler*15 + h) / 16;
+      if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN
+       {
+         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN;
+         if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil;
+       }
+      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
+      GasMischanteil = hoehenregler;
+     }
+  }
+  */  // MartinR: so war es Ende
+        // MartinR: neuer Höhenregler Anfang
+         if(HoehenReglerAktiv && !(Parameter_UserParam1 > 140))      // kein HH
+        {
+          h_delta = (int) SollHoehe - (int) HoehenWert; // positiver Wert: MK ist unterhalb der Sollhöhe
+          h_p = (h_delta * (int) Parameter_Hoehe_P) / (16 / STICK_GAIN); // Differenz bestimmen --> P-Anteil
+      h_d = (int) (HoeheD)/(64/STICK_GAIN);    // D-Anteil (8*8=64)
+          // ACC-D-Anteil
+      tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
+      if(tmp_int > 50*STICK_GAIN) tmp_int = 50*STICK_GAIN;
+      else if(tmp_int < -(50*STICK_GAIN)) tmp_int = -(50*STICK_GAIN);
+      h_d += tmp_int; // positiver D-Anteil bedeutet Steigen
+          h = SchwebeGas + h_p - h_d ;
+          hoehenregler = (hoehenregler*7 + h) / 8;
+          if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN
+      {
+        if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN;
+        if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil;
+      }
+          if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
+          //else hoehenregler += (GasMischanteil-hoehenregler)/16  ; // um die Höhe noch leicht mit Gas korrigieren zu können
+          //if(SchwebeGas < 20)  SchwebeGas = 20; // nicht unter 20
+          GasMischanteil = hoehenregler;
+        }
+        else  // Integratoren auf Null halten
+        {
+        Mess_Integral_Hoch = 0;
+        hoehenregler = GasMischanteil;
+        h_p = 0 ;
+        h_d = 0 ;
+        }
+  }
+         // MartinR: neuer Höhenregler Ende
+  if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
+ // MartinR: Ende neuer Höhenregler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mischer und PI-Regler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
+        // MartinR : so war es Anfang
-    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
+        /*
     if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
     else  SummeNick += DiffNick; // I-Anteil bei HH
     if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
     if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
     pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick
+    // Motor Vorn
+        */
+        // MartinR : so war es Ende
+        // MartinR : geändert Anfang
+        if(IntegralFaktor) // MartinR : ACC-Mode
+         {
+          SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
+          if(SummeNick >  (STICK_GAIN * 8000L)) SummeNick =  (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf
+      if(SummeNick < -(8000L * STICK_GAIN)) SummeNick = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf
+          pd_ergebnis_nick = DiffNick + (SummeNick / Ki);
+          SummeNickHH = 0 ;
+         }
+    else // MartinR : HH-Mode
+         {
+          SummeNickHH += DiffNick; // I-Anteil bei HH
+      if(SummeNickHH >  (STICK_GAIN * 8000L)) SummeNickHH =  (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf
+      if(SummeNickHH < -(8000L * STICK_GAIN)) SummeNickHH = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf
+          pd_ergebnis_nick = DiffNick + SummeNickHH / KiHH; // MartinR: PI-Regler für Nick bei HH
+          SummeNick = 0;
+     }
+        // MartinR : geändert Ende
     // Motor Vorn
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
     if(pd_ergebnis_nick >  tmp_int) pd_ergebnis_nick =  tmp_int;
+    if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;
     if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Achse
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+        // MartinR : so war es Anfang
-        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
+        /*
     if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung
     else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
     if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
+    if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
+    pd_ergebnis_roll = DiffRoll + SummeRoll / Ki;       // PI-Regler für Roll
+        */
+        // MartinR : so war es Ende
+        // MartinR : geändert Anfang
+        if(IntegralFaktor) // MartinR : ACC-Mode
+         {
+         SummeRoll += IntegralRollMalFaktor - StickRoll;
+         if(SummeRoll >  (STICK_GAIN * 8000L)) SummeRoll =  (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf
+     if(SummeRoll < -(8000L * STICK_GAIN)) SummeRoll = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf
+         tmp_int = SummeRoll / Ki;
+         pd_ergebnis_roll = DiffRoll + tmp_int;         // MartinR: PI-Regler im ACC-Mode
+         //SummeRollHH = (IntegralRollMalFaktor + tmp_int - stick_roll_neutral + (TrimRoll * STICK_GAIN / 2)) * KiHH;// MartinR: Startwert von SummeRollHH bei Umschaltung auf HH
+         // MartinR: Hintergrund: pd_ergebnis_xx soll sich beim Umschalten nicht ändern!
+         SummeRollHH = 0;
+         }
+    else // MartinR : HH-Mode
+         {
+          SummeRollHH += DiffRoll;  // I-Anteil bei HH
+      if(SummeRollHH >  (STICK_GAIN * 8000L)) SummeRollHH =  (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf
+      if(SummeRollHH < -(8000L * STICK_GAIN)) SummeRollHH = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf
+          pd_ergebnis_roll = DiffRoll + SummeRollHH / KiHH;     // MartinR: PI-Regler für Roll bei HH
+          SummeRoll = 0;
+         }
+        // MartinR : geändert Ende
     if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
     pd_ergebnis_roll = DiffRoll + SummeRoll / Ki;       // PI-Regler für Roll
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
     if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;

Subversion Repositories FlightCtrl

(root)/branches/FC_075b_ACC-HH_MartinR/fc.c - Rev 1232 → 1247