WebSVN - FlightCtrl - Diff - Rev 2380 and 2384 - /branches/V2.01a_ACC-HH

 unsigned char CalibrationDone = 0;
 char NeueKompassRichtungMerken = 0;
 int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 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_GlobalConfig;
 unsigned char Parameter_ExtraConfig;
 unsigned char Parameter_MaximumAltitude;
 unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;
 unsigned char CareFree = 0;
-const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; // 15° steps
+//const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; // 15° steps // MartinR: so war es
+const signed char sintab[62] = { 0, 2, 4, 6, 8, 10, 12, 13, 14, 15, 16, 16, 16, 16, 16, 15, 14, 13, 12, 10, 8, 6, 4, 2, 0, -2, -4, -6, -8, -10, -12, -13, -14, -15, -16, -16, -16, -16, -16, -15, -14, -13, -12, -10, -8, -6, -4, -2, 0, 2, 4, 6, 8, 10, 12, 13, 14, 15, 16, 16, 16}; // 7,5° steps //MartinR
+signed char cosinus, sinus; // MartinR : extern für PAN-Funktion
 signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
+//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 char stick_p; // MartinR: Test
+unsigned char Parameter_NaviGpsModeControl;     // MartinR:
 int MaxStickNick = 0,MaxStickRoll = 0;
 unsigned int  modell_fliegt = 0;
 volatile unsigned char FC_StatusFlags = 0, FC_StatusFlags2 = 0;
 long GIER_GRAD_FAKTOR = 1291;
 signed int KopplungsteilNickRoll,KopplungsteilRollNick;
     DebugOut.Analog[11] = ErsatzKompassInGrad;
     DebugOut.Analog[12] = Motor[0].SetPoint;
     DebugOut.Analog[13] = Motor[1].SetPoint;
     DebugOut.Analog[14] = Motor[2].SetPoint;
     DebugOut.Analog[15] = Motor[3].SetPoint;
+        DebugOut.Analog[16] = cosinus;  // MartinR: zur Einstellung der Pan-Funktion
+        DebugOut.Analog[17] = sinus;  // MartinR: test zur Einstellung der Pan-Funktion
+        DebugOut.Analog[18] = ServoPanValue;  // MartinR:  zur Einstellung der Pan-Funktion
+        DebugOut.Analog[19] = ServoRollValue;  // MartinR:  Test
     DebugOut.Analog[20] = ServoNickValue;
         DebugOut.Analog[21] = HoverGas;
     DebugOut.Analog[22] = Capacity.ActualCurrent;
     DebugOut.Analog[23] = Capacity.UsedCapacity;
         DebugOut.Analog[24] = SollHoehe/10;
             tmpl4 *= Parameter_AchsKopplung2; //65
             tmpl4 /= 4096L;
             KopplungsteilNickRoll = tmpl3;
             KopplungsteilRollNick = tmpl4;
             tmpl4 -= tmpl3;
+                        if(IntegralFaktor) // MartinR: nur im ACC-Mode
+                        {
             ErsatzKompass += tmpl4;
             if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen
+                        }
             tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
             tmpl *= Parameter_AchsKopplung1;  // 90
             tmpl /= 4096L;
             tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
             tmpl2 *= Parameter_AchsKopplung1;
 #define D_LIMIT 128
    MesswertNick = HiResNick / 8;
