WebSVN - FlightCtrl - Diff - Rev 1400 and 1452

 long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
 long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
 volatile long Mess_Integral_Hoch = 0;
 int  KompassValue = 0;
 int  KompassStartwert = 0;
-int  KompassEinschaltStartwert = 0;
 int  KompassRichtung = 0;
 unsigned int  KompassSignalSchlecht = 500;
 unsigned char  MAX_GAS,MIN_GAS;
 unsigned char HoehenReglerAktiv = 0;
 unsigned char TrichterFlug = 0;
 signed int tmp_motorwert[MAX_MOTORS];
 unsigned char LoadHandler = 0;
 #define LIMIT_MIN(value, min) {if(value < min) value = min;}
 #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;}
+uint8_t servoValues[6] = { 127, 127, 127, 127, 127 , 127 };
+uint8_t MakeByte(int16_t i) {
+        if (i < 0) return 0;
+        if (i > 255) return 255;
+        return i;
+}
 int MotorSmoothing(int neu, int alt)
+{
  int motor;
     Mess_Integral_Gier = 0;
     StartLuftdruck = Luftdruck;
     VarioMeter = 0;
     Mess_Integral_Hoch = 0;
     KompassStartwert = KompassValue;
-        KompassEinschaltStartwert = KompassValue;
     GPS_Neutral();
     beeptime = 50;
         Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
         Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
     ExternHoehenValue = 0;
 void SendMotorData(void)
 //############################################################################
+{
  unsigned char i;
     if(!MotorenEin)
-        {
          MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY);
-                 for(i=0;i<MAX_MOTORS;i++)
-                  {
-                   if(!PC_MotortestActive)  MotorTest[i] = 0;
-                   Motor[i] = MotorTest[i];
-                  }
+        else
-          if(PC_MotortestActive) PC_MotortestActive--;
-        }
-        else MikroKopterFlags |= FLAG_MOTOR_RUN;
+                MikroKopterFlags |= FLAG_MOTOR_RUN;
-    DebugOut.Analog[12] = Motor[0];
-    DebugOut.Analog[13] = Motor[1];
-    DebugOut.Analog[14] = Motor[3];
     DebugOut.Analog[15] = Motor[2];
     //Start I2C Interrupt Mode
     twi_state = 0;
  Ki = 10300 / (Parameter_I_Faktor + 1);
  MAX_GAS = EE_Parameter.Gas_Max;
  MIN_GAS = EE_Parameter.Gas_Min;
+}
 //############################################################################
 //
 void MotorRegler(void)
 //############################################################################
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Emfang gut
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         if(SenderOkay > 140)
+            {
                         MikroKopterFlags &= ~FLAG_NOTLANDUNG;
             RcLostTimer = EE_Parameter.NotGasZeit * 50;
             if(GasMischanteil > 40 && MotorenEin)
+                {
                 if(modell_fliegt < 0xffff) modell_fliegt++;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Einschalten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                     if(++delay_einschalten > 200)
+                        {
+                        beeptime = 1000;
                         delay_einschalten = 200;
                         modell_fliegt = 1;
                         MotorenEin = 1;
                         sollGier = 0;
                         Mess_Integral_Gier = 0;
                         Mess_IntegralNick2 = IntegralNick;
                         Mess_IntegralRoll2 = IntegralRoll;
                         SummeNick = 0;
                         SummeRoll = 0;
                         MikroKopterFlags |= FLAG_START;
-                        KompassEinschaltStartwert = KompassValue;
+                        }
+                    }
                     else delay_einschalten = 0;
                 //Auf Neutralwerte setzen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
+                    {
                     if(++delay_ausschalten > 200)  // nicht sofort
+                        {
+                          beeptime = 1000;
                          MotorenEin = 0;
                          delay_ausschalten = 200;
                          modell_fliegt = 0;
+                        }
+                    }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(!NewPpmData-- || (MikroKopterFlags & FLAG_NOTLANDUNG))
+  {
-        static int stick_nick,stick_roll;
-        int e_stick_nick, e_stick_nick_diff, e_stick_roll, e_stick_roll_diff;
-        int stick_p, stick_d;
         static int stick_nick,stick_roll;
-    ParameterZuordnung();
-        stick_p = EE_Parameter.Stick_P;
-        stick_d = EE_Parameter.Stick_D;
-        if (Parameter_UserParam8) {
-                int angle, stick_nick_tmp, stick_roll_tmp;
-                if (Parameter_UserParam8 == 254) {
-                        angle = -45;
-                } else if (Parameter_UserParam8 > 128) {
-                        angle = KompassValue;
-                } else {
-                        angle = (ErsatzKompass / GIER_GRAD_FAKTOR);
-                }
-                angle = ((KompassEinschaltStartwert - angle + 360) % 360);
+    ParameterZuordnung();
-                stick_nick_tmp = (long)c_cos_8192(angle) * (long)PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] / 8192L;
-                stick_roll_tmp = (long)c_sin_8192(angle) * (long)PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] / 8192L;
-                e_stick_nick = stick_roll_tmp + stick_nick_tmp;
-                e_stick_roll = stick_roll_tmp - stick_nick_tmp;
-                stick_nick_tmp = (long)c_cos_8192(angle) * (long)PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] / 8192L;
-                stick_roll_tmp = (long)c_sin_8192(angle) * (long)PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] / 8192L;
-                e_stick_nick_diff = stick_roll_tmp + stick_nick_tmp;
-                e_stick_roll_diff = stick_roll_tmp - stick_nick_tmp;
-                if (Parameter_UserParam8 == 255) {
-                        int gier = abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]);
-                        if (gier > 100) {
-                                stick_d /= 3;
-                        } else if (gier > 50) {
-                                stick_d /= 2;
-                        }
-                        if (stick_d < 1) {
-                                stick_d = 1;
-                        }
-                }
-/*
-DebugOut.Analog[12] = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];
-DebugOut.Analog[13] = e_stick_nick;
-DebugOut.Analog[14] = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];
-DebugOut.Analog[15] = e_stick_roll;
-*/
-        } else {
+    stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
-                e_stick_nick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];
-                e_stick_roll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];
-                e_stick_nick_diff = PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]];
-                e_stick_roll_diff = PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]];
-        }
-    stick_nick = (stick_nick * 3 + e_stick_nick * stick_p) / 4;
-    stick_nick += e_stick_nick_diff * stick_d;
+    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 + e_stick_roll * stick_p) / 4;
+    stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
-    stick_roll += e_stick_roll_diff * stick_d;
+    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
     StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
     DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4);
     DebugOut.Analog[2] = Mittelwert_AccNick / 4;
     DebugOut.Analog[3] = Mittelwert_AccRoll / 4;
     DebugOut.Analog[4] = MesswertGier;
     DebugOut.Analog[5] = HoehenWert/5;
