WebSVN - FlightCtrl - Diff - Rev 1378 and 1550 - /branches/V0.78g

 int   ErsatzKompassInGrad; // Kompasswert in Grad
 int   GierGyroFehler = 0;
 char GyroFaktor,GyroFaktorGier;
 char IntegralFaktor,IntegralFaktorGier;
 int  DiffNick,DiffRoll;
-int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
+//int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
+unsigned char Poti[9] = {0,0,0,0,0,0,0,0};
 volatile unsigned char SenderOkay = 0;
 volatile unsigned char SenderRSSI = 0;
 int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
 char MotorenEin = 0;
 long HoehenWert = 0;
 unsigned char Parameter_NaviGpsACC;
 unsigned char Parameter_NaviOperatingRadius;
 unsigned char Parameter_NaviWindCorrection;
 unsigned char Parameter_NaviSpeedCompensation;
 unsigned char Parameter_ExternalControl;
+unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;
 struct mk_param_struct EE_Parameter;
 signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
 int MaxStickNick = 0,MaxStickRoll = 0;
 unsigned int  modell_fliegt = 0;
-volatile unsigned char MikroKopterFlags = 0;
+volatile unsigned char FCFlags = 0;
 long GIER_GRAD_FAKTOR = 1291;
 signed int KopplungsteilNickRoll,KopplungsteilRollNick;
-unsigned char RequiredMotors = 4;
-unsigned char Motor[MAX_MOTORS];
 signed int tmp_motorwert[MAX_MOTORS];
 unsigned char LoadHandler = 0;
-#define LIMIT_MIN(value, min) {if(value < min) value = min;}
+#define LIMIT_MIN(value, min) {if(value <= min) value = min;}
-#define LIMIT_MAX(value, max) {if(value > max) value = max;}
+#define LIMIT_MAX(value, max) {if(value >= max) value = max;}
-#define LIMIT_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
+#define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;}
 int MotorSmoothing(int neu, int alt)
+{
  int motor;
     ExternHoehenValue = 0;
     ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
     GierGyroFehler = 0;
     SendVersionToNavi = 1;
     LED_Init();
-    MikroKopterFlags |= FLAG_CALIBRATE;
+    FCFlags |= FCFLAG_CALIBRATE;
     FromNaviCtrl_Value.Kalman_K = -1;
     FromNaviCtrl_Value.Kalman_MaxDrift = 0;
     FromNaviCtrl_Value.Kalman_MaxFusion = 32;
-    Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110;
-    Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110;
+   for(i=0;i<8;i++)
+    {
-    Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110;
+     Poti[i] =  PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 110;
-    Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110;
+        }
     SenderOkay = 100;
     if(ServoActive)
+         {
                 HEF4017R_ON;
                 DDRD  |=0x80; // enable J7 -> Servo signal
 //############################################################################
+{
     static signed long tmpl,tmpl2,tmpl3,tmpl4;
         static signed int oldNick, oldRoll, d2Roll, d2Nick;
         signed long winkel_nick, winkel_roll;
+    unsigned char i;
         MesswertGier = (signed int) AdNeutralGier - AdWertGier;
 //    MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
     MesswertNick = (signed int) AdWertNickFilter / 8;
     MesswertRoll = (signed int) AdWertRollFilter / 8;
     RohMesswertNick = MesswertNick;
     if(RohMesswertNick > 256)       MesswertNick += 1 * (RohMesswertNick - 256);
     else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256);
     if(RohMesswertRoll > 256)       MesswertRoll += 1 * (RohMesswertRoll - 256);
     else if(RohMesswertRoll < -256) MesswertRoll += 1 * (RohMesswertRoll + 256);
+  }
+  for(i=0;i<8;i++)
+    {
-    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
+     int tmp;
-    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
+         tmp = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 110;
-    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
-    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
-    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
+         if(tmp > 255) tmp = 255; else if(tmp < 0) tmp = 0;
-    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
+     if(tmp != Poti[i])
+          {
-    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
+           Poti[i] += (tmp - Poti[i]) / 8;
-    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
+       if(Poti[i] > tmp) Poti[i]--;
+           else Poti[i]++;
+          }
+        }
+}
 //############################################################################
 // Messwerte beim Ermitteln der Nullage
 void CalibrierMittelwert(void)
 //############################################################################
