WebSVN - FlightCtrl - Diff - Rev 972 and 973 - /branches/KalmanFilter MikeW/FlightControl.c

   VARIABLES
 *****************************************************************************/
 extern void GPSupdate(void);
-int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0;
+f32_t AdNeutralNick = 0.0F, AdNeutralRoll = 0.0F, AdNeutralGier = 0.0F;
 int NeutralAccX = 0, NeutralAccY = 0, NeutralAccZ = 0;
-int AverageRoll_X = 0, AverageRoll_Y = 0, AverageRoll_Z = 0;
-int AveragerACC_X = 0, AveragerACC_Y = 0, AveragerACC_Z = 0;
+int AverageRoll = 0, AverageNick = 0, AverageGier = 0;
-int Roll_X_Off = 0, Roll_Y_Off = 0, Roll_Z_Off = 0;
+int AveragerACC_X = 0, AveragerACC_Y = 0, AveragerACC_Z = 0;
   LastAltitude = CurrentAltitude;
   Delay_ms_Mess(300);
   ANALOG_OFF;
-  AdNeutralNick = AdWertNick_Raw;
+  AdNeutralNick = (f32_t) AdWertNick_Raw;
-  AdNeutralRoll = AdWertRoll_Raw;
+  AdNeutralRoll = (f32_t) AdWertRoll_Raw;
-  AdNeutralGier = AdWertGier_Raw;
+  AdNeutralGier = (f32_t) AdWertGier_Raw;
   NeutralAccY = AdWertAccRoll_Raw;
   NeutralAccX = AdWertAccNick_Raw;
   AccumulatedACC_Y = 0;
   AccumulatedACC_Y_cnt = 0;
   AccumulatedACC_Z = 0;
   AccumulatedACC_Z_cnt = 0;
-  AccumulatedRoll_X = 0;
+  AccumulatedRoll = 0;
-  AccumulatedRoll_X_cnt = 0;
+  AccumulatedRoll_cnt = 0;
-  AccumulatedRoll_Y = 0;
+  AccumulatedNick = 0;
-  AccumulatedRoll_Y_cnt = 0;
+  AccumulatedNick_cnt = 0;
-  AccumulatedRoll_Z = 0;
+  AccumulatedGier = 0;
-  AccumulatedRoll_Z_cnt = 0;
+  AccumulatedGier_cnt = 0;
   AveragerACC_X = 0;
   @author         Michael Walter
 **************************************************************************** */
 void GetMeasurements(void)
+{
   ANALOG_OFF;
-  AverageRoll_X = AccumulatedRoll_X / AccumulatedRoll_X_cnt;
+  AverageRoll = AccumulatedRoll / AccumulatedRoll_cnt;
-  AverageRoll_Y = AccumulatedRoll_Y / AccumulatedRoll_Y_cnt;
+  AverageNick = AccumulatedNick / AccumulatedNick_cnt;
-  AverageRoll_Z = AccumulatedRoll_Z / AccumulatedRoll_Z_cnt;
+  AverageGier = AccumulatedGier / AccumulatedGier_cnt;
   /* Get Pressure Differenz */
   CurrentAltitude = InitialAltitude - AccumulatedAirPressure / AccumulatedAirPressure_cnt;
   AccumulatedAirPressure_cnt = 0;
+  }
   AirPressureCnt++;
   //if ((GPS_Roll == 0 && GPS_Nick == 0) || (maxDistance / 10 > 10))
   {
-    Roll_X_Offset = 0.9995F * Roll_X_Offset + 0.0005F * (float) (MAX(-60, MIN(60,AverageRoll_X)));
+    AdNeutralNick = 0.999F * AdNeutralNick + 0.001F * AdWertNick_Raw;
-    Roll_Y_Offset = 0.9995F * Roll_Y_Offset + 0.0005F * (float) (MAX(-60, MIN(60,AverageRoll_Y)));
     AdNeutralRoll = 0.999F * AdNeutralRoll + 0.001F * AdWertRoll_Raw;
     if (abs(StickGier) < 15 || MotorenEin == 0)
     {
-      Roll_Z_Offset = 0.9998F * Roll_Z_Offset + 0.0002F * (float) ( MAX(-60, MIN(60,AverageRoll_Z)));
+       AdNeutralGier = 0.999F * AdNeutralGier + 0.001F * AdWertGier_Raw;
+    }
+  }
 #if 1
