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)); |