+{
     unsigned char i;
     if(PlatinenVersion == 13) SucheGyroOffset();
     // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
         ANALOG_OFF;
         MesswertNick = AdWertNick;
         Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
         Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
         Mittelwert_AccHoch = (long)AdWertAccHoch;
    // ADC einschalten
     ANALOG_ON;
-    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
-    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
-    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
-    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
-    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
+   for(i=0;i<8;i++)
+    {
+     int tmp;
-    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
+         tmp = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 110;
-    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
+         LIMIT_MIN_MAX(tmp, 0, 255);
-    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
+     if(Poti[i] > tmp) Poti[i]--;  else  if(Poti[i] < tmp) Poti[i]++;
+        }
         Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
         Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
+}
 //############################################################################
+{
  unsigned char i;
     if(!MotorenEin)
+        {
-         MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY);
+         FCFlags &= ~(FCFLAG_MOTOR_RUN | FCFLAG_FLY);
                  for(i=0;i<MAX_MOTORS;i++)
+                  {
                    if(!PC_MotortestActive)  MotorTest[i] = 0;
-                   Motor[i] = MotorTest[i];
+                   Motor[i].SetPoint = MotorTest[i];
+                  }
           if(PC_MotortestActive) PC_MotortestActive--;
+        }
-        else MikroKopterFlags |= FLAG_MOTOR_RUN;
+        else FCFlags |= FCFLAG_MOTOR_RUN;
-    DebugOut.Analog[12] = Motor[0];
+    DebugOut.Analog[12] = Motor[0].SetPoint;
-    DebugOut.Analog[13] = Motor[1];
+    DebugOut.Analog[13] = Motor[1].SetPoint;
-    DebugOut.Analog[14] = Motor[3];
+    DebugOut.Analog[14] = Motor[2].SetPoint;
-    DebugOut.Analog[15] = Motor[2];
+    DebugOut.Analog[15] = Motor[3].SetPoint;
     //Start I2C Interrupt Mode
     twi_state = 0;
 //############################################################################
 // Trägt ggf. das Poti als Parameter ein
 void ParameterZuordnung(void)
 //############################################################################
+{
- #define CHK_POTI_MM(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
- #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; }
+ #define CHK_POTI(b,a) {if(a < 248) b = a; else b = Poti[255 - a];}
- CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
+ #define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max);}
  CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
  CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
- CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
- CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z,0,255);
- CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
  CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
- CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
- CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,0,255);
- CHK_POTI(Parameter_Gyro_Gier_P,EE_Parameter.Gyro_Gier_P,10,255);
- CHK_POTI(Parameter_Gyro_Gier_I,EE_Parameter.Gyro_Gier_I,0,255);
- CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
- CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
- CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
- CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
- CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
- CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255);
- CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255);
- CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);
- CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255);
- CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
- CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl,0,255);
- CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
- CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);
- CHK_POTI(Parameter_AchsKopplung2,    EE_Parameter.AchsKopplung2,0,255);
- CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection,0,255);
-// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
- 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_Servo3,EE_Parameter.Servo3);
+ CHK_POTI(Parameter_Servo4,EE_Parameter.Servo4);
+ CHK_POTI(Parameter_Servo5,EE_Parameter.Servo5);
+ CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe);
+ CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe);
+ CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung);
+ CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z);
+ CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung);
+ CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I);
+ CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D);
+ CHK_POTI(Parameter_Gyro_Gier_P,EE_Parameter.Gyro_Gier_P);
+ CHK_POTI(Parameter_Gyro_Gier_I,EE_Parameter.Gyro_Gier_I);
+ CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor);
+ CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1);
+ CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2);
+ CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3);
+ CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4);
+ CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5);
+ CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6);
+ CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7);
+ CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8);
+ CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl);
+ CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl);
+ CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit);
+ CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1);
+ CHK_POTI(Parameter_AchsKopplung2,    EE_Parameter.AchsKopplung2);
+ CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection);
+// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
+ CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability);
- CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255);
+ CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl);
  Ki = 10300 / (Parameter_I_Faktor + 1);
  MAX_GAS = EE_Parameter.Gas_Max;
  MIN_GAS = EE_Parameter.Gas_Min;
