WebSVN - FlightCtrl - Diff - Rev 1638 and 1639 - /branches/V0.88n_ACC-HH_HR

 int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
 int TrimNick, TrimRoll;
 int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
 int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
 int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
-volatile float NeutralAccZ = 0;
+int NeutralAccZ = 0;
 unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
 long IntegralNick = 0,IntegralNick2 = 0;
 long IntegralRoll = 0,IntegralRoll2 = 0;
 long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
 long Integral_Gier = 0;
  else   motor = neu - (alt - neu)*1;
 //if(Poti2 < 20)  return(neu);
  return(motor);
+}
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+//  Debugwerte zuordnen
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+void CopyDebugValues(void)
+{
+    DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4);
+    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] = AdWertAccHoch;// 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] = NeutralAccZ;
+DebugOut.Analog[18] = TransmitBlConfig;
+        //DebugOut.Analog[16] = Motor[0].Temperature;
+    //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[30] = GPS_Nick;
+    DebugOut.Analog[31] = GPS_Roll;
+}
 void Piep(unsigned char Anzahl, unsigned int dauer)
+{
  if(MotorenEin) return; //auf keinen Fall im Flug!
  while(Anzahl--)
     Parameter_AchsKopplung1 = 0;
     Parameter_AchsKopplung2 = 0;
     ExpandBaro = 0;
+    TransmitBlConfig = 1;
         motorread = 0;
     CalibrierMittelwert();
     CalibrierMittelwert();
             NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);
             NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
             NeutralAccZ = Aktuell_az;
                 // Save ACC neutral settings to eeprom
-                SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccY);
+                SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX);
-                SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccX);
+                SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY);
                 SetParamWord(PID_ACC_TOP,  (uint16_t)NeutralAccZ);
+    }
     else
+    {
     DebugOut.Analog[15] = Motor[3].SetPoint;
     //Start I2C Interrupt Mode
     twi_state = 0;
     motor = 0;
-    i2c_start();
+    I2C_Start();
+}
     MittelIntegralNick2 = 0;
     MittelIntegralRoll2 = 0;
     ZaehlMessungen = 0;
  } //  ZaehlMessungen >= ABGLEICH_ANZAHL
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Gieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //    if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
+          }
           if(KompassSignalSchlecht) KompassSignalSchlecht--;
+        }
         else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
+     }
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-//  Debugwerte zuordnen
-// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  if(!TimerWerteausgabe--)
-   {
-    TimerWerteausgabe = 24;
-    DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4);
-    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[8] = KompassValue;
-    DebugOut.Analog[9] = UBat;
-    DebugOut.Analog[10] = SenderOkay;
-    DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
-//DebugOut.Analog[16] = Motor[0].Temperature;
-    //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[30] = GPS_Nick;
-    DebugOut.Analog[31] = GPS_Roll;
-  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Höhenregelung
 // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen
   GasMischanteil *= STICK_GAIN;
         // if height control is activated
  if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick))  // Höhenregelung
+        {
                 #define HOVER_GAS_AVERAGE 4096L         // 4096 * 2ms = 8.2s averaging
                 #define HC_GAS_AVERAGE 4                        // 4 * 2ms= 8ms averaging

Subversion Repositories FlightCtrl

(root)/branches/V0.88n_ACC-HH_HR_MartinR/fc.c @ 2177 - Rev 1638 → 1639