Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2351 → Rev 2352

/test_branches/FC2_2/analog.c
183,6 → 183,7
long my_ACC_AltitudeFusion(unsigned char init)
{
static long HoeheAlt, HoehenDiff, VerticalVelocitySum, AccVertical, VerticalVelocity;
signed int tmpK;
if(init) // beim Sensor Kalibrieren ist das 2 und beim Messbereich Erweitern ist das 1
{
if(init == 2) VerticalVelocitySum = 0;
192,7 → 193,10
else
{
HoehenDiff = HoehenWert - HoeheAlt;
AccVertical = (long)(AdWertAccHoch + 172)*CosAttitude/8192 - 172;
tmpK = AdWertAccRoll/32;
AccVertical = (long)(AdWertAccHoch + 172)*CosAttitude/8192 - 172 + tmpK*tmpK;
tmpK = AdWertAccNick/32;
AccVertical += tmpK*tmpK;
VerticalVelocitySum = VerticalVelocitySum - VerticalVelocity + AccVertical*13 + HoehenDiff*500; // Fusion vert. velocity, T=2s
VerticalVelocity = (VerticalVelocitySum + 512)/1024; // cm/s
HoeheAlt = HoehenWert;
/test_branches/FC2_2/fc.c
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)