+   MesswertRoll = HiResRoll / 8;
+        // MartinR : so war es Anfang
-   MesswertRoll = HiResRoll / 8;
+        /*
    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
+#ifdef NO_GYRO_PROGRESSION      // MartinR; keine Begrenzung
+        #warning : "### NO_GYRO_PROGRESSION ###"
+        //nichts tun
+#else                                   // mit Begrenzung
+         // 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 <  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;  }
+#endif
   if(Parameter_Gyro_D)
+  {
  Parameter_ExtraConfig = EE_Parameter.ExtraConfig;
 // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
  CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability);
  CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl);
  Ki = 10300 / (Parameter_I_Faktor + 1);
+ //Ki = (10300 / (Parameter_I_Faktor + 4)) + (StickGas /2); // MartinR: Test Gasabhängige Regelung
+ if(Parameter_UserParam1 > 50) KiHH = 10300 / (Parameter_UserParam2 + 1); else  KiHH = Ki; // MartinR : für HH über Schalter
+ //if(Parameter_UserParam1 > 50) KiHH = (10300 / (Parameter_UserParam2 + 4)) + (StickGas /2); else  KiHH = Ki; // MartinR : für HH über Schalter  // MartinR: Test Gasabhängige Regelung
+ //Parameter_NaviGpsModeControl = EE_Parameter.NaviGpsModeControl; //MartinR: Standard: EE_Parameter.NaviGpsModeControl wird übertragen
+ Parameter_NaviGpsModeControl = GetChannelValue(EE_Parameter.NaviGpsModeChannel); //MartinR: Standard: EE_Parameter.NaviGpsModeChannel wird übertragen
+ if(!IntegralFaktor) Parameter_NaviGpsModeControl= 0; // MartinR: wenn HH dann GPS auf free- Mode
+  // 0 = free; 100 = AID; 200 = coming home //neu
  MAX_GAS = EE_Parameter.Gas_Max;
  MIN_GAS = EE_Parameter.Gas_Min;
  if(EE_Parameter.CareFreeChannel)
 void MotorRegler(void)
 //############################################################################
+{
          int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;
          int GierMischanteil,GasMischanteil;
+         static long SummeNickHH=0,SummeRollHH=0; // MartinR: hinzugefügt
      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;
 //                                                                      ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
                                                                         NeueKompassRichtungMerken = 100; // 2 sekunden
 #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
                                                                         SpeakHoTT = SPEAK_STARTING;
 #endif
+                                                                        // MartinR: hinzugefügt Anfang
+                                                                        stick_nick_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; //  aktueller Stickwert wird als Neutralposition im HH verwendet, MartinR
+                                                                        stick_roll_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; //  aktueller Stickwert wird als Neutralposition im HH verwendet, MartinR
+                                                                        SummeNickHH = 0 ; // Zurücksetzen der Integratoren
+                                                                        SummeRollHH = 0 ; // Zurücksetzen der Integratoren
+                                                                        // MartinR: hinzugefügt Ende
+                                                                }
                                                                 else
+                                                                {
                                                                         beeptime = 1500; // indicate missing calibration
 #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
+  {
         static int stick_nick,stick_roll;
         unsigned char stick_p;
     ParameterZuordnung();
         stick_p = EE_Parameter.Stick_P;
+        // MartinR: original:
+        /*
     stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * stick_p) / 4;
     stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
     stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4;
     stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
+                        */
+// MartinR: geändert Anfang
+         if(IntegralFaktor)  // ACC-Mode
+                {
+                stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * stick_p) / 4;
+                stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4;
+                stick_nick_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
+                stick_roll_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_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;
+                }
+        else            // HH-Mode
+                {
+                        if(Parameter_UserParam1 > 49)   // MartinR: zweiter Stick_P Wert nur, wenn HH über Schalter aktiv ist
+                        {
+                        stick_nick = ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] - stick_nick_neutral) * Parameter_UserParam3);
+                        stick_roll = ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] - stick_roll_neutral) * Parameter_UserParam3);
+                        }
+                        else
+                        {
+                        stick_nick = ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] - stick_nick_neutral) * stick_p);
+                        stick_roll = ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] - stick_roll_neutral) * stick_p);
+                        }
+                }
+// MartinR: geändert Ende
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // CareFree und freie Wahl der vorderen Richtung
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+        //if(CareFree) // MartinR: so war es
-        if(CareFree)
+        if(CareFree && IntegralFaktor) // MartinR: CareFree nur im ACC-Mode
+        {
                 signed int nick, roll;
                 nick = stick_nick / 4;
                 roll = stick_roll / 4;
                 StickNick = ((FromNC_Rotate_C * nick) + (FromNC_Rotate_S * roll)) / (32 / 4);
                 StickRoll = ((FromNC_Rotate_C * roll) - (FromNC_Rotate_S * nick)) / (32 / 4);
+        }
         else
         {
+                //FromNC_Rotate_C = sintab[EE_Parameter.OrientationAngle + 6]; //MartinR: so war es
-                FromNC_Rotate_C = sintab[EE_Parameter.OrientationAngle + 6];
+                FromNC_Rotate_C = (sintab[EE_Parameter.OrientationAngle * 2 + 12]) / 2; //MartinR: feinere Auflösung
+                //FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle]; //MartinR: so war es
-                FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle];
+                FromNC_Rotate_S = (sintab[EE_Parameter.OrientationAngle * 2]) / 2; //MartinR: feinere Auflösung
                 StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8;
                 StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8;