+}
 //############################################################################
 //
 void MotorRegler(void)
 //############################################################################
+        {
         if(RcLostTimer) RcLostTimer--;
         else
+         {
           MotorenEin = 0;
-          MikroKopterFlags &= ~FLAG_NOTLANDUNG;
+          FCFlags &= ~FCFLAG_NOTLANDUNG;
+         }
         ROT_ON;
         if(modell_fliegt > 1000)  // wahrscheinlich in der Luft --> langsam absenken
+            {
             GasMischanteil = EE_Parameter.NotGas;
-            MikroKopterFlags |= FLAG_NOTLANDUNG;
+            FCFlags |= FCFLAG_NOTLANDUNG;
             PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
             PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
             PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
             PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
             PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Emfang gut
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         if(SenderOkay > 140)
+            {
-                        MikroKopterFlags &= ~FLAG_NOTLANDUNG;
+                        FCFlags &= ~FCFLAG_NOTLANDUNG;
             RcLostTimer = EE_Parameter.NotGasZeit * 50;
             if(GasMischanteil > 40 && MotorenEin)
+                {
                 if(modell_fliegt < 0xffff) modell_fliegt++;
+                }
                   NeueKompassRichtungMerken = 1;
                   sollGier = 0;
                   Mess_Integral_Gier = 0;
 //                  Mess_Integral_Gier2 = 0;
+                 }
-                } else MikroKopterFlags |= FLAG_FLY;
+                } else FCFlags |= FCFLAG_FLY;
             if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
+                {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                            beeptime = 1000;
+                          }
                           else
+                          {
                                ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
+                               LipoDetection(0);
+                                                   LIBFC_ReceiverInit();
                            if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+                            {
                              if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
+                            }
                                                    ServoActive = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gas ist unten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
             if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)
