| 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; |