Rev 1744 | Rev 1765 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1744 | Rev 1760 | ||
---|---|---|---|
Line 147... | Line 147... | ||
147 | unsigned char Parameter_NaviWindCorrection; |
147 | unsigned char Parameter_NaviWindCorrection; |
148 | unsigned char Parameter_NaviSpeedCompensation; |
148 | unsigned char Parameter_NaviSpeedCompensation; |
149 | unsigned char Parameter_ExternalControl; |
149 | unsigned char Parameter_ExternalControl; |
150 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
150 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
151 | unsigned char CareFree = 0; |
151 | unsigned char CareFree = 0; |
- | 152 | const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; |
|
Line 152... | Line 153... | ||
152 | 153 | ||
153 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
154 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
154 | int MaxStickNick = 0,MaxStickRoll = 0; |
155 | int MaxStickNick = 0,MaxStickRoll = 0; |
155 | unsigned int modell_fliegt = 0; |
156 | unsigned int modell_fliegt = 0; |
Line 191... | Line 192... | ||
191 | // DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
192 | // DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
192 | DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
193 | DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
193 | DebugOut.Analog[30] = GPS_Nick; |
194 | DebugOut.Analog[30] = GPS_Nick; |
194 | DebugOut.Analog[31] = GPS_Roll; |
195 | DebugOut.Analog[31] = GPS_Roll; |
195 | if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe; |
196 | if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe; |
- | 197 | ||
- | 198 | if(Capacity.MinOfMaxPWM < 250/* && modell_fliegt > 500*/) { beeptime = 1000; DebugOut.Analog[25]++; } |
|
- | 199 | ||
196 | } |
200 | } |
Line -... | Line 201... | ||
- | 201 | ||
- | 202 | ||
197 | 203 | ||
198 | void Piep(unsigned char Anzahl, unsigned int dauer) |
204 | void Piep(unsigned char Anzahl, unsigned int dauer) |
199 | { |
205 | { |
200 | if(MotorenEin) return; //auf keinen Fall im Flug! |
206 | if(MotorenEin) return; //auf keinen Fall im Flug! |
201 | while(Anzahl--) |
207 | while(Anzahl--) |
Line 527... | Line 533... | ||
527 | { |
533 | { |
528 | if(!PC_MotortestActive) MotorTest[i] = 0; |
534 | if(!PC_MotortestActive) MotorTest[i] = 0; |
529 | Motor[i].SetPoint = MotorTest[i]; |
535 | Motor[i].SetPoint = MotorTest[i]; |
530 | Motor[i].SetPointLowerBits = 0; |
536 | Motor[i].SetPointLowerBits = 0; |
531 | /* |
537 | /* |
532 | Motor[i].SetPoint = MotorTest[i] / 4; // testing the high resolution |
538 | Motor[i].SetPoint = MotorTest[i] / 4; // testing the high resolution |
533 | Motor[i].SetPointLowerBits = MotorTest[i] % 4; |
539 | Motor[i].SetPointLowerBits = MotorTest[i] % 4; |
534 | */ |
540 | */ |
535 | } |
541 | } |
536 | if(PC_MotortestActive) PC_MotortestActive--; |
542 | if(PC_MotortestActive) PC_MotortestActive--; |
537 | } |
543 | } |
538 | else FCFlags |= FCFLAG_MOTOR_RUN; |
544 | else FCFlags |= FCFLAG_MOTOR_RUN; |
Line 539... | Line 545... | ||
539 | 545 | ||
540 | if(I2C_TransferActive) |
546 | if(I2C_TransferActive) |
541 | { |
- | |
542 | DebugOut.Analog[16]++; // transmission was not finished yet |
547 | { |
543 | // I2C_TransferActive = 0; // enable for the next time |
548 | I2C_TransferActive = 0; // enable for the next time |
544 | } |
549 | } |
545 | //else |
550 | else |
546 | { |
551 | { |
547 | motor_write = 0; |
- | |
548 | //Start I2C Interrupt Mode |
552 | motor_write = 0; |
549 | I2C_Start(TWI_STATE_MOTOR_TX); |
553 | I2C_Start(TWI_STATE_MOTOR_TX); //Start I2C Interrupt Mode |
550 | } |
554 | } |
Line 759... | Line 763... | ||
759 | else delay_neutral = 0; |
763 | else delay_neutral = 0; |
760 | } |
764 | } |
761 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
765 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
762 | // Gas ist unten |
766 | // Gas ist unten |
763 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
767 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | 768 | DebugOut.Analog[16] = EE_Parameter.MotorSafetySwitch; |
|
- | 769 | ||
764 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120) |
770 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120) |
765 | { |
771 | { |
766 | // Motoren Starten |
772 | // Motoren Starten |
767 | if(!MotorenEin) |
773 | if(!MotorenEin) |
768 | { |
774 | { |
769 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) |
775 | if((PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) && (PPM_in[EE_Parameter.MotorSafetySwitch] < -75 || EE_Parameter.MotorSafetySwitch == 0)) |
770 | { |
776 | { |
771 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
777 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
772 | // Einschalten |
778 | // Einschalten |
773 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
779 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
774 | if(++delay_einschalten > 200) |
780 | if(++delay_einschalten > 200) |
Line 801... | Line 807... | ||
801 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
807 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
802 | // Auschalten |
808 | // Auschalten |
803 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
809 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
804 | else // only if motors are running |
810 | else // only if motors are running |
805 | { |
811 | { |
806 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) |
812 | if((PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) && (PPM_in[EE_Parameter.MotorSafetySwitch] < -75 || EE_Parameter.MotorSafetySwitch == 0)) |
807 | { |
813 | { |
808 | if(++delay_ausschalten > 200) // nicht sofort |
814 | if(++delay_ausschalten > 200) // nicht sofort |
809 | { |
815 | { |
810 | MotorenEin = 0; |
816 | MotorenEin = 0; |
811 | delay_ausschalten = 0; |
817 | delay_ausschalten = 0; |
Line 830... | Line 836... | ||
830 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
836 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
Line 831... | Line 837... | ||
831 | 837 | ||
832 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
838 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
833 | // CareFree und freie Wahl der vorderen Richtung |
839 | // CareFree und freie Wahl der vorderen Richtung |
834 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
835 | signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; |
- | |
836 | 840 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
837 | if(CareFree) |
841 | if(CareFree) |
838 | { |
842 | { |
839 | signed int nick, roll; |
843 | signed int nick, roll; |
840 | nick = stick_nick / 4; |
844 | nick = stick_nick / 4; |
Line 848... | Line 852... | ||
848 | FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle]; |
852 | FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle]; |
849 | StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8; |
853 | StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8; |
850 | StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8; |
854 | StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8; |
851 | } |
855 | } |
Line 852... | Line -... | ||
852 | - | ||
853 | 856 | ||
854 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
857 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
855 | if(StickGier > 2) StickGier -= 2; else |
858 | if(StickGier > 2) StickGier -= 2; else |
Line 856... | Line 859... | ||
856 | if(StickGier < -2) StickGier += 2; else StickGier = 0; |
859 | if(StickGier < -2) StickGier += 2; else StickGier = 0; |
Line 1701... | Line 1704... | ||
1701 | else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
1704 | else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
Line 1702... | Line 1705... | ||
1702 | 1705 | ||
1703 | if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
1706 | if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
Line 1704... | Line 1707... | ||
1704 | else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
1707 | else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
1705 | 1708 | ||
1706 | LIMIT_MIN_MAX(tmp_int,MIN_GAS * 4,MAX_GAS * 4); |
1709 | LIMIT_MIN_MAX(tmp_int,(int) MIN_GAS * 4,(int) MAX_GAS * 4); |
1707 | Motor[i].SetPoint = tmp_int / 4; |
1710 | Motor[i].SetPoint = tmp_int / 4; |
1708 | Motor[i].SetPointLowerBits = (tmp_int % 4)<<1; // (3 bits total) |
1711 | Motor[i].SetPointLowerBits = (tmp_int % 4)<<1; // (3 bits total) |
1709 | tmp_motorwert[i] = tmp_int; |
1712 | tmp_motorwert[i] = tmp_int; |
1710 | } |
1713 | } |
1711 | else |
1714 | else |
1712 | { |
1715 | { |
1713 | Motor[i].SetPoint = 0; |
1716 | Motor[i].SetPoint = 0; |
1714 | Motor[i].SetPointLowerBits = 0; |
1717 | Motor[i].SetPointLowerBits = 0; |
1715 | } |
- | |
1716 | } |
1718 | } |