| 115,7 → 115,7 |
|---|
| volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; |
| |
| // stick values derived by rc channels readings |
| int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickGas = 0; |
| int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickThrust = 0; |
| // stick values derived by uart inputs |
| int16_t ExternStickPitch = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHightValue = -20; |
| |
| 417,7 → 417,7 |
|---|
| CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7,0,255); |
| CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8,0,255); |
| CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255); |
| CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit,0,255); |
| CHK_POTI(FCParam.LoopThrustLimit,ParamSet.LoopThrustLimit,0,255); |
| CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255); |
| CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255); |
| CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255); |
| 432,7 → 432,7 |
|---|
| void MotorRegler(void) |
| { |
| int16_t MotorValue, pd_ergebnis, h, tmp_int; |
| int16_t YawMixingFraction, GasMixingFraction; |
| int16_t YawMixingFraction, ThrustMixingFraction; |
| static int32_t SumPitch = 0, SumRoll = 0; |
| static int32_t SetPointYaw = 0, tmp_long, tmp_long2; |
| static int32_t IntegralErrorPitch = 0; |
| 451,10 → 451,10 |
|---|
| |
| GRN_ON; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // determine GAS value |
| // determine thrust value |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| GasMixingFraction = StickGas; |
| if(GasMixingFraction < 0) GasMixingFraction = 0; |
| ThrustMixingFraction = StickThrust; |
| if(ThrustMixingFraction < 0) ThrustMixingFraction = 0; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // RC-signal is bad |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 482,7 → 482,7 |
|---|
| ROT_ON; // set red led |
| if(Modell_Is_Flying > 2000) // wahrscheinlich in der Luft --> langsam absenken |
| { |
| GasMixingFraction = ParamSet.EmergencyGas; // set emergency gas |
| ThrustMixingFraction = ParamSet.EmergencyThrust; // set emergency thrust |
| EmergencyLanding = 1; // enable emergency landing |
| // set neutral rc inputs |
| PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
| 501,12 → 501,12 |
|---|
| { |
| EmergencyLanding = 0; // switch off emergency landing if RC-signal is okay |
| // reset emergency timer |
| RcLostTimer = ParamSet.EmergencyGasDuration * 50; |
| if(GasMixingFraction > 40) |
| RcLostTimer = ParamSet.EmergencyThrustDuration * 50; |
| if(ThrustMixingFraction > 40) |
| { |
| if(Modell_Is_Flying < 0xFFFF) Modell_Is_Flying++; |
| } |
| if((Modell_Is_Flying < 200) || (GasMixingFraction < 40)) |
| if((Modell_Is_Flying < 200) || (ThrustMixingFraction < 40)) |
| { |
| SumPitch = 0; |
| SumRoll = 0; |
| 514,7 → 514,7 |
|---|
| Reading_IntegralYaw2 = 0; |
| } |
| // if motors are off and the thrust stick is in the upper position |
| if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && MotorsOn == 0) |
| if((PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] > 80) && MotorsOn == 0) |
| { |
| // and if the yaw stick is in the leftmost position |
| if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
| 581,7 → 581,7 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // thrust stick is down |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < -85) |
| if(PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < -85) |
| { |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // and yaw stick is rightmost --> start motors |
| 635,9 → 635,9 |
|---|
| StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4; |
| StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D; |
| |
| // direct mapping of yaw and gas |
| // direct mapping of yaw and thrust |
| StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; |
| StickGas = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;// shift to positive numbers |
| StickThrust = PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] + 120;// shift to positive numbers |
| |
| // update max stick positions for pitch and roll |
| if(abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) > MaxStickPitch) |
| 686,7 → 686,7 |
|---|
| StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P; |
| StickYaw += ExternControl.Yaw; |
| ExternHightValue = (int16_t) ExternControl.Hight * (int16_t)ParamSet.Hight_Gain; |
| if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
| if(ExternControl.Thrust < StickThrust) StickThrust = ExternControl.Thrust; |
| } |
| // disable I part of gyro control feedback |
| if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor = 0; |
| 733,19 → 733,19 |
|---|
| |
| if(Looping_Left || Looping_Right) Looping_Roll = 1; else Looping_Roll = 0; |
| if(Looping_Top || Looping_Down) {Looping_Pitch = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Pitch = 0; |
| } // End of new RC values |
| } // End of new RC-Values or Emergency Landing |
| |
| |
| if(Looping_Roll) BeepTime = 100; |
| if(Looping_Roll || Looping_Pitch) |
| { |
| if(GasMixingFraction > ParamSet.LoopGasLimit) GasMixingFraction = ParamSet.LoopGasLimit; |
| if(ThrustMixingFraction > ParamSet.LoopThrustLimit) ThrustMixingFraction = ParamSet.LoopThrustLimit; |
| } |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // in case of emergency landing |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| |
| // set all inputs to save values |
| if(EmergencyLanding) |
| { |
| StickYaw = 0; |
| 804,11 → 804,11 |
|---|
| tmp_long2 /= 3; |
| } |
| |
| #define AUSGLEICH 32 |
| if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
| if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
| if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
| if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
| #define BALANCE 32 |
| if(tmp_long > BALANCE) tmp_long = BALANCE; |
| if(tmp_long < -BALANCE) tmp_long =-BALANCE; |
| if(tmp_long2 > BALANCE) tmp_long2 = BALANCE; |
| if(tmp_long2 <-BALANCE) tmp_long2 =-BALANCE; |
| |
| Reading_IntegralPitch -= tmp_long; |
| Reading_IntegralRoll -= tmp_long2; |
| 1051,7 → 1051,7 |
|---|
| // DebugOut.Analog[10] = FCParam.Gyro_I; |
| // DebugOut.Analog[10] = ParamSet.Gyro_I; |
| // DebugOut.Analog[9] = CompassOffCourse; |
| // DebugOut.Analog[10] = GasMixingFraction; |
| // DebugOut.Analog[10] = ThrustMixingFraction; |
| // DebugOut.Analog[3] = HightD * 32; |
| // DebugOut.Analog[4] = hoehenregler; |
| } |
| 1110,7 → 1110,7 |
|---|
| h = ReadingHight; |
| if((h > SetPointHight) && HightControlActive) // zu hoch --> drosseln |
| { h = ((h - SetPointHight) * (int) FCParam.Hight_P) / 16; // Differenz bestimmen --> P-Anteil |
| h = GasMixingFraction - h; // vom Gas abziehen |
| h = ThrustMixingFraction - h; // vom Gas abziehen |
| h -= (HightD * FCParam.AirPressure_D)/8; // D-Anteil |
| tmp_int = ((Reading_Integral_Top / 512) * (signed long) FCParam.Hight_ACC_Effect) / 32; |
| if(tmp_int > 50) tmp_int = 50; |
| 1117,20 → 1117,20 |
|---|
| else if(tmp_int < -50) tmp_int = -50; |
| h -= tmp_int; |
| hoehenregler = (hoehenregler*15 + h) / 16; |
| if(hoehenregler < ParamSet.Hight_MinGas) // nicht unter MIN |
| if(hoehenregler < ParamSet.Hight_MinThrust) // nicht unter MIN |
| { |
| if(GasMixingFraction >= ParamSet.Hight_MinGas) hoehenregler = ParamSet.Hight_MinGas; |
| if(GasMixingFraction < ParamSet.Hight_MinGas) hoehenregler = GasMixingFraction; |
| if(ThrustMixingFraction >= ParamSet.Hight_MinThrust) hoehenregler = ParamSet.Hight_MinThrust; |
| if(ThrustMixingFraction < ParamSet.Hight_MinThrust) hoehenregler = ThrustMixingFraction; |
| } |
| if(hoehenregler > GasMixingFraction) hoehenregler = GasMixingFraction; // nicht mehr als Gas |
| GasMixingFraction = hoehenregler; |
| if(hoehenregler > ThrustMixingFraction) hoehenregler = ThrustMixingFraction; // nicht mehr als Gas |
| ThrustMixingFraction = hoehenregler; |
| } |
| } |
| if(GasMixingFraction > ParamSet.Gas_Max - 20) GasMixingFraction = ParamSet.Gas_Max - 20; |
| if(ThrustMixingFraction > ParamSet.Trust_Max - 20) ThrustMixingFraction = ParamSet.Trust_Max - 20; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Mischer und PI-Regler |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| DebugOut.Analog[7] = GasMixingFraction; |
| DebugOut.Analog[7] = ThrustMixingFraction; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Gier-Anteil |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1138,12 → 1138,12 |
|---|
| YawMixingFraction = ReadingYaw - SetPointYaw; // Regler für Gier |
| // YawMixingFraction = 0; |
| |
| if(YawMixingFraction > (GasMixingFraction / 2)) YawMixingFraction = GasMixingFraction / 2; |
| if(YawMixingFraction < -(GasMixingFraction / 2)) YawMixingFraction = -(GasMixingFraction / 2); |
| if(YawMixingFraction > ((ParamSet.Gas_Max - GasMixingFraction))) YawMixingFraction = ((ParamSet.Gas_Max - GasMixingFraction)); |
| if(YawMixingFraction < -((ParamSet.Gas_Max - GasMixingFraction))) YawMixingFraction = -((ParamSet.Gas_Max - GasMixingFraction)); |
| if(YawMixingFraction > (ThrustMixingFraction / 2)) YawMixingFraction = ThrustMixingFraction / 2; |
| if(YawMixingFraction < -(ThrustMixingFraction / 2)) YawMixingFraction = -(ThrustMixingFraction / 2); |
| if(YawMixingFraction > ((ParamSet.Trust_Max - ThrustMixingFraction))) YawMixingFraction = ((ParamSet.Trust_Max - ThrustMixingFraction)); |
| if(YawMixingFraction < -((ParamSet.Trust_Max - ThrustMixingFraction))) YawMixingFraction = -((ParamSet.Trust_Max - ThrustMixingFraction)); |
| |
| if(GasMixingFraction < 20) YawMixingFraction = 0; |
| if(ThrustMixingFraction < 20) YawMixingFraction = 0; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Pitch-Achse |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1154,20 → 1154,20 |
|---|
| if(SumPitch < -16000) SumPitch = -16000; |
| pd_ergebnis = DiffPitch + Ki * SumPitch; // PI-Regler für Pitch |
| // Motor Vorn |
| tmp_int = (long)((long)FCParam.DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64; |
| tmp_int = (long)((long)FCParam.DynamicStability * (long)(ThrustMixingFraction + abs(YawMixingFraction)/2)) / 64; |
| if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
| if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
| |
| MotorValue = GasMixingFraction + pd_ergebnis + YawMixingFraction; // Mischer |
| MotorValue = ThrustMixingFraction + pd_ergebnis + YawMixingFraction; // Mischer |
| if ((MotorValue < 0)) MotorValue = 0; |
| else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
| if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
| else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
| if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
| Motor_Front = MotorValue; |
| // Motor Heck |
| MotorValue = GasMixingFraction - pd_ergebnis + YawMixingFraction; |
| MotorValue = ThrustMixingFraction - pd_ergebnis + YawMixingFraction; |
| if ((MotorValue < 0)) MotorValue = 0; |
| else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
| if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
| else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
| if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
| Motor_Rear = MotorValue; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Roll-Achse |
| 1178,23 → 1178,23 |
|---|
| if(SumRoll > 16000) SumRoll = 16000; |
| if(SumRoll < -16000) SumRoll = -16000; |
| pd_ergebnis = DiffRoll + Ki * SumRoll; // PI-Regler für Roll |
| tmp_int = (long)((long)FCParam.DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64; |
| tmp_int = (long)((long)FCParam.DynamicStability * (long)(ThrustMixingFraction + abs(YawMixingFraction)/2)) / 64; |
| if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
| if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
| // Motor Links |
| MotorValue = GasMixingFraction + pd_ergebnis - YawMixingFraction; |
| MotorValue = ThrustMixingFraction + pd_ergebnis - YawMixingFraction; |
| #define GRENZE Poti1 |
| |
| if ((MotorValue < 0)) MotorValue = 0; |
| else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
| if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
| else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
| if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
| Motor_Left = MotorValue; |
| // Motor Rechts |
| MotorValue = GasMixingFraction - pd_ergebnis - YawMixingFraction; |
| MotorValue = ThrustMixingFraction - pd_ergebnis - YawMixingFraction; |
| |
| if ((MotorValue < 0)) MotorValue = 0; |
| else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
| if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
| else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
| if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
| Motor_Right = MotorValue; |
| // +++++++++++++++++++++++++++++++++++++++++++++++ |
| } |