-    DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az;
+    //DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az;
     DebugOut.Analog[8] = KompassValue;
     DebugOut.Analog[9] = UBat;
     DebugOut.Analog[10] = SenderOkay;
     DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
     //DebugOut.Analog[16] = Mittelwert_AccHoch;
   if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0)
+   {
     modell_fliegt = 1;
         GasMischanteil = MIN_GAS;
+   }
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+/*// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mischer und PI-Regler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   DebugOut.Analog[7] = GasMischanteil;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gier-Anteil
     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
     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;*/
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// Universal Mixer
+// Servo Mixer
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+        {
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+                int16_t nick, roll, tmp, pitch, throttle;
- for(i=0; i<MAX_MOTORS; i++)
+                int16_t servo[6];
- {
+        // throttle
+                if (MotorenEin == 0 || MikroKopterFlags & FLAG_NOTLANDUNG) {
+                        tmp = 0;
+                } else {
+                throttle = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + Mixer.Motor[2][0];
-  signed int tmp_int;
+                if (throttle < 0) throttle = 0;
+                tmp = throttle / Mixer.Motor[3][0];
-  if(Mixer.Motor[i][0] > 0)
+                if (tmp > Mixer.Motor[3][1] * 2) tmp = Mixer.Motor[3][1] * 2;
+            }
+        servo[4] = tmp;
-   {
+                // pitch
-    tmp_int =  ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L;
+        pitch = throttle / Mixer.Motor[4][0] + Mixer.Motor[4][2];
+        if (pitch > Mixer.Motor[4][1]) pitch = Mixer.Motor[4][1];
+                servo[0] = -pitch;
+                servo[1] = pitch;
+                servo[2] = pitch;
+        nick = IntegralNick / (EE_Parameter.GyroAccFaktor * 4);
+        roll = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4);
+                // nick
-    tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L;
+        nick /= Mixer.Motor[0][1];
-    tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L;
+        nick -= PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] / Mixer.Motor[5][0];
-    tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;
+        servo[0] += 127 + Mixer.Motor[0][0] - nick;
-    tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]);  // Filter
+        servo[1] += 127 + Mixer.Motor[0][0] + nick;
+        servo[2] += 127 + Mixer.Motor[0][0] - nick;
-        tmp_int = tmp_motorwert[i] / STICK_GAIN;
+                // roll
+        roll /= Mixer.Motor[0][2];
+        roll -= PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] / Mixer.Motor[6][0];
-        CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS);
+        servo[0] += roll;
+        servo[1] += roll;
-    Motor[i] = tmp_int;
+        // gier
+        tmp = MesswertGier / Mixer.Motor[0][3];
+        servo[3] = 127 + Mixer.Motor[1][1] + tmp + (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] / Mixer.Motor[7][0]);
-   }
+        // not used
+        servo[5] = 127;
+        for(tmp = 0; tmp < 6; tmp++) {
-   else Motor[i] = 0;
+                servoValues[tmp] = MakeByte(servo[tmp]);
- }
+        }
-/*
-if(Poti1 > 20)  Motor1 = 0;
+DebugOut.Analog[12] = servo[0];
-if(Poti1 > 90)  Motor6 = 0;
+DebugOut.Analog[13] = servo[1];
+DebugOut.Analog[14] = servo[2];
+DebugOut.Analog[15] = servo[3];
-if(Poti1 > 140) Motor2 = 0;
+DebugOut.Analog[6] = pitch;
+DebugOut.Analog[7] = servo[4];
-//if(Poti1 > 200) Motor7 = 0;
+        }
 */

Subversion Repositories FlightCtrl

(root)/branches/V0.76g-acid/fc.c - Rev 1400 → 1452