Rev 1972 | Rev 1983 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1972 | Rev 1982 | ||
---|---|---|---|
Line 1036... | Line 1036... | ||
1036 | long tmp_long, tmp_long2; |
1036 | long tmp_long, tmp_long2; |
1037 | if(FromNaviCtrl_Value.Kalman_K > 0 /*&& !TrichterFlug*/) |
1037 | if(FromNaviCtrl_Value.Kalman_K > 0 /*&& !TrichterFlug*/) |
1038 | { |
1038 | { |
1039 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccNick - FromNaviCtrl.AccErrorN)); |
1039 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccNick - FromNaviCtrl.AccErrorN)); |
1040 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccRoll - FromNaviCtrl.AccErrorR)); |
1040 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccRoll - FromNaviCtrl.AccErrorR)); |
- | 1041 | ||
- | 1042 | DebugOut.Analog[16] = (Mittelwert_AccRoll - FromNaviCtrl.AccErrorR)/4; |
|
- | 1043 | DebugOut.Analog[17] = (Mittelwert_AccNick - FromNaviCtrl.AccErrorN)/4; |
|
- | 1044 | ||
1041 | tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
1045 | tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
1042 | tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
1046 | tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
1043 | if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
1047 | if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
1044 | { |
1048 | { |
1045 | tmp_long /= 2; |
1049 | tmp_long /= 2; |