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