Rev 787 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 787 | Rev 851 | ||
---|---|---|---|
Line 151... | Line 151... | ||
151 | CalibrierMittelwert(); |
151 | CalibrierMittelwert(); |
Line 152... | Line 152... | ||
152 | 152 | ||
153 | acc_neutral.X = abs(Mittelwert_AccNick) / ACC_AMPLIFY; |
153 | acc_neutral.X = abs(Mittelwert_AccNick) / ACC_AMPLIFY; |
154 | acc_neutral.Y = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
154 | acc_neutral.Y = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
155 | acc_neutral.Z = Aktuell_az; |
155 | acc_neutral.Z = Aktuell_az; |
156 | 156 | /* |
|
157 | while (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 100) //Warten, bis Benutzer den Kopter neu ausgerichtet hat |
157 | while (PPM_in_gas > 100) //Warten, bis Benutzer den Kopter neu ausgerichtet hat |
158 | { |
158 | { |
Line 159... | Line 159... | ||
159 | uint8_t delay=9; |
159 | uint8_t delay=9; |
160 | 160 | |
|
Line 165... | Line 165... | ||
165 | beeptime = 100; |
165 | beeptime = 100; |
166 | } |
166 | } |
167 | } |
167 | } |
168 | Delay_ms_Mess(100); |
168 | Delay_ms_Mess(100); |
169 | acc_neutral.C = Aktuell_az; |
169 | acc_neutral.C = Aktuell_az; |
170 | 170 | */ |
|
171 | eeprom_write_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct)); |
171 | eeprom_write_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct)); |
172 | } |
172 | } |
Line 173... | Line 173... | ||
173 | 173 | ||
174 | //############################################################################ |
174 | //############################################################################ |
Line 448... | Line 448... | ||
448 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
448 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
449 | // Gaswert ermitteln |
449 | // Gaswert ermitteln |
450 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
450 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
451 | GasMischanteil = StickGas; |
451 | GasMischanteil = StickGas; |
452 | if(GasMischanteil < 0) GasMischanteil = 0; |
452 | if(GasMischanteil < 0) GasMischanteil = 0; |
- | 453 | ||
- | 454 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 455 | // Datenübernahme von RC |
|
- | 456 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 457 | int16_t PPM_in_nick, PPM_in_roll, PPM_in_gier, PPM_diff_nick, PPM_diff_roll, PPM_in_gas; |
|
- | 458 | ||
- | 459 | uint8_t tmp_sreg = SREG; |
|
- | 460 | cli(); |
|
- | 461 | PPM_in_nick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; |
|
- | 462 | PPM_diff_nick = PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]]; |
|
- | 463 | PPM_in_roll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; |
|
- | 464 | PPM_diff_roll = PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]]; |
|
- | 465 | PPM_in_gier = PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
|
- | 466 | PPM_in_gas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]]; |
|
- | 467 | SREG = tmp_sreg; |
|
- | 468 | ||
453 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
469 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
454 | // Emfang schlecht |
470 | // Emfang schlecht |
455 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
471 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
456 | if(SenderOkay < 100) |
472 | if(SenderOkay < 100) |
457 | { |
473 | { |
Line 472... | Line 488... | ||
472 | ROT_ON; |
488 | ROT_ON; |
473 | if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken |
489 | if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken |
474 | { |
490 | { |
475 | GasMischanteil = EE_Parameter.NotGas; |
491 | GasMischanteil = EE_Parameter.NotGas; |
476 | Notlandung = 1; |
492 | Notlandung = 1; |
477 | PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
493 | PPM_diff_nick = 0; |
478 | PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
494 | PPM_diff_roll = 0; |
479 | PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
495 | PPM_in_nick = 0; |
480 | PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
496 | PPM_in_roll = 0; |
481 | PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0; |
497 | PPM_in_gier = 0; |
482 | } |
498 | } |
483 | else MotorenEin = 0; |
499 | else MotorenEin = 0; |
484 | } |
500 | } |
485 | else |
501 | else |
486 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
502 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 499... | Line 515... | ||
499 | SummeNick = 0; |
515 | SummeNick = 0; |
500 | SummeRoll = 0; |
516 | SummeRoll = 0; |
501 | Mess_Integral_Gier = 0; |
517 | Mess_Integral_Gier = 0; |
502 | Mess_Integral_Gier2 = 0; |
518 | Mess_Integral_Gier2 = 0; |
503 | } |
519 | } |
504 | if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) |
520 | if((PPM_in_gas > 80) && MotorenEin == 0) |
505 | { |
521 | { |
506 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
522 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
507 | // auf Nullwerte kalibrieren |
523 | // auf Nullwerte kalibrieren |
508 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
524 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
509 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte |
525 | if(PPM_in_gier > 75) // Neutralwerte |
510 | { |
526 | { |
511 | if(++delay_neutral > 200) // nicht sofort |
527 | if(++delay_neutral > 200) // nicht sofort |
512 | { |
528 | { |
513 | GRN_OFF; |
529 | GRN_OFF; |
514 | MotorenEin = 0; |
530 | MotorenEin = 0; |
515 | delay_neutral = 0; |
531 | delay_neutral = 0; |
516 | modell_fliegt = 0; |
532 | modell_fliegt = 0; |
517 | if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70) |
533 | if(PPM_in_nick > 70 || abs(PPM_in_roll) > 70) |
518 | { |
534 | { |
519 | unsigned char setting=1; |
535 | unsigned char setting=1; |
520 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1; |
536 | if(PPM_in_roll > 70 && PPM_in_nick < 70) setting = 1; |
521 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2; |
537 | if(PPM_in_roll > 70 && PPM_in_nick > 70) setting = 2; |
522 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3; |
538 | if(PPM_in_roll < 70 && PPM_in_nick > 70) setting = 3; |
523 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4; |
539 | if(PPM_in_roll <-70 && PPM_in_nick > 70) setting = 4; |
524 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5; |
540 | if(PPM_in_roll <-70 && PPM_in_nick < 70) setting = 5; |
525 | eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken |
541 | eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken |
526 | } |
542 | } |
527 | ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], sizeof(struct mk_param_struct)); |
543 | ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], sizeof(struct mk_param_struct)); |
528 | SetNeutral(); |
544 | SetNeutral(); |
529 | Piep(GetActiveParamSetNumber()); |
545 | Piep(GetActiveParamSetNumber()); |
Line 532... | Line 548... | ||
532 | else delay_neutral = 0; |
548 | else delay_neutral = 0; |
533 | } |
549 | } |
534 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
550 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
535 | // Gas ist unten |
551 | // Gas ist unten |
536 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
552 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
537 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120) |
553 | if(PPM_in_gas < 35-120) |
538 | { |
554 | { |
539 | // Starten |
555 | // Starten |
540 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) |
556 | if(PPM_in_gier < -75) |
541 | { |
557 | { |
542 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
558 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
543 | // Einschalten |
559 | // Einschalten |
544 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
560 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
545 | if(++delay_einschalten > 200) |
561 | if(++delay_einschalten > 200) |
Line 561... | Line 577... | ||
561 | else delay_einschalten = 0; |
577 | else delay_einschalten = 0; |
562 | //Auf Neutralwerte setzen |
578 | //Auf Neutralwerte setzen |
563 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
579 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
564 | // Auschalten |
580 | // Auschalten |
565 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
581 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
566 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) |
582 | if(PPM_in_gier > 75) |
567 | { |
583 | { |
568 | if(++delay_ausschalten > 200) // nicht sofort |
584 | if(++delay_ausschalten > 200) // nicht sofort |
569 | { |
585 | { |
570 | MotorenEin = 0; |
586 | MotorenEin = 0; |
571 | delay_ausschalten = 200; |
587 | delay_ausschalten = 200; |
Line 581... | Line 597... | ||
581 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
597 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
582 | if(!NewPpmData-- || Notlandung) |
598 | if(!NewPpmData-- || Notlandung) |
583 | { |
599 | { |
584 | int tmp_int; |
600 | int tmp_int; |
585 | ParameterZuordnung(); |
601 | ParameterZuordnung(); |
586 | StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
602 | StickNick = (StickNick * 3 + PPM_in_nick * EE_Parameter.Stick_P) / 4; |
587 | StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
603 | StickNick += PPM_diff_nick * EE_Parameter.Stick_D; |
588 | StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
604 | StickRoll = (StickRoll * 3 + PPM_in_roll * EE_Parameter.Stick_P) / 4; |
589 | StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
605 | StickRoll += PPM_diff_roll * EE_Parameter.Stick_D; |
590 | 606 | ||
591 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
607 | StickGier = -PPM_in_gier; |
592 | StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
608 | StickGas = PPM_in_gas + 120; |
593 | 609 | ||
594 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick) |
610 | if(abs(PPM_in_nick) > MaxStickNick) |
595 | MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--; |
611 | MaxStickNick = abs(PPM_in_nick); else MaxStickNick--; |
596 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll) |
612 | if(abs(PPM_in_roll) > MaxStickRoll) |
597 | MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--; |
613 | MaxStickRoll = abs(PPM_in_roll); else MaxStickRoll--; |
598 | if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;} |
614 | if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;} |
Line 599... | Line 615... | ||
599 | 615 | ||
600 | GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
616 | GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
Line 636... | Line 652... | ||
636 | if(GyroFaktor < 0) GyroFaktor = 0; |
652 | if(GyroFaktor < 0) GyroFaktor = 0; |
637 | if(IntegralFaktor < 0) IntegralFaktor = 0; |
653 | if(IntegralFaktor < 0) IntegralFaktor = 0; |
638 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
654 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
639 | // Looping? |
655 | // Looping? |
640 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
656 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
641 | if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
657 | if((PPM_in_roll > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
642 | else |
658 | else |
643 | { |
659 | { |
644 | { |
660 | { |
645 | if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0; |
661 | if((PPM_in_roll < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0; |
646 | } |
662 | } |
647 | } |
663 | } |
648 | if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1; |
664 | if((PPM_in_roll < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1; |
649 | else |
665 | else |
650 | { |
666 | { |
651 | if(Looping_Rechts) // Hysterese |
667 | if(Looping_Rechts) // Hysterese |
652 | { |
668 | { |
653 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0; |
669 | if(PPM_in_roll > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0; |
654 | } |
670 | } |
655 | } |
671 | } |
Line 656... | Line 672... | ||
656 | 672 | ||
657 | if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1; |
673 | if((PPM_in_nick > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1; |
658 | else |
674 | else |
659 | { |
675 | { |
660 | if(Looping_Oben) // Hysterese |
676 | if(Looping_Oben) // Hysterese |
661 | { |
677 | { |
662 | if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0; |
678 | if((PPM_in_nick < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0; |
663 | } |
679 | } |
664 | } |
680 | } |
665 | if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1; |
681 | if((PPM_in_nick < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1; |
666 | else |
682 | else |
667 | { |
683 | { |
668 | if(Looping_Unten) // Hysterese |
684 | if(Looping_Unten) // Hysterese |
669 | { |
685 | { |
670 | if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0; |
686 | if(PPM_in_nick > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0; |
671 | } |
687 | } |
Line 672... | Line 688... | ||
672 | } |
688 | } |
673 | 689 | ||
Line 688... | Line 704... | ||
688 | if(Notlandung) |
704 | if(Notlandung) |
689 | { |
705 | { |
690 | StickGier = 0; |
706 | StickGier = 0; |
691 | StickNick = 0; |
707 | StickNick = 0; |
692 | StickRoll = 0; |
708 | StickRoll = 0; |
693 | GyroFaktor = 0.1; |
709 | GyroFaktor = 0.5; |
694 | IntegralFaktor = 0.005; |
710 | IntegralFaktor = 0.003; |
695 | Looping_Roll = 0; |
711 | Looping_Roll = 0; |
696 | Looping_Nick = 0; |
712 | Looping_Nick = 0; |
697 | } |
713 | } |
Line 728... | Line 744... | ||
728 | long tmp_long, tmp_long2; |
744 | long tmp_long, tmp_long2; |
729 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
745 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
730 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
746 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
731 | tmp_long /= 16; |
747 | tmp_long /= 16; |
732 | tmp_long2 /= 16; |
748 | tmp_long2 /= 16; |
733 | if((MaxStickNick > 15) || (MaxStickRoll > 15)) |
749 | if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in_gier) > 25)) |
734 | { |
- | |
735 | tmp_long /= 3; |
- | |
736 | tmp_long2 /= 3; |
- | |
737 | } |
- | |
738 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
- | |
739 | { |
750 | { |
740 | tmp_long /= 3; |
751 | tmp_long /= 3; |
741 | tmp_long2 /= 3; |
752 | tmp_long2 /= 3; |
742 | } |
753 | } |
- | 754 | //if(abs(PPM_in_gier) > 25) |
|
- | 755 | // { |
|
- | 756 | // tmp_long /= 3; |
|
- | 757 | // tmp_long2 /= 3; |
|
- | 758 | // } |
|
Line 743... | Line 759... | ||
743 | 759 | ||
744 | #define AUSGLEICH 32 |
760 | #define AUSGLEICH 32 |
745 | if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
761 | if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
746 | if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
762 | if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
Line 772... | Line 788... | ||
772 | ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich; |
788 | ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich; |
Line 773... | Line 789... | ||
773 | 789 | ||
774 | LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL; |
790 | LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL; |
Line 775... | Line 791... | ||
775 | LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; |
791 | LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; |
776 | 792 | ||
777 | if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) |
793 | if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in_gier) > 25)) |
778 | { |
794 | { |
779 | LageKorrekturNick /= 2; |
795 | LageKorrekturNick /= 2; |
Line 925... | Line 941... | ||
925 | { |
941 | { |
926 | if (!updKompass--) // Aufruf mit ~10 Hz |
942 | if (!updKompass--) // Aufruf mit ~10 Hz |
927 | { |
943 | { |
928 | updKompass = 49; |
944 | updKompass = 49; |
Line 929... | Line 945... | ||
929 | 945 | ||
930 | if ((MaxStickNick < 75) && (MaxStickRoll < 75)) // Bei extremen Flugmanövern keine Kompassauswertung |
946 | if ((MaxStickNick < 50) && (MaxStickRoll < 50)) // Bei extremen Flugmanövern keine Kompassauswertung |
931 | { |
947 | { |
932 | KompassValue = heading_MM3(); |
948 | KompassValue = heading_MM3(); |
933 | KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
949 | KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
934 | if(NeueKompassRichtungMerken) |
950 | if(NeueKompassRichtungMerken) |
Line 1039... | Line 1055... | ||
1039 | if(Notlandung) SollHoehe = 0; |
1055 | if(Notlandung) SollHoehe = 0; |
1040 | h = HoehenWert; |
1056 | h = HoehenWert; |
1041 | if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln |
1057 | if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln |
1042 | { h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil |
1058 | { h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil |
1043 | h = GasMischanteil - h; // vom Gas abziehen |
1059 | h = GasMischanteil - h; // vom Gas abziehen |
1044 | h -= (HoeheD * Parameter_Luftdruck_D)/8; // D-Anteil |
1060 | h -= (HoeheD * Parameter_Luftdruck_D) / 8; // D-Anteil |
1045 | tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32; |
1061 | tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32; |
1046 | if(tmp_int > 50) tmp_int = 50; |
1062 | if(tmp_int > 50) tmp_int = 50; |
1047 | else if(tmp_int < -50) tmp_int = -50; |
1063 | else if(tmp_int < -50) tmp_int = -50; |
1048 | h -= tmp_int; |
1064 | h -= tmp_int; |
1049 | hoehenregler = (hoehenregler*15 + h) / 16; |
1065 | hoehenregler = (hoehenregler*15 + h) / 16; |