WebSVN - FlightCtrl - Diff - Rev 1880 and 1897 - /branches/V0.84a_MartinW

 #include "main.h"
 #include "mymath.h"
+#include "isqrt.h"
+//MartinW; added vars
+unsigned char loop1, loop2, loop3;
+unsigned char settingdest = 5;
+int keynumber=-7;
+unsigned short CurrentOffset = 0;///
+unsigned char pos1, pos2, pos3, pos4=0;
+unsigned char Motors0,Motors1,Motors2,Motors3,Motors4,Motors5,Motors6,Motors7;
+unsigned char Motors0max,Motors1max,Motors2max,Motors3max,Motors4max,Motors5max,Motors6max,Motors7max;
+unsigned short MotorsTmax;
+unsigned char updatemotors=5;
+//Panorama Trigger;
+int degreeold =0;
+int degreedivold =0;
+int degreediv =0;
+unsigned int panograd=0;
+unsigned char panotrigger=0;
+ unsigned char calibration_done = 0;
+///MartinW; added vars END
 #include "isqrt.h"
 unsigned char h,m,s;
 unsigned int BaroExpandActive = 0;
 int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
 long IntegralRoll = 0,IntegralRoll2 = 0;
 long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
 long Integral_Gier = 0;
 long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
 long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
-long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
+//long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; ///MartinW so war es
+long Mess_Integral_Gier = 0; ///MartinW: Mess_Integral_Gier2 unbenutzt
 long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
 long SummeNick=0,SummeRoll=0;
 volatile long Mess_Integral_Hoch = 0;
 int  KompassValue = -1;
 int  KompassSollWert = 0;
     DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
     DebugOut.Analog[12] = Motor[0].SetPoint;
     DebugOut.Analog[13] = Motor[1].SetPoint;
     DebugOut.Analog[14] = Motor[2].SetPoint;
     DebugOut.Analog[15] = Motor[3].SetPoint;
+///MartinW added Debug ouputs
+        DebugOut.Analog[16] = Motor[4].SetPoint;
+    DebugOut.Analog[17] = Motor[5].SetPoint;
+        DebugOut.Analog[18] = FromNC_AltitudeSpeed;     ///
+        //DebugOut.Analog[18] = Motor[6].SetPoint;      // v0.84a
+        //DebugOut.Analog[24] = Motor[6].SetPoint;      // on v0.84a = SollHoehe/5
+    DebugOut.Analog[25] = Motor[6].SetPoint;
+        DebugOut.Analog[26] = Motor[7].SetPoint;
+        //DebugOut.Analog[27] = Motor[9].SetPoint;      // on v0.84a = KompassSollWert
+///MartinW added Debug ouputs END
     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;
     DebugOut.Analog[31] = GPS_Roll;
     if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe;
+}
      static long IntegralFehlerNick = 0;
      static long IntegralFehlerRoll = 0;
          static unsigned int RcLostTimer;
          static unsigned char delay_neutral = 0;
          static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
-         static unsigned char calibration_done = 0;
+         //static unsigned char calibration_done = 0;// moved up
      static char NeueKompassRichtungMerken = 0;
      static long ausgleichNick, ausgleichRoll;
      int IntegralNickMalFaktor,IntegralRollMalFaktor;
          unsigned char i;
         Mittelwert();
                            SetNeutral(0);
                            calibration_done = 1;
                                                    ServoActive = 1;
                                                    DDRD  |=0x80; // enable J7 -> Servo signal
                            Piep(GetActiveParamSet(),120);
