| 71,9 → 71,12 |
|---|
| long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; |
| long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2; |
| volatile long Mess_Integral_Hoch = 0; |
| volatile int KompassValue = 0; |
| volatile int KompassStartwert = 0; |
| volatile int KompassRichtung = 0; |
| |
| int KompassValue = 0; |
| int KompassStartwert = 0; |
| int KompassRichtung = 0; |
| uint8_t updKompass; |
| |
| unsigned int KompassSignalSchlecht = 500; |
| unsigned char MAX_GAS,MIN_GAS; |
| unsigned char Notlandung = 0; |
| 158,6 → 161,12 |
|---|
| acc_neutral.Y = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
| acc_neutral.Z = Aktuell_az; |
| |
| Piep(2); |
| while (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -100) //Warten, bis Benutzer den Kopter neu ausgerichtet hat |
| |
| Delay_ms_Mess(100); |
| acc_neutral.compass = Aktuell_az; |
| |
| eeprom_write_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct)); |
| } |
| |
| 987,44 → 996,50 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll); |
| |
| if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
| { |
| int w,v,r,fehler,korrektur; |
| w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
| v = abs(IntegralRoll /512); |
| if(v > w) w = v; // grösste Neigung ermitteln |
| korrektur = w / 8 + 1; |
| if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht) |
| { |
| beeptime = 200; |
| // KompassStartwert = KompassValue; |
| KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
| NeueKompassRichtungMerken = 0; |
| } |
| fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
| ErsatzKompass += (fehler * 8) / korrektur; |
| w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
| w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
| if(w > 0) |
| { |
| if(!KompassSignalSchlecht) |
| { |
| GierGyroFehler += fehler; |
| v = 64 + ((MaxStickNick + MaxStickRoll)) / 8; |
| r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180; |
| // r = KompassRichtung; |
| v = (r * w) / v; // nach Kompass ausrichten |
| w = 3 * Parameter_KompassWirkung; |
| if(v > w) v = w; // Begrenzen |
| else |
| if(v < -w) v = -w; |
| Mess_Integral_Gier += v; |
| } |
| if(KompassSignalSchlecht) KompassSignalSchlecht--; |
| } |
| if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
| { |
| int w,v,r,fehler,korrektur; |
| |
| if (!updKompass--) // Aufruf mit ~20 Hz |
| { |
| updKompass = 25; |
| KompassValue = heading_MM3(); |
| KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
| } |
| w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
| v = abs(IntegralRoll /512); |
| if(v > w) w = v; // grösste Neigung ermitteln |
| korrektur = w / 8 + 1; |
| if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht) |
| { |
| beeptime = 200; |
| //KompassStartwert = KompassValue; |
| KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
| NeueKompassRichtungMerken = 0; |
| } |
| fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
| ErsatzKompass += (fehler * 8) / korrektur; |
| w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
| w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
| if(w > 0) |
| { |
| if(!KompassSignalSchlecht) |
| { |
| GierGyroFehler += fehler; |
| v = 64 + ((MaxStickNick + MaxStickRoll)) / 8; |
| r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180; |
| //r = KompassRichtung; |
| v = (r * w) / v; // nach Kompass ausrichten |
| w = 3 * Parameter_KompassWirkung; |
| if(v > w) v = w; // Begrenzen |
| else if(v < -w) v = -w; |
| Mess_Integral_Gier += v; |
| } |
| if(KompassSignalSchlecht) KompassSignalSchlecht--; |
| } |
| else KompassSignalSchlecht = 250; // so lange das Signal taub stellen --> ca. 0,5 sek |
| |
| } |
| } |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |