Rev 1400 | Rev 1453 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1400 | Rev 1452 | ||
---|---|---|---|
Line 78... | Line 78... | ||
78 | long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; |
78 | long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; |
79 | long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2; |
79 | long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2; |
80 | volatile long Mess_Integral_Hoch = 0; |
80 | volatile long Mess_Integral_Hoch = 0; |
81 | int KompassValue = 0; |
81 | int KompassValue = 0; |
82 | int KompassStartwert = 0; |
82 | int KompassStartwert = 0; |
83 | int KompassEinschaltStartwert = 0; |
- | |
84 | int KompassRichtung = 0; |
83 | int KompassRichtung = 0; |
85 | unsigned int KompassSignalSchlecht = 500; |
84 | unsigned int KompassSignalSchlecht = 500; |
86 | unsigned char MAX_GAS,MIN_GAS; |
85 | unsigned char MAX_GAS,MIN_GAS; |
87 | unsigned char HoehenReglerAktiv = 0; |
86 | unsigned char HoehenReglerAktiv = 0; |
88 | unsigned char TrichterFlug = 0; |
87 | unsigned char TrichterFlug = 0; |
Line 161... | Line 160... | ||
161 | signed int tmp_motorwert[MAX_MOTORS]; |
160 | signed int tmp_motorwert[MAX_MOTORS]; |
162 | unsigned char LoadHandler = 0; |
161 | unsigned char LoadHandler = 0; |
163 | #define LIMIT_MIN(value, min) {if(value < min) value = min;} |
162 | #define LIMIT_MIN(value, min) {if(value < min) value = min;} |
164 | #define LIMIT_MAX(value, max) {if(value > max) value = max;} |
163 | #define LIMIT_MAX(value, max) {if(value > max) value = max;} |
165 | #define LIMIT_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
164 | #define LIMIT_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
- | 165 | uint8_t servoValues[6] = { 127, 127, 127, 127, 127 , 127 }; |
|
- | 166 | ||
- | 167 | uint8_t MakeByte(int16_t i) { |
|
- | 168 | if (i < 0) return 0; |
|
- | 169 | if (i > 255) return 255; |
|
- | 170 | return i; |
|
- | 171 | } |
|
Line 166... | Line 172... | ||
166 | 172 | ||
167 | int MotorSmoothing(int neu, int alt) |
173 | int MotorSmoothing(int neu, int alt) |
168 | { |
174 | { |
169 | int motor; |
175 | int motor; |
Line 249... | Line 255... | ||
249 | Mess_Integral_Gier = 0; |
255 | Mess_Integral_Gier = 0; |
250 | StartLuftdruck = Luftdruck; |
256 | StartLuftdruck = Luftdruck; |
251 | VarioMeter = 0; |
257 | VarioMeter = 0; |
252 | Mess_Integral_Hoch = 0; |
258 | Mess_Integral_Hoch = 0; |
253 | KompassStartwert = KompassValue; |
259 | KompassStartwert = KompassValue; |
254 | KompassEinschaltStartwert = KompassValue; |
- | |
255 | GPS_Neutral(); |
260 | GPS_Neutral(); |
256 | beeptime = 50; |
261 | beeptime = 50; |
257 | Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L; |
262 | Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L; |
258 | Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L; |
263 | Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L; |
259 | ExternHoehenValue = 0; |
264 | ExternHoehenValue = 0; |
Line 471... | Line 476... | ||
471 | void SendMotorData(void) |
476 | void SendMotorData(void) |
472 | //############################################################################ |
477 | //############################################################################ |
473 | { |
478 | { |
474 | unsigned char i; |
479 | unsigned char i; |
475 | if(!MotorenEin) |
480 | if(!MotorenEin) |
476 | { |
- | |
477 | MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY); |
481 | MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY); |
478 | for(i=0;i<MAX_MOTORS;i++) |
- | |
479 | { |
- | |
480 | if(!PC_MotortestActive) MotorTest[i] = 0; |
- | |
481 | Motor[i] = MotorTest[i]; |
- | |
482 | } |
482 | else |
483 | if(PC_MotortestActive) PC_MotortestActive--; |
- | |
484 | } |
- | |
485 | else MikroKopterFlags |= FLAG_MOTOR_RUN; |
483 | MikroKopterFlags |= FLAG_MOTOR_RUN; |
Line 486... | Line -... | ||
486 | - | ||
487 | DebugOut.Analog[12] = Motor[0]; |
- | |
488 | DebugOut.Analog[13] = Motor[1]; |
- | |
489 | DebugOut.Analog[14] = Motor[3]; |
- | |
Line 490... | Line 484... | ||
490 | DebugOut.Analog[15] = Motor[2]; |
484 | |
491 | 485 | ||
492 | //Start I2C Interrupt Mode |
486 | //Start I2C Interrupt Mode |
493 | twi_state = 0; |
487 | twi_state = 0; |
Line 538... | Line 532... | ||
538 | Ki = 10300 / (Parameter_I_Faktor + 1); |
532 | Ki = 10300 / (Parameter_I_Faktor + 1); |
539 | MAX_GAS = EE_Parameter.Gas_Max; |
533 | MAX_GAS = EE_Parameter.Gas_Max; |
540 | MIN_GAS = EE_Parameter.Gas_Min; |
534 | MIN_GAS = EE_Parameter.Gas_Min; |
541 | } |
535 | } |
Line 542... | Line -... | ||
542 | - | ||
543 | 536 | ||
544 | //############################################################################ |
537 | //############################################################################ |
545 | // |
538 | // |
546 | void MotorRegler(void) |
539 | void MotorRegler(void) |
547 | //############################################################################ |
540 | //############################################################################ |
Line 597... | Line 590... | ||
597 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
590 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
598 | // Emfang gut |
591 | // Emfang gut |
599 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
592 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
600 | if(SenderOkay > 140) |
593 | if(SenderOkay > 140) |
601 | { |
594 | { |
- | 595 | ||
602 | MikroKopterFlags &= ~FLAG_NOTLANDUNG; |
596 | MikroKopterFlags &= ~FLAG_NOTLANDUNG; |
603 | RcLostTimer = EE_Parameter.NotGasZeit * 50; |
597 | RcLostTimer = EE_Parameter.NotGasZeit * 50; |
604 | if(GasMischanteil > 40 && MotorenEin) |
598 | if(GasMischanteil > 40 && MotorenEin) |
605 | { |
599 | { |
606 | if(modell_fliegt < 0xffff) modell_fliegt++; |
600 | if(modell_fliegt < 0xffff) modell_fliegt++; |
Line 695... | Line 689... | ||
695 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
689 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
696 | // Einschalten |
690 | // Einschalten |
697 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
691 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
698 | if(++delay_einschalten > 200) |
692 | if(++delay_einschalten > 200) |
699 | { |
693 | { |
- | 694 | beeptime = 1000; |
|
700 | delay_einschalten = 200; |
695 | delay_einschalten = 200; |
701 | modell_fliegt = 1; |
696 | modell_fliegt = 1; |
702 | MotorenEin = 1; |
697 | MotorenEin = 1; |
703 | sollGier = 0; |
698 | sollGier = 0; |
704 | Mess_Integral_Gier = 0; |
699 | Mess_Integral_Gier = 0; |
Line 708... | Line 703... | ||
708 | Mess_IntegralNick2 = IntegralNick; |
703 | Mess_IntegralNick2 = IntegralNick; |
709 | Mess_IntegralRoll2 = IntegralRoll; |
704 | Mess_IntegralRoll2 = IntegralRoll; |
710 | SummeNick = 0; |
705 | SummeNick = 0; |
711 | SummeRoll = 0; |
706 | SummeRoll = 0; |
712 | MikroKopterFlags |= FLAG_START; |
707 | MikroKopterFlags |= FLAG_START; |
713 | KompassEinschaltStartwert = KompassValue; |
- | |
714 | } |
708 | } |
715 | } |
709 | } |
716 | else delay_einschalten = 0; |
710 | else delay_einschalten = 0; |
717 | //Auf Neutralwerte setzen |
711 | //Auf Neutralwerte setzen |
718 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
712 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 720... | Line 714... | ||
720 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
714 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
721 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) |
715 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) |
722 | { |
716 | { |
723 | if(++delay_ausschalten > 200) // nicht sofort |
717 | if(++delay_ausschalten > 200) // nicht sofort |
724 | { |
718 | { |
- | 719 | beeptime = 1000; |
|
725 | MotorenEin = 0; |
720 | MotorenEin = 0; |
726 | delay_ausschalten = 200; |
721 | delay_ausschalten = 200; |
727 | modell_fliegt = 0; |
722 | modell_fliegt = 0; |
728 | } |
723 | } |
729 | } |
724 | } |
Line 735... | Line 730... | ||
735 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
730 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 736... | Line 731... | ||
736 | 731 | ||
737 | if(!NewPpmData-- || (MikroKopterFlags & FLAG_NOTLANDUNG)) |
732 | if(!NewPpmData-- || (MikroKopterFlags & FLAG_NOTLANDUNG)) |
738 | { |
733 | { |
739 | static int stick_nick,stick_roll; |
- | |
740 | int e_stick_nick, e_stick_nick_diff, e_stick_roll, e_stick_roll_diff; |
- | |
741 | int stick_p, stick_d; |
- | |
742 | 734 | static int stick_nick,stick_roll; |
|
743 | ParameterZuordnung(); |
- | |
744 | - | ||
745 | stick_p = EE_Parameter.Stick_P; |
- | |
746 | stick_d = EE_Parameter.Stick_D; |
- | |
747 | - | ||
748 | if (Parameter_UserParam8) { |
- | |
749 | - | ||
750 | int angle, stick_nick_tmp, stick_roll_tmp; |
- | |
751 | - | ||
752 | if (Parameter_UserParam8 == 254) { |
- | |
753 | angle = -45; |
- | |
754 | } else if (Parameter_UserParam8 > 128) { |
- | |
755 | angle = KompassValue; |
- | |
756 | } else { |
- | |
757 | angle = (ErsatzKompass / GIER_GRAD_FAKTOR); |
- | |
758 | } |
- | |
759 | - | ||
760 | angle = ((KompassEinschaltStartwert - angle + 360) % 360); |
- | |
761 | 735 | ParameterZuordnung(); |
|
762 | stick_nick_tmp = (long)c_cos_8192(angle) * (long)PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] / 8192L; |
- | |
763 | stick_roll_tmp = (long)c_sin_8192(angle) * (long)PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] / 8192L; |
- | |
764 | - | ||
765 | e_stick_nick = stick_roll_tmp + stick_nick_tmp; |
- | |
766 | e_stick_roll = stick_roll_tmp - stick_nick_tmp; |
- | |
767 | - | ||
768 | stick_nick_tmp = (long)c_cos_8192(angle) * (long)PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] / 8192L; |
- | |
769 | stick_roll_tmp = (long)c_sin_8192(angle) * (long)PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] / 8192L; |
- | |
770 | - | ||
771 | e_stick_nick_diff = stick_roll_tmp + stick_nick_tmp; |
- | |
772 | e_stick_roll_diff = stick_roll_tmp - stick_nick_tmp; |
- | |
773 | - | ||
774 | if (Parameter_UserParam8 == 255) { |
- | |
775 | - | ||
776 | int gier = abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]); |
- | |
777 | if (gier > 100) { |
- | |
778 | stick_d /= 3; |
- | |
779 | } else if (gier > 50) { |
- | |
780 | stick_d /= 2; |
- | |
781 | } |
- | |
782 | - | ||
783 | if (stick_d < 1) { |
- | |
784 | stick_d = 1; |
- | |
785 | } |
- | |
786 | - | ||
787 | } |
- | |
788 | - | ||
789 | /* |
- | |
790 | DebugOut.Analog[12] = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; |
- | |
791 | DebugOut.Analog[13] = e_stick_nick; |
- | |
792 | DebugOut.Analog[14] = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; |
- | |
793 | DebugOut.Analog[15] = e_stick_roll; |
- | |
794 | */ |
- | |
795 | - | ||
796 | } else { |
- | |
797 | 736 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
|
798 | e_stick_nick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; |
- | |
799 | e_stick_roll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; |
- | |
800 | e_stick_nick_diff = PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]]; |
- | |
801 | e_stick_roll_diff = PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]]; |
- | |
802 | - | ||
803 | } |
- | |
804 | - | ||
805 | stick_nick = (stick_nick * 3 + e_stick_nick * stick_p) / 4; |
- | |
806 | stick_nick += e_stick_nick_diff * stick_d; |
737 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
Line 807... | Line 738... | ||
807 | StickNick = stick_nick - (GPS_Nick + GPS_Nick2); |
738 | StickNick = stick_nick - (GPS_Nick + GPS_Nick2); |
808 | 739 | ||
809 | stick_roll = (stick_roll * 3 + e_stick_roll * stick_p) / 4; |
740 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
Line 810... | Line 741... | ||
810 | stick_roll += e_stick_roll_diff * stick_d; |
741 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
811 | StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); |
742 | StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); |
812 | 743 | ||
Line 1231... | Line 1162... | ||
1231 | DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
1162 | DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
1232 | DebugOut.Analog[2] = Mittelwert_AccNick / 4; |
1163 | DebugOut.Analog[2] = Mittelwert_AccNick / 4; |
1233 | DebugOut.Analog[3] = Mittelwert_AccRoll / 4; |
1164 | DebugOut.Analog[3] = Mittelwert_AccRoll / 4; |
1234 | DebugOut.Analog[4] = MesswertGier; |
1165 | DebugOut.Analog[4] = MesswertGier; |
1235 | DebugOut.Analog[5] = HoehenWert/5; |
1166 | DebugOut.Analog[5] = HoehenWert/5; |
1236 | DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
1167 | //DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
1237 | DebugOut.Analog[8] = KompassValue; |
1168 | DebugOut.Analog[8] = KompassValue; |
1238 | DebugOut.Analog[9] = UBat; |
1169 | DebugOut.Analog[9] = UBat; |
1239 | DebugOut.Analog[10] = SenderOkay; |
1170 | DebugOut.Analog[10] = SenderOkay; |
1240 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
1171 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
1241 | //DebugOut.Analog[16] = Mittelwert_AccHoch; |
1172 | //DebugOut.Analog[16] = Mittelwert_AccHoch; |
Line 1592... | Line 1523... | ||
1592 | if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0) |
1523 | if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0) |
1593 | { |
1524 | { |
1594 | modell_fliegt = 1; |
1525 | modell_fliegt = 1; |
1595 | GasMischanteil = MIN_GAS; |
1526 | GasMischanteil = MIN_GAS; |
1596 | } |
1527 | } |
1597 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1528 | /*// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1598 | // + Mischer und PI-Regler |
1529 | // + Mischer und PI-Regler |
1599 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1530 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1600 | DebugOut.Analog[7] = GasMischanteil; |
1531 | DebugOut.Analog[7] = GasMischanteil; |
1601 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1532 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1602 | // Gier-Anteil |
1533 | // Gier-Anteil |
Line 1640... | Line 1571... | ||
1640 | if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
1571 | if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
1641 | if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
1572 | if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
1642 | pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
1573 | pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
1643 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1574 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1644 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
1575 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
1645 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
1576 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;*/ |
Line 1646... | Line 1577... | ||
1646 | 1577 | ||
1647 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1578 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1648 | // Universal Mixer |
1579 | // Servo Mixer |
- | 1580 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 1581 | { |
|
1649 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1582 | int16_t nick, roll, tmp, pitch, throttle; |
1650 | for(i=0; i<MAX_MOTORS; i++) |
1583 | int16_t servo[6]; |
1651 | { |
1584 | |
- | 1585 | // throttle |
|
- | 1586 | if (MotorenEin == 0 || MikroKopterFlags & FLAG_NOTLANDUNG) { |
|
- | 1587 | tmp = 0; |
|
- | 1588 | } else { |
|
- | 1589 | throttle = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + Mixer.Motor[2][0]; |
|
1652 | signed int tmp_int; |
1590 | if (throttle < 0) throttle = 0; |
- | 1591 | tmp = throttle / Mixer.Motor[3][0]; |
|
1653 | if(Mixer.Motor[i][0] > 0) |
1592 | if (tmp > Mixer.Motor[3][1] * 2) tmp = Mixer.Motor[3][1] * 2; |
- | 1593 | } |
|
- | 1594 | servo[4] = tmp; |
|
- | 1595 | ||
1654 | { |
1596 | // pitch |
1655 | tmp_int = ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L; |
1597 | pitch = throttle / Mixer.Motor[4][0] + Mixer.Motor[4][2]; |
- | 1598 | if (pitch > Mixer.Motor[4][1]) pitch = Mixer.Motor[4][1]; |
|
- | 1599 | ||
- | 1600 | servo[0] = -pitch; |
|
- | 1601 | servo[1] = pitch; |
|
- | 1602 | servo[2] = pitch; |
|
- | 1603 | ||
- | 1604 | ||
- | 1605 | nick = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); |
|
- | 1606 | roll = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
|
- | 1607 | ||
- | 1608 | // nick |
|
1656 | tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L; |
1609 | nick /= Mixer.Motor[0][1]; |
1657 | tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L; |
1610 | nick -= PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] / Mixer.Motor[5][0]; |
1658 | tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
1611 | servo[0] += 127 + Mixer.Motor[0][0] - nick; |
1659 | tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]); // Filter |
1612 | servo[1] += 127 + Mixer.Motor[0][0] + nick; |
- | 1613 | servo[2] += 127 + Mixer.Motor[0][0] - nick; |
|
- | 1614 | ||
1660 | tmp_int = tmp_motorwert[i] / STICK_GAIN; |
1615 | // roll |
- | 1616 | roll /= Mixer.Motor[0][2]; |
|
- | 1617 | roll -= PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] / Mixer.Motor[6][0]; |
|
1661 | CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS); |
1618 | servo[0] += roll; |
- | 1619 | servo[1] += roll; |
|
1662 | Motor[i] = tmp_int; |
1620 | |
- | 1621 | // gier |
|
- | 1622 | tmp = MesswertGier / Mixer.Motor[0][3]; |
|
- | 1623 | servo[3] = 127 + Mixer.Motor[1][1] + tmp + (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] / Mixer.Motor[7][0]); |
|
- | 1624 | ||
1663 | } |
1625 | // not used |
- | 1626 | servo[5] = 127; |
|
- | 1627 | ||
- | 1628 | for(tmp = 0; tmp < 6; tmp++) { |
|
1664 | else Motor[i] = 0; |
1629 | servoValues[tmp] = MakeByte(servo[tmp]); |
1665 | } |
1630 | } |
1666 | /* |
1631 | |
1667 | if(Poti1 > 20) Motor1 = 0; |
1632 | DebugOut.Analog[12] = servo[0]; |
1668 | if(Poti1 > 90) Motor6 = 0; |
1633 | DebugOut.Analog[13] = servo[1]; |
- | 1634 | DebugOut.Analog[14] = servo[2]; |
|
- | 1635 | DebugOut.Analog[15] = servo[3]; |
|
1669 | if(Poti1 > 140) Motor2 = 0; |
1636 | DebugOut.Analog[6] = pitch; |
- | 1637 | DebugOut.Analog[7] = servo[4]; |
|
1670 | //if(Poti1 > 200) Motor7 = 0; |
1638 | |
- | 1639 | } |
|
1671 | */ |
1640 |