1481,7 → 1481,7 |
LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t |
HeightDeviation = (int)(tmp_long); // positive when too high |
tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 16L; // p-part |
LIMIT_MIN_MAX(tmp_long, -255 * STICK_GAIN, 255 * STICK_GAIN); // more than 2 times the full range makes no sense |
LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 255 * STICK_GAIN); // more than the full range makes no sense |
HCGas -= tmp_long; |
// ------------------------- D-Part 1: Vario Meter ---------------------------- |
tmp_int = VarioMeter / 8; |
1502,11 → 1502,10 |
tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
HCGas -= tmp_int; |
|
// limit deviation from hoover point within the target region |
if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
{ |
unsigned int tmp,min,max; |
signed int tmp,min,max; |
if(abs(HeightDeviation) < 60) |
{ |
LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point |
1515,12 → 1514,16 |
{ |
tmp = (abs(HeightDeviation) - 60)/32; |
if(tmp > 15) tmp = 15; |
max = (HoverGasMax * (tmp + 16)) / 16; |
min = (HoverGasMin * (16 - tmp)) / 16; |
LIMIT_MIN_MAX(HCGas, min, max); // limit gas around the hoover point |
DebugOut.Analog[16] = min; |
DebugOut.Analog[17] = max; |
DebugOut.Analog[18] = SollHoehe; |
if(HeightDeviation > 0) |
{ |
min = (HoverGasMin * (16 - tmp)) / 16; |
LIMIT_MIN_MAX(HCGas, min, HoverGasMax); // limit gas around the hoover point |
} |
else |
{ |
max = (HoverGasMax * (tmp + 16)) / 16; |
LIMIT_MIN_MAX(HCGas, HoverGasMin, max); // limit gas around the hoover point |
} |
} |
} |
// strech control output by inverse attitude projection 1/cos |
1647,12 → 1650,7 |
else SummeNick += DiffNick; // I-Anteil bei HH |
if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
|
//if(Parameter_UserParam5 > 50) |
pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 + SummeNick / Ki; // PI-Regler für Nick |
//else |
// pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick |
|
// Motor Vorn |
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
1666,12 → 1664,7 |
else SummeRoll += DiffRoll; // I-Anteil bei HH |
if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
|
//if(Parameter_UserParam5 > 50) |
pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8 + SummeRoll / Ki; // PI-Regler für Roll |
//else |
// pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
|
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
1704,15 → 1697,7 |
if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
} |
|
/* |
if(Parameter_UserParam5 > 50) |
{ |
if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
// else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
else tmp_int -= Parameter_UserParam6 * (tmp_motorwert[i] - tmp_int); // MotorSmoothing |
} |
*/ |
|
LIMIT_MIN_MAX(tmp_int,MIN_GAS * 4,MAX_GAS * 4); |
Motor[i].SetPoint = tmp_int / 4; |
Motor[i].SetPointLowerBits = (tmp_int % 4)<<1; // (3 bits total) |