213,6 → 213,9 |
SendVersionToNavi = 1; |
LED_Init(); |
MikroKopterFlags |= FLAG_CALIBRATE; |
FromNaviCtrl_Value.Kalman_K = -1; |
FromNaviCtrl_Value.Kalman_MaxDrift = EE_Parameter.Driftkomp * 16; |
FromNaviCtrl_Value.Kalman_MaxFusion = 32; |
} |
|
//############################################################################ |
811,32 → 814,54 |
if(!Looping_Nick && !Looping_Roll) |
{ |
long tmp_long, tmp_long2; |
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)) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
|
if(FromNaviCtrl_Value.Kalman_K != -1) |
{ |
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
{ |
tmp_long /= 2; |
tmp_long2 /= 2; |
} |
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(tmp_long > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) FromNaviCtrl_Value.Kalman_MaxFusion; |
if(tmp_long < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long)-FromNaviCtrl_Value.Kalman_MaxFusion; |
if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion; |
if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion; |
} |
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)) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
#define AUSGLEICH 32 |
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
|
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
} |
|
Mess_IntegralNick -= tmp_long; |
Mess_IntegralRoll -= tmp_long2; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
if(ZaehlMessungen >= ABGLEICH_ANZAHL) |
{ |
static int cnt = 0; |
860,7 → 885,7 |
LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL; |
LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; |
|
if((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) |
if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1)) |
{ |
LageKorrekturNick /= 2; |
LageKorrekturRoll /= 2; |
908,7 → 933,7 |
#define BEWEGUNGS_LIMIT 20000 |
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1;// + labs(IntegralFehlerNick) / 4096; |
if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT) |
if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*16)) |
{ |
if(IntegralFehlerNick > FEHLER_LIMIT2) |
{ |
939,6 → 964,7 |
KompassSignalSchlecht = 1000; |
} |
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift/16; |
if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; |
if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; |
|
946,7 → 972,7 |
cnt = 1;// + labs(IntegralFehlerNick) / 4096; |
|
ausgleichRoll = 0; |
if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT) |
if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*16)) |
{ |
if(IntegralFehlerRoll > FEHLER_LIMIT2) |
{ |
975,8 → 1001,8 |
cnt = 0; |
KompassSignalSchlecht = 1000; |
} |
|
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift/16; |
if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; |
if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; |
} |
1093,7 → 1119,8 |
DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar; |
DebugOut.Analog[19] = WinkelOut.CalcState; |
DebugOut.Analog[20] = ServoValue; |
|
DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
DebugOut.Analog[30] = GPS_Nick; |
DebugOut.Analog[31] = GPS_Roll; |
|