1483,16 → 1483,9 |
// ------------------------- D-Part 1: Vario Meter ---------------------------- |
tmp_int = VarioMeter / 8; |
LIMIT_MIN_MAX(tmp_int, -127, 128); |
if(!HeightTrimmingFlag) |
{ |
tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
} |
else |
{ |
tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 16L; // weniger D zum Höhe ändern |
LIMIT_MIN_MAX(tmp_int,-16 * STICK_GAIN, 16 * STICK_GAIN); |
} |
tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
if(HeightTrimmingFlag) tmp_int /= 4; // reduce d-part while trimming setpoint |
GasReduction += tmp_int; |
} // EOF no baro range expanding |
// ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1513,27 → 1506,29 |
// limit deviation from hoover point within the target region |
if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
{ |
signed int tmp,min,max; |
if(abs(HeightDeviation) < 60) |
{ |
LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point |
} |
else |
{ |
tmp = (abs(HeightDeviation) - 60)/32; |
unsigned int tmp; |
tmp = abs(HeightDeviation); |
if(tmp <= 60) |
{ |
LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point |
} |
else |
{ |
tmp = (tmp - 60) / 32; |
if(tmp > 15) tmp = 15; |
if(HeightDeviation > 0) |
{ |
min = (HoverGasMin * (16 - tmp)) / 16; |
LIMIT_MIN_MAX(HCGas, min, HoverGasMax); // limit gas around the hoover point |
} |
else |
if(HeightDeviation > 0) |
{ |
max = (HoverGasMax * (tmp + 16)) / 16; |
LIMIT_MIN_MAX(HCGas, HoverGasMin, max); // limit gas around the hoover point |
tmp = (HoverGasMin * (16 - tmp)) / 16; |
LIMIT_MIN_MAX(HCGas, tmp, HoverGasMax); // limit gas around the hoover point |
} |
} |
else |
{ |
tmp = (HoverGasMax * (tmp + 16)) / 16; |
LIMIT_MIN_MAX(HCGas, HoverGasMin, tmp); // limit gas around the hoover point |
} |
} |
} |
|
// strech control output by inverse attitude projection 1/cos |
// + 1/cos(angle) ++++++++++++++++++++++++++ |
tmp_long2 = (int32_t)HCGas; |