| 653,17 → 653,29 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Kompass |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
| if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
| { |
| if(NeueKompassRichtungMerken) |
| int w,v; |
| static int SignalSchlecht = 0; |
| w = abs(IntegralNick /1024); // mit zunehmender Neigung den Einfluss drosseln |
| v = abs(IntegralRoll /1024); |
| if(v > w) w = v; // grösste Neigung ermitteln |
| if(w < 35 && NeueKompassRichtungMerken && !SignalSchlecht) |
| { |
| KompassStartwert = KompassValue; |
| NeueKompassRichtungMerken = 0; |
| } |
| ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| Mess_Integral_Gier -= (KompassRichtung * Parameter_KompassWirkung) / 32; // nach Kompass ausrichten |
| ANALOG_ON; // ADC einschalten |
| } |
| w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
| w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
| if(w > 0) |
| { |
| ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
| ANALOG_ON; // ADC einschalten |
| if(SignalSchlecht) SignalSchlecht--; |
| } |
| else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
| } |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| |
| 677,11 → 689,11 |
|---|
| // DebugOut.Analog[0] = MesswertNick; |
| // DebugOut.Analog[1] = MesswertRoll; |
| // DebugOut.Analog[2] = MesswertGier; |
| // DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
| // DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
| DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
| DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
| |
| DebugOut.Analog[0] = MM3.NickGrad; |
| DebugOut.Analog[1] = MM3.RollGrad; |
| // DebugOut.Analog[0] = MM3.NickGrad; |
| // DebugOut.Analog[1] = MM3.RollGrad; |
| DebugOut.Analog[2] = Mittelwert_AccNick; |
| DebugOut.Analog[3] = Mittelwert_AccRoll; |
| DebugOut.Analog[4] = MesswertGier; |