182,6 → 182,8 |
signed int Variance = 0; |
signed int CosAttitude; // for projection of hoover gas |
unsigned char ACC_AltitudeControl = 0; |
long NickSum, RollSum; |
signed int AccNick_err, AccRoll_err; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debugwerte zuordnen |
1213,20 → 1215,22 |
} |
else |
{ |
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
tmp_long /= 16; |
tmp_long2 /= 16; |
if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
if(MotorenEin) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
NickSum = NickSum + (long)((AdWertNickFilter + (TrimNick * 9))*9 - AccNick_err); |
AccNick_err = NickSum / 2048; // T=4s, gain: 8*9/4=18, res: 0.025deg |
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccNick + AccNick_err)); |
tmp_long /= 16; // T ca. 1s |
RollSum = RollSum + (long)((AdWertRollFilter + (TrimRoll * 9))*9 - AccRoll_err); |
AccRoll_err = RollSum / 2048; |
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccRoll + AccRoll_err)); |
tmp_long2 /= 16; |
} else { // zum Abgleich Acc zu Gyro |
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
tmp_long /= 16; |
tmp_long2 /= 16; |
} |
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
KompassFusion = 25; |
#define AUSGLEICH 32 |
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
1715,7 → 1719,7 |
else SollHoehe -= EE_Parameter.Hoehe_Verstaerkung / 3; |
} |
HeightTrimming = 0; |
LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied |
LIMIT_MIN_MAX(SollHoehe, (HoehenWertF-1024), (HoehenWertF+1024)); // max. 10m Unterschied |
if(Parameter_ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100; |
//update hoover gas stick value when setpoint is shifted |
if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0) |