| 638,18 → 638,20 |
|---|
| ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| Mess_IntegralNick -= tmp_long; |
| Mess_IntegralRoll -= tmp_long2; |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Gieren |
| // Gieren wie in .66c |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| sollGier = StickGier; |
| if(abs(StickGier) > 35) |
| if(abs(StickGier) > 20) // war 35 |
| { |
| if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
| } |
| tmp_int = EE_Parameter.Gier_P * (sollGier * abs(sollGier)) / 256; // expo |
| tmp_int = (long)EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
| tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
| sollGier = tmp_int; |
| Mess_Integral_Gier -= tmp_int; |
| if(Mess_Integral_Gier > 30000) Mess_Integral_Gier = 30000; // begrenzen |
| if(Mess_Integral_Gier <-30000) Mess_Integral_Gier =-30000; |
| if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen |
| if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
| |
| ANALOG_ON; // ADC einschalten |
| |
| 659,7 → 661,6 |
|---|
| if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
| { |
| int w,v; |
| static uint8_t SignalSchlecht = 0; |
| |
| if (!updKompass--) // Aufruf mit ~10 Hz |
| { |
| 668,10 → 669,10 |
|---|
| updKompass = 50; |
| } |
| |
| w = abs(IntegralNick /1024); // mit zunehmender Neigung den Einfluss drosseln |
| v = abs(IntegralRoll /1024); |
| w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
| v = abs(IntegralRoll /512); |
| if(v > w) w = v; // grösste Neigung ermitteln |
| if(w < 35 && NeueKompassRichtungMerken && !SignalSchlecht) |
| if(w < 35 && NeueKompassRichtungMerken) |
| { |
| KompassStartwert = KompassValue; |
| NeueKompassRichtungMerken = 0; |
| 680,17 → 681,13 |
|---|
| w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
| if(w > 0) |
| { |
| if(!SignalSchlecht) |
| { |
| ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
| ANALOG_ON; // ADC einschalten |
| } |
| else SignalSchlecht--; |
| |
| } |
| else SignalSchlecht = 250; // so lange das Signal taub stellen --> ca. 1/2 sek |
| } |
| else beeptime = 100; |
| } |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| |