WebSVN - FlightCtrl - Diff - Rev 1862 and 1865 - /branches/V0.88e_ACC-HH_HR

     DebugOut.Analog[14] = Motor[2].SetPoint;
     DebugOut.Analog[15] = Motor[3].SetPoint;
     DebugOut.Analog[20] = ServoNickValue;
     DebugOut.Analog[22] = Capacity.ActualCurrent;
     DebugOut.Analog[23] = Capacity.UsedCapacity;
+DebugOut.Analog[24] = SollHoehe/5;
 //    DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ;
 //    DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
     DebugOut.Analog[27] = KompassSollWert;
         DebugOut.Analog[29] = Capacity.MinOfMaxPWM;
     DebugOut.Analog[30] = GPS_Nick;
     stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // CareFree und freie Wahl der vorderen Richtung
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-if(CareFree)
+        if(CareFree)
- {
+        {
-    signed int nick, roll;
+                signed int nick, roll;
-        nick = stick_nick / 4;
+                nick = stick_nick / 4;
-        roll = stick_roll / 4;
+                roll = stick_roll / 4;
-    StickNick = ((FromNC_Rotate_C * nick) + (FromNC_Rotate_S * roll)) / (32 / 4);
+                StickNick = ((FromNC_Rotate_C * nick) + (FromNC_Rotate_S * roll)) / (32 / 4);
-    StickRoll = ((FromNC_Rotate_C * roll) - (FromNC_Rotate_S * nick)) / (32 / 4);
+                StickRoll = ((FromNC_Rotate_C * roll) - (FromNC_Rotate_S * nick)) / (32 / 4);
- }
+        }
- else
+        else
- {
+        {
-        FromNC_Rotate_C = sintab[EE_Parameter.OrientationAngle + 6];
+                FromNC_Rotate_C = sintab[EE_Parameter.OrientationAngle + 6];
-        FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle];
+                FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle];
-    StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8;
+                StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8;
-    StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8;
+                StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8;
+        }
     StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
        if(v > w) w = v; // grösste Neigung ermitteln
        korrektur = w / 4 + 1;
            // Kompassfehlerwert bestimmen
            fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
            // GIER_GRAD_FAKTOR ist ca. 1200
            // Kompasswert einloggen
        if(KompassSignalSchlecht) KompassSignalSchlecht--;
            else
        if(w < 25)
+        {
      // MK Gieren
         if(!NeueKompassRichtungMerken)
+       {
-           r = ((540 + (KompassSollWert - ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
-DebugOut.Analog[19] = r;
+           r = ((540 + (KompassSollWert - ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
-           v = r * (Parameter_KompassWirkung/2);  // nach Kompass ausrichten
-//           if(abs(r) > 20) v *= 2; // über 20° -> doppelt so schnell
+           v = r * (Parameter_KompassWirkung/2);  // nach Kompass ausrichten
                    CompassGierSetpoint = v / 16;
        }
-     else CompassGierSetpoint = 0;
-/*
-DebugOut.Analog[16] = fehler;
-DebugOut.Analog[17] = korrektur;
-*/
-DebugOut.Analog[25] = CompassGierSetpoint;
-DebugOut.Analog[18] = KompassFusion;
-/*
-       w = (w * Parameter_KompassWirkung) / 32;           // auf die Wirkung normieren
-       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
-       if(w >= 0)
-        {
-          if(!KompassSignalSchlecht)
-          {
-           v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
-DebugOut.Analog[16] = korrektur;
-           r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassSollWert) % 360) - 180;
-           v = (r * w) / v;  // nach Kompass ausrichten
-           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 CompassGierSetpoint = 0;
       } // CalculateCompassTimer
+     }
          else CompassGierSetpoint = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                                                 SollHoehe = HoehenWert; // update setpoint to current heigth
+                                        }
                                         FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP;
                                         HeightTrimming += abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD));
                                         VarioCharacter = '+';
+                                        WaypointTrimming = 0;
                                 } // gas stick is below hoover point
                                 else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit )
+                                {
                                         if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP)
+                                        {
                                                 SollHoehe = HoehenWert; // update setpoint to current heigth
+                                        }
                                         FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN;
                                         HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD));
                                         VarioCharacter = '-';
+                                        WaypointTrimming = 0;
+                                }
                                 else // Gas Stick in Hover Range
+                                {
                                         VarioCharacter = '=';
                     if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint > SollHoehe) // von NC gesteuert -> Steigen
+                                         {
                                                 FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP;
                                                 HeightTrimming += FromNC_AltitudeSpeed;
                                                 WaypointTrimming = 10;
                                                 VarioCharacter = '^';
+                                                if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN)  // changed from sinking to rising
+                                                {
+                                                        FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN;
+                                                        SollHoehe = HoehenWert; // update setpoint to current heigth
+                                                }
+                                         }
                                          else
                     if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint < SollHoehe) // von NC gesteuert -> sinken
+                                         {
                                                 FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN;
                                                 HeightTrimming -= FromNC_AltitudeSpeed;
                                                 WaypointTrimming = -10;
                                                 VarioCharacter = 'v';
+                                                if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) // changed from rising to sinking
+                                                {
+                                                        FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP;
+                                                        SollHoehe = HoehenWert; // update setpoint to current heigth
+                                                }
+                                         }
                                         else
                                         if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN))
+                                        {
                                                 if(!WaypointTrimming) SollHoehe = HoehenWert; // update setpoint to current height
                                 {   // reduce the time constant of averaging by factor of 2 to get much faster a stable value
                                         HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L);
                                         HoverGasFilter += 4L * tmp_long2;
+                                }
                           else //later
-                          if(abs(VarioMeter) < 100) // only on small vertical speed
+                          if(abs(VarioMeter) < 100 && abs(HoehenWert - SollHoehe) < 256) // only on small vertical speed & difference is small (only descending)
+                                {
                                         HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE;
                                         HoverGasFilter += tmp_long2;
+                                }
                                 HoverGas = (int16_t)(HoverGasFilter/HOVER_GAS_AVERAGE);

Subversion Repositories FlightCtrl

(root)/branches/V0.88e_ACC-HH_HR_MartinR/fc.c @ 2165 - Rev 1862 → 1865