Rev 706 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 706 | Rev 707 | ||
---|---|---|---|
Line 113... | Line 113... | ||
113 | 113 | ||
114 | // setpoints for motors |
114 | // setpoints for motors |
Line 115... | Line 115... | ||
115 | volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; |
115 | volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; |
116 | 116 | ||
117 | // stick values derived by rc channels readings |
117 | // stick values derived by rc channels readings |
118 | int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickGas = 0; |
118 | int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickThrust = 0; |
Line 119... | Line 119... | ||
119 | // stick values derived by uart inputs |
119 | // stick values derived by uart inputs |
Line 415... | Line 415... | ||
415 | CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5,0,255); |
415 | CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5,0,255); |
416 | CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6,0,255); |
416 | CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6,0,255); |
417 | CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7,0,255); |
417 | CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7,0,255); |
418 | CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8,0,255); |
418 | CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8,0,255); |
419 | CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255); |
419 | CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255); |
420 | CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit,0,255); |
420 | CHK_POTI(FCParam.LoopThrustLimit,ParamSet.LoopThrustLimit,0,255); |
421 | CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255); |
421 | CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255); |
422 | CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255); |
422 | CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255); |
423 | CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255); |
423 | CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255); |
Line 424... | Line 424... | ||
424 | 424 | ||
Line 430... | Line 430... | ||
430 | /* MotorControl */ |
430 | /* MotorControl */ |
431 | /************************************************************************/ |
431 | /************************************************************************/ |
432 | void MotorRegler(void) |
432 | void MotorRegler(void) |
433 | { |
433 | { |
434 | int16_t MotorValue, pd_ergebnis, h, tmp_int; |
434 | int16_t MotorValue, pd_ergebnis, h, tmp_int; |
435 | int16_t YawMixingFraction, GasMixingFraction; |
435 | int16_t YawMixingFraction, ThrustMixingFraction; |
436 | static int32_t SumPitch = 0, SumRoll = 0; |
436 | static int32_t SumPitch = 0, SumRoll = 0; |
437 | static int32_t SetPointYaw = 0, tmp_long, tmp_long2; |
437 | static int32_t SetPointYaw = 0, tmp_long, tmp_long2; |
438 | static int32_t IntegralErrorPitch = 0; |
438 | static int32_t IntegralErrorPitch = 0; |
439 | static int32_t IntegralErrorRoll = 0; |
439 | static int32_t IntegralErrorRoll = 0; |
440 | static uint16_t RcLostTimer; |
440 | static uint16_t RcLostTimer; |
Line 449... | Line 449... | ||
449 | 449 | ||
Line 450... | Line 450... | ||
450 | Mean(); |
450 | Mean(); |
451 | 451 | ||
452 | GRN_ON; |
452 | GRN_ON; |
453 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
453 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
454 | // determine GAS value |
454 | // determine thrust value |
455 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
455 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
456 | GasMixingFraction = StickGas; |
456 | ThrustMixingFraction = StickThrust; |
457 | if(GasMixingFraction < 0) GasMixingFraction = 0; |
457 | if(ThrustMixingFraction < 0) ThrustMixingFraction = 0; |
458 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
458 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
459 | // RC-signal is bad |
459 | // RC-signal is bad |
460 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
460 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 480... | Line 480... | ||
480 | EmergencyLanding = 0; // emergency landing is over |
480 | EmergencyLanding = 0; // emergency landing is over |
481 | } |
481 | } |
482 | ROT_ON; // set red led |
482 | ROT_ON; // set red led |
483 | if(Modell_Is_Flying > 2000) // wahrscheinlich in der Luft --> langsam absenken |
483 | if(Modell_Is_Flying > 2000) // wahrscheinlich in der Luft --> langsam absenken |
484 | { |
484 | { |
485 | GasMixingFraction = ParamSet.EmergencyGas; // set emergency gas |
485 | ThrustMixingFraction = ParamSet.EmergencyThrust; // set emergency thrust |
486 | EmergencyLanding = 1; // enable emergency landing |
486 | EmergencyLanding = 1; // enable emergency landing |
487 | // set neutral rc inputs |
487 | // set neutral rc inputs |
488 | PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
488 | PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
489 | PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
489 | PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
490 | PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
490 | PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
Line 499... | Line 499... | ||
499 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
499 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
500 | if(SenderOkay > 140) |
500 | if(SenderOkay > 140) |
501 | { |
501 | { |
502 | EmergencyLanding = 0; // switch off emergency landing if RC-signal is okay |
502 | EmergencyLanding = 0; // switch off emergency landing if RC-signal is okay |
503 | // reset emergency timer |
503 | // reset emergency timer |
504 | RcLostTimer = ParamSet.EmergencyGasDuration * 50; |
504 | RcLostTimer = ParamSet.EmergencyThrustDuration * 50; |
505 | if(GasMixingFraction > 40) |
505 | if(ThrustMixingFraction > 40) |
506 | { |
506 | { |
507 | if(Modell_Is_Flying < 0xFFFF) Modell_Is_Flying++; |
507 | if(Modell_Is_Flying < 0xFFFF) Modell_Is_Flying++; |
508 | } |
508 | } |
509 | if((Modell_Is_Flying < 200) || (GasMixingFraction < 40)) |
509 | if((Modell_Is_Flying < 200) || (ThrustMixingFraction < 40)) |
510 | { |
510 | { |
511 | SumPitch = 0; |
511 | SumPitch = 0; |
512 | SumRoll = 0; |
512 | SumRoll = 0; |
513 | Reading_IntegralYaw = 0; |
513 | Reading_IntegralYaw = 0; |
514 | Reading_IntegralYaw2 = 0; |
514 | Reading_IntegralYaw2 = 0; |
515 | } |
515 | } |
516 | // if motors are off and the thrust stick is in the upper position |
516 | // if motors are off and the thrust stick is in the upper position |
517 | if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && MotorsOn == 0) |
517 | if((PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] > 80) && MotorsOn == 0) |
518 | { |
518 | { |
519 | // and if the yaw stick is in the leftmost position |
519 | // and if the yaw stick is in the leftmost position |
520 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
520 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
521 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
521 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
522 | // calibrate the neutral readings of all attitude sensors |
522 | // calibrate the neutral readings of all attitude sensors |
Line 579... | Line 579... | ||
579 | else delay_neutral = 0; |
579 | else delay_neutral = 0; |
580 | } |
580 | } |
581 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
581 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
582 | // thrust stick is down |
582 | // thrust stick is down |
583 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
583 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
584 | if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < -85) |
584 | if(PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < -85) |
585 | { |
585 | { |
586 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
586 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
587 | // and yaw stick is rightmost --> start motors |
587 | // and yaw stick is rightmost --> start motors |
588 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
588 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
589 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
589 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
Line 633... | Line 633... | ||
633 | StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4; |
633 | StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4; |
634 | StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D; |
634 | StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D; |
635 | StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4; |
635 | StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4; |
636 | StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D; |
636 | StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D; |
Line 637... | Line 637... | ||
637 | 637 | ||
638 | // direct mapping of yaw and gas |
638 | // direct mapping of yaw and thrust |
639 | StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; |
639 | StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; |
Line 640... | Line 640... | ||
640 | StickGas = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;// shift to positive numbers |
640 | StickThrust = PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] + 120;// shift to positive numbers |
641 | 641 | ||
642 | // update max stick positions for pitch and roll |
642 | // update max stick positions for pitch and roll |
643 | if(abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) > MaxStickPitch) |
643 | if(abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) > MaxStickPitch) |
Line 684... | Line 684... | ||
684 | { |
684 | { |
685 | StickPitch += (int16_t) ExternControl.Pitch * (int16_t) ParamSet.Stick_P; |
685 | StickPitch += (int16_t) ExternControl.Pitch * (int16_t) ParamSet.Stick_P; |
686 | StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P; |
686 | StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P; |
687 | StickYaw += ExternControl.Yaw; |
687 | StickYaw += ExternControl.Yaw; |
688 | ExternHightValue = (int16_t) ExternControl.Hight * (int16_t)ParamSet.Hight_Gain; |
688 | ExternHightValue = (int16_t) ExternControl.Hight * (int16_t)ParamSet.Hight_Gain; |
689 | if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
689 | if(ExternControl.Thrust < StickThrust) StickThrust = ExternControl.Thrust; |
690 | } |
690 | } |
691 | // disable I part of gyro control feedback |
691 | // disable I part of gyro control feedback |
692 | if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor = 0; |
692 | if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor = 0; |
693 | // avoid negative scaling factors |
693 | // avoid negative scaling factors |
694 | if(Gyro_P_Factor < 0) Gyro_P_Factor = 0; |
694 | if(Gyro_P_Factor < 0) Gyro_P_Factor = 0; |
Line 731... | Line 731... | ||
731 | } |
731 | } |
732 | } |
732 | } |
Line 733... | Line 733... | ||
733 | 733 | ||
734 | if(Looping_Left || Looping_Right) Looping_Roll = 1; else Looping_Roll = 0; |
734 | if(Looping_Left || Looping_Right) Looping_Roll = 1; else Looping_Roll = 0; |
735 | if(Looping_Top || Looping_Down) {Looping_Pitch = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Pitch = 0; |
735 | if(Looping_Top || Looping_Down) {Looping_Pitch = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Pitch = 0; |
Line 736... | Line 736... | ||
736 | } // End of new RC values |
736 | } // End of new RC-Values or Emergency Landing |
737 | 737 | ||
738 | 738 | ||
739 | if(Looping_Roll) BeepTime = 100; |
739 | if(Looping_Roll) BeepTime = 100; |
740 | if(Looping_Roll || Looping_Pitch) |
740 | if(Looping_Roll || Looping_Pitch) |
Line 741... | Line 741... | ||
741 | { |
741 | { |
742 | if(GasMixingFraction > ParamSet.LoopGasLimit) GasMixingFraction = ParamSet.LoopGasLimit; |
742 | if(ThrustMixingFraction > ParamSet.LoopThrustLimit) ThrustMixingFraction = ParamSet.LoopThrustLimit; |
743 | } |
743 | } |
744 | - | ||
- | 744 | ||
745 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
745 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
746 | // in case of emergency landing |
746 | // in case of emergency landing |
747 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
747 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
748 | 748 | // set all inputs to save values |
|
749 | if(EmergencyLanding) |
749 | if(EmergencyLanding) |
Line 802... | Line 802... | ||
802 | { |
802 | { |
803 | tmp_long /= 3; |
803 | tmp_long /= 3; |
804 | tmp_long2 /= 3; |
804 | tmp_long2 /= 3; |
805 | } |
805 | } |
Line 806... | Line 806... | ||
806 | 806 | ||
807 | #define AUSGLEICH 32 |
807 | #define BALANCE 32 |
808 | if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
808 | if(tmp_long > BALANCE) tmp_long = BALANCE; |
809 | if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
809 | if(tmp_long < -BALANCE) tmp_long =-BALANCE; |
810 | if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
810 | if(tmp_long2 > BALANCE) tmp_long2 = BALANCE; |
Line 811... | Line 811... | ||
811 | if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
811 | if(tmp_long2 <-BALANCE) tmp_long2 =-BALANCE; |
812 | 812 | ||
813 | Reading_IntegralPitch -= tmp_long; |
813 | Reading_IntegralPitch -= tmp_long; |
814 | Reading_IntegralRoll -= tmp_long2; |
814 | Reading_IntegralRoll -= tmp_long2; |
Line 1049... | Line 1049... | ||
1049 | // DebugOut.Analog[10] = Reading_IntegralYaw / 128; |
1049 | // DebugOut.Analog[10] = Reading_IntegralYaw / 128; |
1050 | // DebugOut.Analog[11] = CompassCourse; |
1050 | // DebugOut.Analog[11] = CompassCourse; |
1051 | // DebugOut.Analog[10] = FCParam.Gyro_I; |
1051 | // DebugOut.Analog[10] = FCParam.Gyro_I; |
1052 | // DebugOut.Analog[10] = ParamSet.Gyro_I; |
1052 | // DebugOut.Analog[10] = ParamSet.Gyro_I; |
1053 | // DebugOut.Analog[9] = CompassOffCourse; |
1053 | // DebugOut.Analog[9] = CompassOffCourse; |
1054 | // DebugOut.Analog[10] = GasMixingFraction; |
1054 | // DebugOut.Analog[10] = ThrustMixingFraction; |
1055 | // DebugOut.Analog[3] = HightD * 32; |
1055 | // DebugOut.Analog[3] = HightD * 32; |
1056 | // DebugOut.Analog[4] = hoehenregler; |
1056 | // DebugOut.Analog[4] = hoehenregler; |
1057 | } |
1057 | } |
Line 1058... | Line 1058... | ||
1058 | 1058 | ||
Line 1108... | Line 1108... | ||
1108 | 1108 | ||
1109 | if(EmergencyLanding) SetPointHight = 0; |
1109 | if(EmergencyLanding) SetPointHight = 0; |
1110 | h = ReadingHight; |
1110 | h = ReadingHight; |
1111 | if((h > SetPointHight) && HightControlActive) // zu hoch --> drosseln |
1111 | if((h > SetPointHight) && HightControlActive) // zu hoch --> drosseln |
1112 | { h = ((h - SetPointHight) * (int) FCParam.Hight_P) / 16; // Differenz bestimmen --> P-Anteil |
1112 | { h = ((h - SetPointHight) * (int) FCParam.Hight_P) / 16; // Differenz bestimmen --> P-Anteil |
1113 | h = GasMixingFraction - h; // vom Gas abziehen |
1113 | h = ThrustMixingFraction - h; // vom Gas abziehen |
1114 | h -= (HightD * FCParam.AirPressure_D)/8; // D-Anteil |
1114 | h -= (HightD * FCParam.AirPressure_D)/8; // D-Anteil |
1115 | tmp_int = ((Reading_Integral_Top / 512) * (signed long) FCParam.Hight_ACC_Effect) / 32; |
1115 | tmp_int = ((Reading_Integral_Top / 512) * (signed long) FCParam.Hight_ACC_Effect) / 32; |
1116 | if(tmp_int > 50) tmp_int = 50; |
1116 | if(tmp_int > 50) tmp_int = 50; |
1117 | else if(tmp_int < -50) tmp_int = -50; |
1117 | else if(tmp_int < -50) tmp_int = -50; |
1118 | h -= tmp_int; |
1118 | h -= tmp_int; |
1119 | hoehenregler = (hoehenregler*15 + h) / 16; |
1119 | hoehenregler = (hoehenregler*15 + h) / 16; |
1120 | if(hoehenregler < ParamSet.Hight_MinGas) // nicht unter MIN |
1120 | if(hoehenregler < ParamSet.Hight_MinThrust) // nicht unter MIN |
1121 | { |
1121 | { |
1122 | if(GasMixingFraction >= ParamSet.Hight_MinGas) hoehenregler = ParamSet.Hight_MinGas; |
1122 | if(ThrustMixingFraction >= ParamSet.Hight_MinThrust) hoehenregler = ParamSet.Hight_MinThrust; |
1123 | if(GasMixingFraction < ParamSet.Hight_MinGas) hoehenregler = GasMixingFraction; |
1123 | if(ThrustMixingFraction < ParamSet.Hight_MinThrust) hoehenregler = ThrustMixingFraction; |
1124 | } |
1124 | } |
1125 | if(hoehenregler > GasMixingFraction) hoehenregler = GasMixingFraction; // nicht mehr als Gas |
1125 | if(hoehenregler > ThrustMixingFraction) hoehenregler = ThrustMixingFraction; // nicht mehr als Gas |
1126 | GasMixingFraction = hoehenregler; |
1126 | ThrustMixingFraction = hoehenregler; |
1127 | } |
1127 | } |
1128 | } |
1128 | } |
1129 | if(GasMixingFraction > ParamSet.Gas_Max - 20) GasMixingFraction = ParamSet.Gas_Max - 20; |
1129 | if(ThrustMixingFraction > ParamSet.Trust_Max - 20) ThrustMixingFraction = ParamSet.Trust_Max - 20; |
1130 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1130 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1131 | // + Mischer und PI-Regler |
1131 | // + Mischer und PI-Regler |
1132 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1132 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1133 | DebugOut.Analog[7] = GasMixingFraction; |
1133 | DebugOut.Analog[7] = ThrustMixingFraction; |
1134 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1134 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1135 | // Gier-Anteil |
1135 | // Gier-Anteil |
1136 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1136 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1137 | #define MUL_G 1.0 |
1137 | #define MUL_G 1.0 |
1138 | YawMixingFraction = ReadingYaw - SetPointYaw; // Regler für Gier |
1138 | YawMixingFraction = ReadingYaw - SetPointYaw; // Regler für Gier |
Line 1139... | Line 1139... | ||
1139 | // YawMixingFraction = 0; |
1139 | // YawMixingFraction = 0; |
1140 | 1140 | ||
1141 | if(YawMixingFraction > (GasMixingFraction / 2)) YawMixingFraction = GasMixingFraction / 2; |
1141 | if(YawMixingFraction > (ThrustMixingFraction / 2)) YawMixingFraction = ThrustMixingFraction / 2; |
1142 | if(YawMixingFraction < -(GasMixingFraction / 2)) YawMixingFraction = -(GasMixingFraction / 2); |
1142 | if(YawMixingFraction < -(ThrustMixingFraction / 2)) YawMixingFraction = -(ThrustMixingFraction / 2); |
Line 1143... | Line 1143... | ||
1143 | if(YawMixingFraction > ((ParamSet.Gas_Max - GasMixingFraction))) YawMixingFraction = ((ParamSet.Gas_Max - GasMixingFraction)); |
1143 | if(YawMixingFraction > ((ParamSet.Trust_Max - ThrustMixingFraction))) YawMixingFraction = ((ParamSet.Trust_Max - ThrustMixingFraction)); |
1144 | if(YawMixingFraction < -((ParamSet.Gas_Max - GasMixingFraction))) YawMixingFraction = -((ParamSet.Gas_Max - GasMixingFraction)); |
1144 | if(YawMixingFraction < -((ParamSet.Trust_Max - ThrustMixingFraction))) YawMixingFraction = -((ParamSet.Trust_Max - ThrustMixingFraction)); |
1145 | 1145 | ||
1146 | if(GasMixingFraction < 20) YawMixingFraction = 0; |
1146 | if(ThrustMixingFraction < 20) YawMixingFraction = 0; |
1147 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1147 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1148 | // Pitch-Achse |
1148 | // Pitch-Achse |
1149 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1149 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1150 | DiffPitch = ReadingPitch - (StickPitch - GPS_Pitch); // Differenz bestimmen |
1150 | DiffPitch = ReadingPitch - (StickPitch - GPS_Pitch); // Differenz bestimmen |
1151 | if(Gyro_I_Factor) SumPitch += IntegralPitch * Gyro_I_Factor - (StickPitch - GPS_Pitch); // I-Anteil bei Winkelregelung |
1151 | if(Gyro_I_Factor) SumPitch += IntegralPitch * Gyro_I_Factor - (StickPitch - GPS_Pitch); // I-Anteil bei Winkelregelung |
1152 | else SumPitch += DiffPitch; // I-Anteil bei HH |
1152 | else SumPitch += DiffPitch; // I-Anteil bei HH |
1153 | if(SumPitch > 16000) SumPitch = 16000; |
1153 | if(SumPitch > 16000) SumPitch = 16000; |
1154 | if(SumPitch < -16000) SumPitch = -16000; |
1154 | if(SumPitch < -16000) SumPitch = -16000; |
1155 | pd_ergebnis = DiffPitch + Ki * SumPitch; // PI-Regler für Pitch |
1155 | pd_ergebnis = DiffPitch + Ki * SumPitch; // PI-Regler für Pitch |
1156 | // Motor Vorn |
1156 | // Motor Vorn |
Line 1157... | Line 1157... | ||
1157 | tmp_int = (long)((long)FCParam.DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64; |
1157 | tmp_int = (long)((long)FCParam.DynamicStability * (long)(ThrustMixingFraction + abs(YawMixingFraction)/2)) / 64; |
1158 | if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
1158 | if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
1159 | if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
1159 | if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
1160 | 1160 | ||
1161 | MotorValue = GasMixingFraction + pd_ergebnis + YawMixingFraction; // Mischer |
1161 | MotorValue = ThrustMixingFraction + pd_ergebnis + YawMixingFraction; // Mischer |
1162 | if ((MotorValue < 0)) MotorValue = 0; |
1162 | if ((MotorValue < 0)) MotorValue = 0; |
1163 | else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
1163 | else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
1164 | if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
1164 | if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
1165 | Motor_Front = MotorValue; |
1165 | Motor_Front = MotorValue; |
1166 | // Motor Heck |
1166 | // Motor Heck |
1167 | MotorValue = GasMixingFraction - pd_ergebnis + YawMixingFraction; |
1167 | MotorValue = ThrustMixingFraction - pd_ergebnis + YawMixingFraction; |
1168 | if ((MotorValue < 0)) MotorValue = 0; |
1168 | if ((MotorValue < 0)) MotorValue = 0; |
1169 | else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
1169 | else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
1170 | if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
1170 | if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
1171 | Motor_Rear = MotorValue; |
1171 | Motor_Rear = MotorValue; |
1172 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1172 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1173 | // Roll-Achse |
1173 | // Roll-Achse |
1174 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1174 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1175 | DiffRoll = ReadingRoll - (StickRoll - GPS_Roll); // Differenz bestimmen |
1175 | DiffRoll = ReadingRoll - (StickRoll - GPS_Roll); // Differenz bestimmen |
1176 | if(Gyro_I_Factor) SumRoll += IntegralRoll * Gyro_I_Factor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
1176 | if(Gyro_I_Factor) SumRoll += IntegralRoll * Gyro_I_Factor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
1177 | else SumRoll += DiffRoll; // I-Anteil bei HH |
1177 | else SumRoll += DiffRoll; // I-Anteil bei HH |
1178 | if(SumRoll > 16000) SumRoll = 16000; |
1178 | if(SumRoll > 16000) SumRoll = 16000; |
1179 | if(SumRoll < -16000) SumRoll = -16000; |
1179 | if(SumRoll < -16000) SumRoll = -16000; |
1180 | pd_ergebnis = DiffRoll + Ki * SumRoll; // PI-Regler für Roll |
1180 | pd_ergebnis = DiffRoll + Ki * SumRoll; // PI-Regler für Roll |
1181 | tmp_int = (long)((long)FCParam.DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64; |
1181 | tmp_int = (long)((long)FCParam.DynamicStability * (long)(ThrustMixingFraction + abs(YawMixingFraction)/2)) / 64; |
1182 | if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
1182 | if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
Line 1183... | Line 1183... | ||
1183 | if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
1183 | if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
1184 | // Motor Links |
1184 | // Motor Links |
1185 | MotorValue = GasMixingFraction + pd_ergebnis - YawMixingFraction; |
1185 | MotorValue = ThrustMixingFraction + pd_ergebnis - YawMixingFraction; |
1186 | #define GRENZE Poti1 |
1186 | #define GRENZE Poti1 |
1187 | 1187 | ||
1188 | if ((MotorValue < 0)) MotorValue = 0; |
1188 | if ((MotorValue < 0)) MotorValue = 0; |
Line 1189... | Line 1189... | ||
1189 | else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
1189 | else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
1190 | if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
1190 | if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
1191 | Motor_Left = MotorValue; |
1191 | Motor_Left = MotorValue; |
1192 | // Motor Rechts |
1192 | // Motor Rechts |
1193 | MotorValue = GasMixingFraction - pd_ergebnis - YawMixingFraction; |
1193 | MotorValue = ThrustMixingFraction - pd_ergebnis - YawMixingFraction; |
1194 | 1194 |