+#ifdef WITH_JETI_BEEP           ///  MartinW main.h
+#warning : "### with JetiBeep Delay ###"
+                                                        JetiBeep = 101;///
+                                                        delayjetibeepset = (GetActiveParamSet()*10);
+#else
+#warning : "### without JetiBeep Delay ###"
+#endif
+                         }
+                        }
+                    }
                  else
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)  // ACC Neutralwerte speichern
+                                                                {
                                                                         modell_fliegt = 1;
                                                                         MotorenEin = 1;
                                                                         sollGier = 0;
                                                                         Mess_Integral_Gier = 0;
-                                                                        Mess_Integral_Gier2 = 0;
+                                                                        //Mess_Integral_Gier2 = 0; //MartinR: Mess_Integral_Gier2 unbenutzt
                                                                         Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
                                                                         Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
                                                                         Mess_IntegralNick2 = IntegralNick;
                                                                         Mess_IntegralRoll2 = IntegralRoll;
                                                                         SummeNick = 0;
     IntegralFaktorGier = Parameter_Gyro_Gier_I;
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //+ Analoge Steuerung per Seriell
+//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+#ifdef WITH_ExternControl               /// MartinW memorysaving
+#warning : "### with ExternControl ###"
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    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;
       MaxStickRoll = abs(StickRoll)/STICK_GAIN;
       if(MaxStickRoll > 100) MaxStickRoll = 100;
+     }
      else MaxStickRoll--;
     if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)  {MaxStickNick = 0; MaxStickRoll = 0;}
+        #else
+#warning : "### without ExternControl ###"
+#endif
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Looping?
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                         else tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L;
             // Gier
                         if(Mixer.Motor[i][3] == 64) tmp_int += GierMischanteil;
                         else if(Mixer.Motor[i][3] == -64) tmp_int -= GierMischanteil;
                         else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;
+#ifdef WITH_ORIGINAL_MOTORSMOOTHING     ///  MartinW main.h means no memsave
+#warning : "### with  WITH_ORIGINAL_MOTORSMOOTHING ###"
-                        if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2;      // MotorSmoothing
+                if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2;      // MotorSmoothing
-                        else tmp_int = 2 * tmp_int - tmp_motorwert[i];                                                              // MotorSmoothing
+                else tmp_int = 2 * tmp_int - tmp_motorwert[i];                                                              // MotorSmoothing
+#else
+#warning : "### without  WITH_ORIGINAL_MOTORSMOOTHING, ajustable ###"
+// MartinW; variable MS START
+if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2;      // MotorSmoothing
+//                      else tmp_int = 2 * tmp_int - tmp_motorwert[i];                                          // MotorSmoothing
+// Arthur P: the original code allowed the motor value to drop to 0 or negative values
+// straight off, i.e. could amplify an intended decrease excessively while upregulation
+// is dampened. The modification would still allow immediate drop below intended value
+// but would dampen this. This would still allow for airbraking of the prop to have effect
+// but it might lead to less sudden excessive drops in rpm with only gradual recovery.
+// 090807 Arthur P: Due to problems with uart.c which still refers to user parameter 1 and 2 and
+// possible timing issues with the shutter interval load, removed the shutter interval functions
+// and switched to use of userparam6 for the motor smoothing.
+// 091114 Inserted modification into 0.76g source code.
+// 20100804 Modified v.0.80d code where motorsmoothing is no longer a separate function.
+// Downsmoothing either uses default v.0.7x+ 150% downstep (user para 7 == 0),
+// 50% downstep (user para 7 == 1 or 2), or downsteps of x% (userpara7 ==):
+// 66.6% (3), 75% (4), 80% (5), 90% (10), 95% (20), 97.5% (40), 98% (50), 99% (100).
+                        else
+                        {
+                                if(Parameter_UserParam7 < 2)
+                                { // Original function
+                                        tmp_int = 2 * tmp_int - tmp_motorwert[i];
+                                }
+                                else
+                                {
+                                        // If userpara7 >= 2 then allow >= 50% of the intended step down to rapidly reach the intended value.
+                                        tmp_int = tmp_int + ((tmp_motorwert[i] - tmp_int)/Parameter_UserParam7);
+                                }
+                        }
+// MartinW; variable MS END
+#endif
                         LIMIT_MIN_MAX(tmp_int,(int) MIN_GAS * 4,(int) MAX_GAS * 4);
                         Motor[i].SetPoint = tmp_int / 4;
                         Motor[i].SetPointLowerBits = (tmp_int % 4)<<1; // (3 bits total)
             tmp_motorwert[i] = tmp_int;
+                }

Subversion Repositories FlightCtrl

(root)/branches/V0.84a_MartinW_Jeti+/fc.c - Rev 1880 → 1897