Rev 477 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 477 | Rev 496 | ||
|---|---|---|---|
| Line 636... | Line 636... | ||
| 636 | if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
636 | if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
| 637 | if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
637 | if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
| 638 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
638 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| 639 | Mess_IntegralNick -= tmp_long; |
639 | Mess_IntegralNick -= tmp_long; |
| 640 | Mess_IntegralRoll -= tmp_long2; |
640 | Mess_IntegralRoll -= tmp_long2; |
| - | 641 | ||
| 641 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
642 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 642 | // Gieren |
643 | // Gieren wie in .66c |
| 643 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
644 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 644 | sollGier = StickGier; |
- | |
| 645 | if(abs(StickGier) > 35) |
645 | if(abs(StickGier) > 20) // war 35 |
| 646 | { |
646 | { |
| 647 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
647 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
| 648 | } |
648 | } |
| 649 | tmp_int = EE_Parameter.Gier_P * (sollGier * abs(sollGier)) / 256; // expo |
649 | tmp_int = (long)EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
| - | 650 | tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
|
| - | 651 | sollGier = tmp_int; |
|
| 650 | Mess_Integral_Gier -= tmp_int; |
652 | Mess_Integral_Gier -= tmp_int; |
| 651 | if(Mess_Integral_Gier > 30000) Mess_Integral_Gier = 30000; // begrenzen |
653 | if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen |
| 652 | if(Mess_Integral_Gier <-30000) Mess_Integral_Gier =-30000; |
654 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
| Line 653... | Line 655... | ||
| 653 | 655 | ||
| Line 654... | Line 656... | ||
| 654 | ANALOG_ON; // ADC einschalten |
656 | ANALOG_ON; // ADC einschalten |
| 655 | 657 | ||
| 656 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
658 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 657 | // Kompass |
659 | // Kompass |
| 658 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
660 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 659 | if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
661 | if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
| 660 | { |
- | |
| Line 661... | Line 662... | ||
| 661 | int w,v; |
662 | { |
| 662 | static uint8_t SignalSchlecht = 0; |
663 | int w,v; |
| 663 | 664 | ||
| 664 | if (!updKompass--) // Aufruf mit ~10 Hz |
665 | if (!updKompass--) // Aufruf mit ~10 Hz |
| 665 | { |
666 | { |
| 666 | KompassValue = heading_MM3(); |
667 | KompassValue = heading_MM3(); |
| Line 667... | Line 668... | ||
| 667 | KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
668 | KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
| 668 | updKompass = 50; |
669 | updKompass = 50; |
| 669 | } |
670 | } |
| 670 | 671 | ||
| 671 | w = abs(IntegralNick /1024); // mit zunehmender Neigung den Einfluss drosseln |
672 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
| 672 | v = abs(IntegralRoll /1024); |
673 | v = abs(IntegralRoll /512); |
| 673 | if(v > w) w = v; // grösste Neigung ermitteln |
674 | if(v > w) w = v; // grösste Neigung ermitteln |
| 674 | if(w < 35 && NeueKompassRichtungMerken && !SignalSchlecht) |
675 | if(w < 35 && NeueKompassRichtungMerken) |
| 675 | { |
676 | { |
| 676 | KompassStartwert = KompassValue; |
677 | KompassStartwert = KompassValue; |
| 677 | NeueKompassRichtungMerken = 0; |
678 | NeueKompassRichtungMerken = 0; |
| 678 | } |
679 | } |
| 679 | w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
- | |
| 680 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
- | |
| 681 | if(w > 0) |
680 | w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
| 682 | { |
681 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
| 683 | if(!SignalSchlecht) |
682 | if(w > 0) |
| 684 | { |
683 | { |
| 685 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
684 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| 686 | Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
- | |
| 687 | ANALOG_ON; // ADC einschalten |
- | |
| 688 | } |
- | |
| 689 | else SignalSchlecht--; |
685 | Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
| - | 686 | ANALOG_ON; // ADC einschalten |
|
| 690 | 687 | } |
|
| 691 | } |
688 | else beeptime = 100; |
| Line 692... | Line 689... | ||
| 692 | else SignalSchlecht = 250; // so lange das Signal taub stellen --> ca. 1/2 sek |
689 | } |
| 693 | } |
690 | |