-  DebugOut.Analog[7] = AdWertNick_Raw;
+  DebugOut.Analog[6] = AdWertNick_Raw;
+  DebugOut.Analog[7] = AdWertRoll_Raw;
+  DebugOut.Analog[8] = AdWertGier_Raw;
+  DebugOut.Analog[9] = AdNeutralNick;
-  DebugOut.Analog[8] = AdWertRoll_Raw;
+  DebugOut.Analog[10] = AdNeutralRoll;
-  DebugOut.Analog[9] = AdWertGier_Raw;
-#endif
-  AverageRoll_X -= Roll_X_Offset;
-  AverageRoll_Y -= Roll_Y_Offset;
+  DebugOut.Analog[11] = AdNeutralGier;
-  AverageRoll_Z -= Roll_Z_Offset;
+#endif
-  AccumulatedRoll_X = 0;
+  AccumulatedRoll = 0;
-  AccumulatedRoll_X_cnt = 0;
+  AccumulatedRoll_cnt = 0;
-  AccumulatedRoll_Y = 0;
+  AccumulatedNick = 0;
-  AccumulatedRoll_Y_cnt = 0;
+  AccumulatedNick_cnt = 0;
-  AccumulatedRoll_Z = 0;
+  AccumulatedGier = 0;
-  AccumulatedRoll_Z_cnt = 0;
+  AccumulatedGier_cnt = 0;
   AccumulatedACC_Y_cnt = 0;
   AccumulatedACC_Z = 0;
   AccumulatedACC_Z_cnt = 0;
-  ANALOG_ON;
-  if (Roll_X_Off > 60)
-  {
-    Roll_X_Off = 60;
-  }
-  if (Roll_X_Off < -60)
-  {
-    Roll_X_Off = -60;
-  }
-  if (Roll_Y_Off > 60)
-  {
-    Roll_Y_Off = 60;
-  }
-  if (Roll_Y_Off < -60)
-  {
-    Roll_Y_Off = -60;
-  }
-  if (Roll_Z_Off > 60)
-  {
-    Roll_Z_Off = 60;
-  }
-  if (Roll_Z_Off < -60)
-  {
-    Roll_Z_Off = -60;
   ANALOG_ON;
+}
 /* ****************************************************************************
   if (abs(StickGier) > 4)
+  {
     sollGier = status.iPsi10 + 4 * StickGier;
+  }
   DiffGier = (sollGier - status.iPsi10);
-  GierMischanteil = (int) (-4 *  DiffGier - 4* (AdWertGier - AdNeutralGier - Roll_Z_Off)) / 10;
+  GierMischanteil = (int) (-4 *  DiffGier - 4* (AdWertGier_Raw - (int) AdNeutralGier)) / 10;
 #define MUL_G  0.8
   if(GierMischanteil > MUL_G * GasMischanteil)
+  {
   if(SummeRoll < -10000)
+  {
     SummeRoll = -10000;
+  }
-  pd_ergebnis = ((scale_p *DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10) ; // + (int)(SummeNick / 2000);
+  pd_ergebnis = ((scale_p *DiffNick + scale_d * (AdWertNick_Raw - (int) AdNeutralNick)) / 10) ; // + (int)(SummeNick / 2000);
   if(pd_ergebnis >  (GasMischanteil + abs(GierMischanteil)))
+  {
     pd_ergebnis =  (GasMischanteil + abs(GierMischanteil));
+  }
+  {
     motorwert = MIN_GAS;
+  }
   Motor_Hinten = motorwert;
-  pd_ergebnis =  ((scale_p * DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10) ; //+ (int)(SummeRoll / 2000);
+  pd_ergebnis =  ((scale_p * DiffRoll + scale_d * (AdWertRoll_Raw - (int) AdNeutralRoll)) / 10) ; //+ (int)(SummeRoll / 2000);
   if(pd_ergebnis >  (GasMischanteil + abs(GierMischanteil)))
+  {
     pd_ergebnis =  (GasMischanteil + abs(GierMischanteil));
+  }

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/FlightControl.c - Rev 972 → 973