+        }
     StickNick -= GPS_Nick;
     StickRoll -= GPS_Roll;
         StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 127;
     GyroFaktor     = (Parameter_Gyro_P + 10.0);
-    IntegralFaktor = Parameter_Gyro_I;
+        // IntegralFaktor = Parameter_Gyro_I; // MartinR: war mal hier
     GyroFaktorGier     = (Parameter_Gyro_Gier_P + 10.0);
     IntegralFaktorGier = Parameter_Gyro_Gier_I;
  MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
  MittelIntegralRoll  += IntegralRoll;
  MittelIntegralNick2 += IntegralNick2;
  MittelIntegralRoll2 += IntegralRoll2;
+  if((!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR:
+          // nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH
+          // um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten!
+          // bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet
+        {
+        IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
+    IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
+    Mess_IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
+    Mess_IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
+    Mess_Integral_Gier = 0;     // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
+        sollGier = 0;
+        Integral_Gier = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
+    //Mess_Integral_Gier2 = 0; // MartinR:  Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
+        KompassSollWert = KompassValue; // MartinR: aktuelle Ausrichtung beibehalten
+        ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; // MartinR: aktuelle Ausrichtung beibehalten
+        NeueKompassRichtungMerken = 1; // MartinR: aktuelle Ausrichtung beibehalten
+        }
+// if(Looping_Nick || Looping_Roll)  // MartinR: so war es
+   if(Looping_Nick || Looping_Roll || (!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: erweitert
- if(Looping_Nick || Looping_Roll)
+   // MartinR: beim ACC-Loop oder beim zurückschalten von HH auf ACC
+  {
     IntegralAccNick = 0;
     IntegralAccRoll = 0;
     MittelIntegralNick = 0;
     ZaehlMessungen = 0;
     LageKorrekturNick = 0;
     LageKorrekturRoll = 0;
+  }
+        if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 49)) IntegralFaktor = 0; // MartinR geändert und verschoben
+          else IntegralFaktor = Parameter_Gyro_I; // MartinR: geändert
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin))
+  {
    long tmp_long, tmp_long2;
  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) // MartinR: so war es
+  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;
     if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    //if(KompassValue >= 0 && (Parameter_GlobalConfig & CFG_KOMPASS_AKTIV)) // MartinR: so war es
