Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 972 → Rev 973

/branches/KalmanFilter MikeW/FlightControl.c
38,11 → 38,10
 
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 AverageRoll = 0, AverageNick = 0, AverageGier = 0;
int AveragerACC_X = 0, AveragerACC_Y = 0, AveragerACC_Z = 0;
int Roll_X_Off = 0, Roll_Y_Off = 0, Roll_Z_Off = 0;
 
f32_t Roll_X_Offset = 0.F, Roll_Y_Offset = 0.F, Roll_Z_Offset = 0.F;
f32_t ACC_X_Offset = 0.F, ACC_Y_Offset = 0.F, ACC_Z_Offset = 0.F;
89,9 → 88,9
Delay_ms_Mess(300);
 
ANALOG_OFF;
AdNeutralNick = AdWertNick_Raw;
AdNeutralRoll = AdWertRoll_Raw;
AdNeutralGier = AdWertGier_Raw;
AdNeutralNick = (f32_t) AdWertNick_Raw;
AdNeutralRoll = (f32_t) AdWertRoll_Raw;
AdNeutralGier = (f32_t) AdWertGier_Raw;
NeutralAccY = AdWertAccRoll_Raw;
NeutralAccX = AdWertAccNick_Raw;
NeutralAccZ = AdWertAccHoch_Raw;
110,12 → 109,12
AccumulatedACC_Z = 0;
AccumulatedACC_Z_cnt = 0;
AccumulatedRoll_X = 0;
AccumulatedRoll_X_cnt = 0;
AccumulatedRoll_Y = 0;
AccumulatedRoll_Y_cnt = 0;
AccumulatedRoll_Z = 0;
AccumulatedRoll_Z_cnt = 0;
AccumulatedRoll = 0;
AccumulatedRoll_cnt = 0;
AccumulatedNick = 0;
AccumulatedNick_cnt = 0;
AccumulatedGier = 0;
AccumulatedGier_cnt = 0;
AveragerACC_X = 0;
AveragerACC_Y = 0;
152,9 → 151,9
void GetMeasurements(void)
{
ANALOG_OFF;
AverageRoll_X = AccumulatedRoll_X / AccumulatedRoll_X_cnt;
AverageRoll_Y = AccumulatedRoll_Y / AccumulatedRoll_Y_cnt;
AverageRoll_Z = AccumulatedRoll_Z / AccumulatedRoll_Z_cnt;
AverageRoll = AccumulatedRoll / AccumulatedRoll_cnt;
AverageNick = AccumulatedNick / AccumulatedNick_cnt;
AverageGier = AccumulatedGier / AccumulatedGier_cnt;
/* Get Pressure Differenz */
CurrentAltitude = InitialAltitude - AccumulatedAirPressure / AccumulatedAirPressure_cnt;
171,32 → 170,31
 
//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)));
Roll_Y_Offset = 0.9995F * Roll_Y_Offset + 0.0005F * (float) (MAX(-60, MIN(60,AverageRoll_Y)));
AdNeutralNick = 0.999F * AdNeutralNick + 0.001F * AdWertNick_Raw;
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[8] = AdWertRoll_Raw;
DebugOut.Analog[9] = AdWertGier_Raw;
DebugOut.Analog[6] = AdWertNick_Raw;
DebugOut.Analog[7] = AdWertRoll_Raw;
DebugOut.Analog[8] = AdWertGier_Raw;
DebugOut.Analog[9] = AdNeutralNick;
DebugOut.Analog[10] = AdNeutralRoll;
DebugOut.Analog[11] = AdNeutralGier;
#endif
AverageRoll_X -= Roll_X_Offset;
AverageRoll_Y -= Roll_Y_Offset;
AverageRoll_Z -= Roll_Z_Offset;
AccumulatedRoll = 0;
AccumulatedRoll_cnt = 0;
AccumulatedNick = 0;
AccumulatedNick_cnt = 0;
AccumulatedGier = 0;
AccumulatedGier_cnt = 0;
AccumulatedRoll_X = 0;
AccumulatedRoll_X_cnt = 0;
AccumulatedRoll_Y = 0;
AccumulatedRoll_Y_cnt = 0;
AccumulatedRoll_Z = 0;
AccumulatedRoll_Z_cnt = 0;
#if 0
ACC_X_Offset = 0.9999F * ACC_X_Offset + 0.0001F * ((float) AccumulatedACC_X / (float) AccumulatedACC_X_cnt);
ACC_Y_Offset = 0.9999F * ACC_Y_Offset + 0.0001F * ((float) AccumulatedACC_Y / (float) AccumulatedACC_Y_cnt);
215,33 → 213,6
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;
}
}
 
/* ****************************************************************************
518,7 → 489,7
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)
578,7 → 549,7
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));
620,7 → 591,7
}
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));