WebSVN - FlightCtrl - Diff - Rev 976 and 977 - /branches/V0.70d HexaLotte/fc.c

 int   GierGyroFehler = 0;
 float GyroFaktor;
 float IntegralFaktor;
 volatile int  DiffNick,DiffRoll;
 int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
+#ifdef HEXAKOPTER
+volatile unsigned char Motor_VorneLinks,Motor_VorneRechts,Motor_HintenLinks,Motor_HintenRechts,Motor_Rechts,Motor_Links, Count;
+#else
 volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
+#endif
 volatile unsigned char SenderOkay = 0;
 int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
 char MotorenEin = 0;
 int HoehenWert = 0;
 int SollHoehe = 0;
     MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
     MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
     MesswertNick = (signed int) AdWertNick - AdNeutralNick;
 //DebugOut.Analog[26] = MesswertNick;
-DebugOut.Analog[28] = MesswertRoll;
+//DebugOut.Analog[28] = MesswertRoll;
 // Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
         Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
 void SendMotorData(void)
 //############################################################################
+{
     if(!MotorenEin)
+        {
+#ifdef HEXAKOPTER
+        Motor_HintenLinks = 0;
+        Motor_HintenRechts = 0;
+        Motor_VorneLinks = 0;
+        Motor_VorneRechts = 0;
+        Motor_Rechts = 0;
+        Motor_Links = 0;
+        if(MotorTest[0]) Motor_VorneLinks = Motor_VorneRechts = MotorTest[0];
+        if(MotorTest[1]) Motor_HintenLinks = Motor_HintenRechts = MotorTest[1];
+        if(MotorTest[2]) Motor_Links = MotorTest[2];
+        if(MotorTest[3]) Motor_Rechts = MotorTest[3];
+#else
         Motor_Hinten = 0;
         Motor_Vorne = 0;
         Motor_Rechts = 0;
         Motor_Links = 0;
         if(MotorTest[0]) Motor_Vorne = MotorTest[0];
         if(MotorTest[1]) Motor_Hinten = MotorTest[1];
         if(MotorTest[2]) Motor_Links = MotorTest[2];
         if(MotorTest[3]) Motor_Rechts = MotorTest[3];
+#endif
         MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY);
         } else MikroKopterFlags |= FLAG_MOTOR_RUN;
+#ifdef HEXAKOPTER
+    DebugOut.Analog[10] = Motor_VorneLinks;
+    DebugOut.Analog[11] = Motor_VorneRechts;
+    DebugOut.Analog[12] = Motor_HintenLinks;
+    DebugOut.Analog[13] = Motor_HintenRechts;
+    DebugOut.Analog[14] = Motor_Links;
+    DebugOut.Analog[15] = Motor_Rechts;
 #else
     DebugOut.Analog[12] = Motor_Vorne;
     DebugOut.Analog[13] = Motor_Hinten;
     DebugOut.Analog[14] = Motor_Links;
     DebugOut.Analog[15] = Motor_Rechts;
 #endif
     //Start I2C Interrupt Mode
     twi_state = 0;
     motor = 0;
     i2c_start();
 void MotorRegler(void)
 //############################################################################