+                {
+                                        // Motoren Starten
+                                        if(!MotorenEin)
-                // Starten
+                        {
-                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
+                                                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
-                    {
+                                                {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Einschalten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-                    if(++delay_einschalten > 200)
+                                                        if(++delay_einschalten > 200)
-                        {
+                                                        {
-                        delay_einschalten = 200;
+                                                                delay_einschalten = 0;
-                        modell_fliegt = 1;
+                                                                modell_fliegt = 1;
-                        MotorenEin = 1;
+                                                                MotorenEin = 1;
-                        sollGier = 0;
+                                                                sollGier = 0;
-                        Mess_Integral_Gier = 0;
+                                                                Mess_Integral_Gier = 0;
-                        Mess_Integral_Gier2 = 0;
+                                                                Mess_Integral_Gier2 = 0;
-                        Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
+                                                                Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
-                        Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
+                                                                Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
-                        Mess_IntegralNick2 = IntegralNick;
+                                                                Mess_IntegralNick2 = IntegralNick;
-                        Mess_IntegralRoll2 = IntegralRoll;
+                                                                Mess_IntegralRoll2 = IntegralRoll;
-                        SummeNick = 0;
+                                                                SummeNick = 0;
-                        SummeRoll = 0;
+                                                                SummeRoll = 0;
-                        MikroKopterFlags |= FLAG_START;
+                                                                FCFlags |= FCFLAG_START;
-                        }
+                                                        }
-                    }
+                                                }
-                    else delay_einschalten = 0;
+                                                else delay_einschalten = 0;
-                //Auf Neutralwerte setzen
+                                        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Auschalten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+                                        else // only if motors are running
+                                        {
-                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
+                                                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
-                    {
+                                                {
-                    if(++delay_ausschalten > 200)  // nicht sofort
+                                                        if(++delay_ausschalten > 200)  // nicht sofort
-                        {
+                                                        {
-                         MotorenEin = 0;
+                                                                MotorenEin = 0;
-                         delay_ausschalten = 200;
+                                                                delay_ausschalten = 0;
-                         modell_fliegt = 0;
+                                                                modell_fliegt = 0;
-                        }
+                                                        }
-                    }
+                                                }
-                else delay_ausschalten = 0;
+                                                else delay_ausschalten = 0;
+                                        }
+                }
+            }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // neue Werte von der Funke
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- if(!NewPpmData-- || (MikroKopterFlags & FLAG_NOTLANDUNG))
+ if(!NewPpmData-- || (FCFlags & FCFLAG_NOTLANDUNG))
+  {
         static int stick_nick,stick_roll;
     ParameterZuordnung();
     stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
+     {
       MaxStickRoll = abs(StickRoll)/STICK_GAIN;
       if(MaxStickRoll > 100) MaxStickRoll = 100;
+     }
      else MaxStickRoll--;
-    if(MikroKopterFlags & FLAG_NOTLANDUNG)  {MaxStickNick = 0; MaxStickRoll = 0;}
+    if(FCFlags & FCFLAG_NOTLANDUNG)  {MaxStickNick = 0; MaxStickRoll = 0;}
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Looping?
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Bei Empfangsausfall im Flug
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-   if(MikroKopterFlags & FLAG_NOTLANDUNG)
+   if(FCFlags & FCFLAG_NOTLANDUNG)
+    {
      StickGier = 0;
      StickNick = 0;
     //DebugOut.Analog[16] = Mittelwert_AccHoch;
     //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
     DebugOut.Analog[18] = VarioMeter;
     DebugOut.Analog[19] = WinkelOut.CalcState;
     DebugOut.Analog[20] = ServoNickValue;
+    DebugOut.Analog[22] = Capacity.ActualCurrent;
+    DebugOut.Analog[23] = Capacity.UsedCapacity;
 //    DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ;
 //    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;
     //DebugOut.Analog[28] = I2CError;
-//    DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
+    DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
     DebugOut.Analog[30] = GPS_Nick;
     DebugOut.Analog[31] = GPS_Roll;
+  }
                 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
                 CosAttitude = c_cos_8192(CosAttitude);  // cos of actual attitude
-                if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG))
+                if(HoehenReglerAktiv && !(FCFlags & FCFLAG_NOTLANDUNG))
+                {
                         #define HEIGHT_TRIM_UP          0x01
                         #define HEIGHT_TRIM_DOWN        0x02
                         static unsigned char HeightTrimmingFlag = 0x00;
+                  {
                 // alternative height control
                 // PD-Control with respect to hoover point
                 // the thrust loss out of horizontal attitude is compensated
                 // the setpoint will be fine adjusted with the gas stick position
-                        if(MikroKopterFlags & FLAG_FLY) // trim setpoint only when flying
+                        if(FCFlags & FCFLAG_FLY) // trim setpoint only when flying
                         {   // gas stick is above hoover point
                                 if(StickGas > (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit)
+                                {
                                         if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN)
+                                        {
                            if(StickGasHoover < 70) StickGasHoover = 70;
                            else if(StickGasHoover > 150) StickGasHoover = 150;
+                       }
+                                }
               if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active
-                        } //if MikroKopterFlags & MKFLAG_FLY
+                        } //if FCFlags & MKFCFLAG_FLY
                         else
+                        {
                          SollHoehe = HoehenWert - 400;
                          if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint;
                          else StickGasHoover = 120;
                         FilterHCGas = GasMischanteil;
+                }
                 // Hoover gas estimation by averaging gas control output on small z-velocities
                 // this is done only if height contol option is selected in global config and aircraft is flying
-                if((MikroKopterFlags & FLAG_FLY) && !(MikroKopterFlags & FLAG_NOTLANDUNG))
+                if((FCFlags & FCFLAG_FLY) && !(FCFlags & FCFLAG_NOTLANDUNG))
+                {
                         if(HooverGasFilter == 0)  HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
                         if(abs(VarioMeter) < 100) // only on small vertical speed
+                        {
     tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L;
     tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;
     tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]);  // Filter
         tmp_int = tmp_motorwert[i] / STICK_GAIN;
         CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS);
-    Motor[i] = tmp_int;
+    Motor[i].SetPoint = tmp_int;
+   }
-   else Motor[i] = 0;
+   else Motor[i].SetPoint = 0;
+ }
-/*
-if(Poti1 > 20)  Motor1 = 0;
-if(Poti1 > 90)  Motor6 = 0;
-if(Poti1 > 140) Motor2 = 0;
-//if(Poti1 > 200) Motor7 = 0;
-*/
+}

Subversion Repositories FlightCtrl

(root)/branches/V0.78g_WKN-hef/fc.c @ 1552 - Rev 1378 → 1550