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