+{
          int motorwert,pd_ergebnis,h,tmp_int;
          int GierMischanteil,GasMischanteil;
+         int NickMischanteil, RollMischanteil;
      static long SummeNick=0,SummeRoll=0;
      static long sollGier = 0,tmp_long,tmp_long2;
      static long IntegralFehlerNick = 0;
      static long IntegralFehlerRoll = 0;
          static unsigned int RcLostTimer;
     DebugOut.Analog[4] = MesswertGier;
     DebugOut.Analog[5] = HoehenWert;
     DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
     DebugOut.Analog[8] = KompassValue;
     DebugOut.Analog[9] = UBat;
-    DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
+    DebugOut.Analog[28] = ErsatzKompass / GIER_GRAD_FAKTOR;
-    DebugOut.Analog[10] = SenderOkay;
+    DebugOut.Analog[29] = SenderOkay;
     //DebugOut.Analog[16] = Mittelwert_AccHoch;
 //    DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
 //    DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
     DebugOut.Analog[19] = WinkelOut.CalcState;
     DebugOut.Analog[20] = ServoValue;
     if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung
     else  SummeNick += DiffNick; // I-Anteil bei HH
     if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
     if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
     pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick
-    // Motor Vorn
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
     if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
     if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
-    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
-    motorwert /= STICK_GAIN;
-        if ((motorwert < 0)) motorwert = 0;
-        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
-        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
-        Motor_Vorne = motorwert;
-    // Motor Heck
-        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
-    motorwert /= STICK_GAIN;
-        if ((motorwert < 0)) motorwert = 0;
-        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
-        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+    NickMischanteil = (pd_ergebnis*4) / 7;
         Motor_Hinten = motorwert;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
     if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
     if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
     pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
     tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
     if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
     if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
+    RollMischanteil = pd_ergebnis/3;
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Mischer für die Motorenanteile
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ #ifdef HEXAKOPTER
+    // Motor Vorn Links
+    motorwert = GasMischanteil + NickMischanteil + RollMischanteil/2 - GierMischanteil;   // Mischer
+    motorwert /= STICK_GAIN;
+        if ((motorwert < 0)) motorwert = 0;
+        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+        Motor_VorneLinks = motorwert;
+    // Motor Vorn Rechts
+        motorwert = GasMischanteil + NickMischanteil - RollMischanteil/2 + GierMischanteil;
+    motorwert /= STICK_GAIN;
+        if ((motorwert < 0)) motorwert = 0;
+        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+        Motor_VorneRechts = motorwert;
+            // Motor Hinten Links
+    motorwert = GasMischanteil - NickMischanteil + RollMischanteil/2 - GierMischanteil;
+    motorwert /= STICK_GAIN;
+        if ((motorwert < 0)) motorwert = 0;
+        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+    Motor_HintenLinks = motorwert;
+    // Motor HintenRechts
+        motorwert = GasMischanteil - NickMischanteil - RollMischanteil/2 + GierMischanteil;
+    motorwert /= STICK_GAIN;
+        if ((motorwert < 0)) motorwert = 0;
+        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+    Motor_HintenRechts = motorwert;
+    // Motor Rechts
+        motorwert = GasMischanteil - RollMischanteil - GierMischanteil;
+    motorwert /= STICK_GAIN;
+        if ((motorwert < 0)) motorwert = 0;
+        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+    Motor_Rechts = motorwert;
     // Motor Links
+        motorwert = GasMischanteil + RollMischanteil + GierMischanteil;
+    motorwert /= STICK_GAIN;
+        if ((motorwert < 0)) motorwert = 0;
+        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+    Motor_Links = motorwert;
+#else
+    // Motor Vorn
+    motorwert = GasMischanteil + NickMischanteil + GierMischanteil;       // Mischer
+    motorwert /= STICK_GAIN;
+        if ((motorwert < 0)) motorwert = 0;
+        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+        Motor_Vorne = motorwert;
+    // Motor Heck
+        motorwert = GasMischanteil - NickMischanteil + GierMischanteil;
+    motorwert /= STICK_GAIN;
+        if ((motorwert < 0)) motorwert = 0;
+        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
+        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
+        Motor_Hinten = motorwert;
+            // Motor Links
-    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
+    motorwert = GasMischanteil + RollMischanteil - GierMischanteil;
     motorwert /= STICK_GAIN;
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
     Motor_Links = motorwert;
     // Motor Rechts
-        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
+        motorwert = GasMischanteil - RollMischanteil - GierMischanteil;
     motorwert /= STICK_GAIN;
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
     Motor_Rechts = motorwert;
-   // +++++++++++++++++++++++++++++++++++++++++++++++
+#endif
+}

Subversion Repositories FlightCtrl

(root)/branches/V0.70d HexaLotte/fc.c @ 1221 - Rev 976 → 977