-    if(KompassValue >= 0 && (Parameter_GlobalConfig & CFG_KOMPASS_AKTIV))
+        if((KompassValue >= 0 && (Parameter_GlobalConfig & CFG_KOMPASS_AKTIV))  &&  !(Parameter_UserParam1 > 50))  // MartinR: bei HH über Schalter wird der Kompass abgeschaltet
+     {
       if(CalculateCompassTimer-- == 1)
+          {
        int w,v,r,fehler,korrektur; // wird von der SPI-Routine auf 1 gesetzt
 #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;
+        //MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);// MartinR so war es
+    //MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);// MartinR so war es
+        if(!IntegralFaktor) // MartinR : HH-Mode hinzugefügt
+        {
+        MesswertNick = (long) ((long)MesswertNick * GyroFaktor) / (256L / STICK_GAIN)  ; // MartinR : hinzugefügt
+        MesswertRoll = (long) ((long)MesswertRoll * GyroFaktor) / (256L / STICK_GAIN) ;  // MartinR : hinzugefügt
+        //MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN);
+        //Mess_Integral_Gier = 0;       // MartinR: nur Kompass wird bei HH deaktiviert
+        //Integral_Gier = 0; // MartinR: nur Kompass wird bei HH deaktiviert
+        }
- if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else  if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
+        else // MartinR: ACC-Mode  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));
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Höhenregelung
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen
   GasMischanteil *= STICK_GAIN;
+//MartinR: Höhenregler nur mit 1284er Prozessor:
+#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
         // 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
                           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
+                        // if(Parameter_HoehenSchalter < 50) // for 3 or 2-state switch height control is disabled in lowest position // MartinR :so war es
+                        if((Parameter_HoehenSchalter < 50) || (Parameter_UserParam1 > 140) )   // MartinR: Schalter aus oder HH ohne Höhenregler über UsererParam1 an
                         {   //height control not active
                                 if(!delay--)
+                                {
 #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
                                     if(!SpeakHoTT && HoehenReglerAktiv)  SpeakHoTT = SPEAK_ALTITUDE_OFF;
+                        }
+                }
                 else // no switchable height control
+                {
                         SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_HoehenSchalter) * (int)EE_Parameter.Hoehe_Verstaerkung;
+                        // HoehenReglerAktiv = 1; // MartinR : so war es
+                        // MartinR : geändert Anfang
+                                if(Parameter_UserParam1 > 140) // HH über Schalter: HH an + Höhenregler abgeschaltet, Nachführen von Parametern
+                                {
+                                        HoehenReglerAktiv = 0;
+                                }
+                                else  // Höhenregler mit Sollhöhe über Poti aktiv
+                                {
-                        HoehenReglerAktiv = 1;
+                                        HoehenReglerAktiv = 1;
+                                }
+                        // MartinR : geändert Ende
+                }
                 // calculate cos of nick and roll angle used for projection of the vertical hoover gas
                 tmp_int  = (int)(IntegralNick/GIER_GRAD_FAKTOR);  // nick angle in deg
                 tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR);  // roll angle in deg
                 CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg
                 LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle
+        {
                 // set undefined state to indicate vario off
                 FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN);
         } // EOF no height control
+// MartinR: Höhenregler nur noch mit 1284er Prozessor
+#endif
    // Limits the maximum gas in case of "Out of Range emergency landing"
    if(NC_To_FC_Flags & NC_TO_FC_EMERGENCY_LANDING)
+        {
          if(GasMischanteil/STICK_GAIN > HooverGasEmergencyPercent && HoverGas) GasMischanteil = HooverGasEmergencyPercent * STICK_GAIN;
+    }
     tmp_int = MAX_GAS*STICK_GAIN;
     if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
     if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Nick / Roll-Achse  // MartinR: um Code zu sparen wurde Nick und Roll zusammengefasst
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
+        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
+        // PI-Regler für Nick und Roll
+        if(EE_Parameter.Gyro_Stability <= 8)
+        {
+        pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 ; // Zwischenergebnis um Code zu sparen
+        pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8;
+        }
+        else
+        {
+        pd_ergebnis_nick = ((EE_Parameter.Gyro_Stability / 2) * DiffNick) / 4; // Überlauf verhindern
+        pd_ergebnis_roll = ((EE_Parameter.Gyro_Stability / 2) * DiffRoll) / 4;  // Überlauf verhindern
+        }
+        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 += (SummeNick / Ki); // PI-Regler für Nick
+          SummeNickHH = 0 ;
+          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
+          pd_ergebnis_roll += (SummeRoll / Ki); // PI-Regler für Roll
+          SummeRollHH = 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 += SummeNickHH / KiHH; // MartinR: PI-Regler für Nick bei HH
+          SummeNick = 0;
+          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 += SummeRollHH / KiHH;       // MartinR: PI-Regler für Roll bei HH
+          SummeRoll = 0;
+     }
+        // MartinR : geändert Ende
+    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;
+        //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;
+// MartinR: alt
 /*
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
     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;
+        // MartinR: alt Ende
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Universal Mixer
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Subversion Repositories FlightCtrl

(root)/branches/V2.01a_ACC-HH_MartinR/fc.c @ 2384 - Rev 2380 → 2384