| 1694,10 → 1694,6 |
|---|
| |
| if(HCActive && !(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
| { |
| #define HC_TRIM_UP 0x01 |
| #define HC_TRIM_DOWN 0x02 |
| static uint8_t HeightTrimmingFlag = 0x00; |
| |
| if((ParamSet.Config2 & CFG2_HEIGHT_LIMIT) || !(ParamSet.Config0 & CFG0_HEIGHT_SWITCH)) |
| { |
| // Holgers original version |
| 1708,7 → 1704,7 |
|---|
| |
| HCGas = GasMixFraction; // take current stick gas as neutral point for the height control |
| HeightTrimming = 0; |
| HeightTrimmingFlag = 0x00; |
| FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
| } |
| else // alternative height control |
| { |
| 1720,31 → 1716,31 |
|---|
| { // gas stick is above hover point |
| if(StickGas > (StickGasHover + HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MAX)) |
| { |
| if(HeightTrimmingFlag & HC_TRIM_DOWN) |
| if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) |
| { |
| HeightTrimmingFlag &= ~HC_TRIM_DOWN; |
| FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN; |
| SetPointHeight = ReadingHeight; // update setpoint to current height |
| } |
| HeightTrimmingFlag |= HC_TRIM_UP; |
| FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
| HeightTrimming += abs(StickGas - (StickGasHover + HC_STICKTHRESHOLD)); |
| VarioCharacter = '+'; |
| } // gas stick is below hover point |
| else if(StickGas < (StickGasHover - HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MIN)) |
| { |
| if(HeightTrimmingFlag & HC_TRIM_UP) |
| if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) |
| { |
| HeightTrimmingFlag &= ~HC_TRIM_UP; |
| FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
| SetPointHeight = ReadingHeight; // update setpoint to current heigth |
| } |
| HeightTrimmingFlag |= HC_TRIM_DOWN; |
| FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
| HeightTrimming -= abs(StickGas - (StickGasHover - HC_STICKTHRESHOLD)); |
| VarioCharacter = '-'; |
| } |
| else // gas stick in hover range |
| { |
| if(HeightTrimmingFlag & (HC_TRIM_UP | HC_TRIM_DOWN)) |
| if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) |
| { |
| HeightTrimmingFlag &= ~(HC_TRIM_UP | HC_TRIM_DOWN); |
| FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
| HeightTrimming = 0; |
| SetPointHeight = ReadingHeight; // update setpoint to current height |
| if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 500; |
| 1808,7 → 1804,7 |
|---|
| tmp_int1 = (tmp_int1 * (int32_t)FCParam.HeightD ) / 4L; // scale to d-gain parameter |
| LIMIT_MIN_MAX(tmp_int1,-64 * STICK_GAIN, 64 * STICK_GAIN); |
| // reduce d-part when setvalue is changing |
| if(HeightTrimmingFlag || (abs(HeightDeviation) > 200) ) tmp_int1 /= 4; |
| if((FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) || (abs(HeightDeviation) > 200) ) tmp_int1 /= 4; |
| else if(ParamSet.Config2 & CFG2_HEIGHT_LIMIT) tmp_int1 /= 8; // reduce d-part in "Deckel" mode |
| GasReduction += tmp_int1; |
| } // EOF no baro range expanding |