| 64,6 → 64,7 |
|---|
| #include "uart.h" |
| #include "rc.h" |
| #include "twimaster.h" |
| #include "mm3.h" |
| |
| unsigned char h,m,s; |
| volatile unsigned int I2CTimeout = 100; |
| 931,26 → 932,32 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Kompass |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(KompassValue && (ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE)) |
| { |
| int w,v; |
| static int SignalSchlecht = 0; |
| 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 < 25 && NeueKompassRichtungMerken && !SignalSchlecht) |
| if(ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE) |
| { |
| int w,v; |
| static uint8_t updKompass = 0; |
| |
| if (!updKompass--) // Aufruf mit ~10 Hz |
| { |
| KompassValue = MM3_heading(); |
| KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
| updKompass = 50; |
| } |
| |
| 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) |
| { |
| KompassStartwert = KompassValue; |
| NeueKompassRichtungMerken = 0; |
| } |
| w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
| w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
| if(w > 0) |
| { |
| KompassStartwert = KompassValue; |
| NeueKompassRichtungMerken = 0; |
| } |
| w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
| w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
| if(w > 0) |
| { |
| if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
| if(SignalSchlecht) SignalSchlecht--; |
| } |
| else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
| Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
| } |
| } |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| |