1588,7 → 1588,7 |
static uint8_t HeightTrimmingFlag = 0x00; |
|
HooverGas = (int16_t)(HooverGasEstimation/HOOVER_GAS_AVERAGE); |
#define HEIGHT_CONTROL_STICKTHRESHOLD 25 * STICK_GAIN |
#define HEIGHT_CONTROL_STICKTHRESHOLD 15 * STICK_GAIN |
|
if(MKFlags & MKFLAG_FLY) // trim setpoint only when flying |
{ // gas stick is above hoover point |
1600,7 → 1600,7 |
SetPointHeight = ReadingHeight; // update setpoint to current heigth |
} |
HeightTrimmingFlag |= HEIGHT_TRIM_UP; |
HeightTrimming += abs(GasMixFraction - (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD))/STICK_GAIN; |
HeightTrimming += abs(GasMixFraction - (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD))/STICK_GAIN; |
} // gas stick is below hoover point |
else if(GasMixFraction < (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD) ) |
{ |
1632,8 → 1632,23 |
|
if(HooverGas == 0) HeightControlGas = GasMixFraction; // take stick gas |
else HeightControlGas = HooverGas; // take hoover gas |
#else // Holger original version |
// start of height control algorithm |
// the height control is only an attenuation of the actual gas stick. |
// I.e. it wikll work only if tha gas stiock is heiger than the hover gas |
// and the hover height will be allways larger than height setpoint. |
if((ReadingHeight > SetPointHeight) && HeightControlActive && !(MKFlags & MKFLAG_EMERGENCY_LANDING)) |
{ |
HeightControlGas = GasMixFraction; // take stick gas |
#endif // from this point the Heigth Control is identical |
HeightControlGas -= ((int16_t)(ReadingHeight - SetPointHeight) * (int16_t)FCParam.HeightP) / 16; // p-part |
HeightControlGas -= (FCParam.HeightD * ReadingVario) / 128; // d-part |
// quadratic scaling to ReadingVario in the D-Part |
int16_t vario; |
vario = ReadingVario / 8; |
if(vario > 8) vario = 8; // limit quadratic part on upward movement to avoid to much gas reduction |
if(vario > 0) vario = ReadingVario + (vario * vario) / 4; |
else vario = ReadingVario - (vario * vario) / 4; |
HeightControlGas -= (FCParam.HeightD * (int32_t)(vario)) / 128L; // d-part |
// acceleration sensor effect |
tmp_int = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN); |
LIMIT_MIN_MAX(tmp_int, -(70 * STICK_GAIN), 70 * STICK_GAIN); // limit acceleration sensor effect |
1655,34 → 1670,7 |
// set GasMixFraction to HeightControlGasFilter |
GasMixFraction = FilterHeightControlGas; |
}// EOF height control active |
#else // Holger original version |
// start of height control algorithm |
// the height control is only an attenuation of the actual gas stick. |
// I.e. it wikll work only if tha gas stiock is heiger than the hover gas |
// and the hover height will be allways larger than height setpoint. |
if((ReadingHeight > SetPointHeight) && HeightControlActive && !(MKFlags & MKFLAG_EMERGENCY_LANDING)) |
{ |
HeightControlGas = GasMixFraction; // take stick gas |
HeightControlGas -= ((int16_t)(ReadingHeight - SetPointHeight) * (int16_t)FCParam.HeightP) / 16; // p-part |
HeightControlGas -= (FCParam.HeightD * ReadingVario) / 128; // d-part |
// acceleration sensor effect |
tmp_int = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN); |
LIMIT_MIN_MAX(tmp_int, -(70 * STICK_GAIN), 70 * STICK_GAIN); // limit acceleration sensor effect |
HeightControlGas -= tmp_int; // acc-part |
|
// update height control gas averaging |
FilterHeightControlGas = (FilterHeightControlGas * (HEIGHT_CONTROL_GAS_AVERAGE - 1) + HeightControlGas) / HEIGHT_CONTROL_GAS_AVERAGE; |
// never lower than height min gas setting |
LIMIT_MIN(FilterHeightControlGas, ParamSet.HeightMinGas * STICK_GAIN); |
// never higher than gas stick (maybe lower than height min gas) |
LIMIT_MAX(FilterHeightControlGas, GasMixFraction); |
// set GasMixFraction to HeightControlGasFilter |
GasMixFraction = FilterHeightControlGas; |
}// EOF height control active |
#endif |
|
|
|
// Hoover gas estimation by averaging gas control output on small z-velocities |
// this is done only if height contol option is selected in global config and aircraft is flying |
if((MKFlags & MKFLAG_FLY) && !(MKFlags & MKFLAG_EMERGENCY_LANDING)) |
1699,9 → 1687,9 |
HooverGasEstimation -= HooverGasEstimation/HOOVER_GAS_AVERAGE; |
HooverGasEstimation += tmp_long3; |
} |
DebugOut.Analog[16] = HooverGasEstimation/HOOVER_GAS_AVERAGE; |
} |
}// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
|
// limit gas to parameter setting |
LIMIT_MIN(GasMixFraction, (ParamSet.GasMin + 10) * STICK_GAIN); |
LIMIT_MAX(GasMixFraction, (ParamSet.GasMax - 20) * STICK_GAIN); |