1527,7 → 1527,7 |
static int16_t HeightTrimming = 0; // rate for change of height setpoint |
static uint8_t HCActive = 0; |
static int16_t FilterHCGas = 0; |
static int16_t HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
static int16_t StickGasHoover = 110, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
static uint32_t HooverGasFilter = 0; |
static uint8_t delay = 100; |
|
1603,45 → 1603,45 |
{ |
// PD-Control with respect to hoover point |
// the setpoint will be fine adjusted with the gas stick position |
#define HEIGHT_TRIM_UP 0x01 |
#define HEIGHT_TRIM_DOWN 0x02 |
#define HC_TRIM_UP 0x01 |
#define HC_TRIM_DOWN 0x02 |
static uint8_t HeightTrimmingFlag = 0x00; |
|
#define HEIGHT_CONTROL_STICKTHRESHOLD 15 * STICK_GAIN |
#define HC_STICKTHRESHOLD 15 |
|
if(MKFlags & MKFLAG_FLY) // trim setpoint only when flying |
{ // gas stick is above hoover point |
if(GasMixFraction > (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD) ) |
if(StickGas > (StickGasHoover + HC_STICKTHRESHOLD) ) |
{ |
if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN) |
if(HeightTrimmingFlag & HC_TRIM_DOWN) |
{ |
HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN; |
HeightTrimmingFlag &= ~HC_TRIM_DOWN; |
SetPointHeight = ReadingHeight; // update setpoint to current height |
} |
HeightTrimmingFlag |= HEIGHT_TRIM_UP; |
HeightTrimming += abs(GasMixFraction - (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD))/STICK_GAIN; |
HeightTrimmingFlag |= HC_TRIM_UP; |
HeightTrimming += abs(StickGas - (StickGasHoover + HC_STICKTHRESHOLD)); |
} // gas stick is below hoover point |
else if(GasMixFraction < (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD) ) |
else if(StickGas < (StickGasHoover - HC_STICKTHRESHOLD) ) |
{ |
if(HeightTrimmingFlag & HEIGHT_TRIM_UP) |
if(HeightTrimmingFlag & HC_TRIM_UP) |
{ |
HeightTrimmingFlag &= ~HEIGHT_TRIM_UP; |
HeightTrimmingFlag &= ~HC_TRIM_UP; |
SetPointHeight = ReadingHeight; // update setpoint to current heigth |
} |
HeightTrimmingFlag |= HEIGHT_TRIM_DOWN; |
HeightTrimming -= abs(GasMixFraction - (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD))/STICK_GAIN; |
HeightTrimmingFlag |= HC_TRIM_DOWN; |
HeightTrimming -= abs(StickGas - (StickGasHoover - HC_STICKTHRESHOLD)); |
} |
else // Gas Stick in Hoover Range |
else // gas stick in hoover range |
{ |
if(HeightTrimmingFlag & (HEIGHT_TRIM_UP|HEIGHT_TRIM_DOWN)) |
if(HeightTrimmingFlag & (HC_TRIM_UP|HC_TRIM_DOWN)) |
{ |
HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP|HEIGHT_TRIM_DOWN); |
HeightTrimmingFlag &= ~(HC_TRIM_UP|HC_TRIM_DOWN); |
HeightTrimming = 0; |
SetPointHeight = ReadingHeight; // update setpoint to current height |
if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 500; |
} |
} |
// Trim height set point |
// trim height set point |
if(abs(HeightTrimming) > 512) |
{ |
SetPointHeight += (HeightTrimming * ParamSet.Height_Gain)/(( 512 * 5 ) / 2); // move setpoint |
1651,7 → 1651,8 |
} //if MKFlags & MKFLAG_FLY |
else // not flying but height control is already active |
{ |
SetPointHeight = ReadingHeight - 200; // setpoint should be 2 meters below actual height to avoid a tske off |
SetPointHeight = ReadingHeight - 200; // setpoint should be 2 meters below actual height to avoid a take off |
StickGasHoover = 110; |
} |
|
HCGas = HooverGas; // take hoover gas (neutral point) |
1680,7 → 1681,7 |
HCGas -= tmp_int; |
|
// limit deviation from hoover point within the target region |
if( (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
{ |
|
LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
1712,7 → 1713,14 |
GasMixFraction = FilterHCGas; |
} // EOF if((ReadingHeight > SetPointHeight) || !(ParamSet.Config2 & CFG2_HEIGHT_LIMIT)) |
}// EOF height control active |
else // HC not active |
{ |
//update hoover gas stick value only if HC is not active |
StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
StickGasHoover = (StickGasHoover * UBat) / LowVoltageWarning; |
} |
|
|
// 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)) |