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; |
// +++++++++++++++++++++++++++++++++++++++++++++++ |
} |