| 1509,7 → 1509,7 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if((ParamSet.Config0 & CFG0_AIRPRESS_SENSOR) && !(LoopingRoll || LoopingNick) ) |
| { |
| #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.1s averaging |
| #define HOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.1s averaging |
| #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging |
| |
| int16_t CosAttitude; // for projection of hoover gas |
| 1517,8 → 1517,8 |
|---|
| static int16_t FilterHCGas = 0; |
| static int16_t HeightTrimming = 0; // rate for change of height setpoint |
| static uint8_t HCActive = 0; |
| static int16_t StickGasHoover = RC_GAS_OFFSET, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
| static uint32_t HooverGasFilter = 0; |
| static int16_t StickGasHover = RC_GAS_OFFSET, HoverGas = 0, HoverGasMin = 0, HoverGasMax = 1023; |
| static uint32_t HoverGasFilter = 0; |
| static uint8_t delay = 100; |
| |
| #define BARO_LIMIT_MAX 0x01 |
| 1527,8 → 1527,8 |
|---|
| static uint8_t BaroFlags = 0; |
| static uint16_t BaroExpandActive = 0; |
| |
| // get the current hooverpoint |
| DebugOut.Analog[21] = HooverGas; |
| // get the current hoverpoint |
| DebugOut.Analog[21] = HoverGas; |
| DebugOut.Analog[18] = ReadingVario; |
| |
| // --------- barometer range expansion ------------------ |
| 1628,7 → 1628,7 |
|---|
| } |
| else // alternative height control |
| { |
| // PD-Control with respect to hoover point |
| // PD-Control with respect to hover point |
| // the setpoint will be fine adjusted with the gas stick position |
| #define HC_TRIM_UP 0x01 |
| #define HC_TRIM_DOWN 0x02 |
| 1637,8 → 1637,8 |
|---|
| #define HC_STICKTHRESHOLD 15 |
| |
| if(MKFlags & MKFLAG_FLY) // trim setpoint only when flying |
| { // gas stick is above hoover point |
| if(StickGas > (StickGasHoover + HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MAX)) |
| { // gas stick is above hover point |
| if(StickGas > (StickGasHover + HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MAX)) |
| { |
| if(HeightTrimmingFlag & HC_TRIM_DOWN) |
| { |
| 1646,9 → 1646,9 |
|---|
| SetPointHeight = ReadingHeight; // update setpoint to current height |
| } |
| HeightTrimmingFlag |= HC_TRIM_UP; |
| HeightTrimming += abs(StickGas - (StickGasHoover + HC_STICKTHRESHOLD)); |
| } // gas stick is below hoover point |
| else if(StickGas < (StickGasHoover - HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MIN)) |
| HeightTrimming += abs(StickGas - (StickGasHover + HC_STICKTHRESHOLD)); |
| } // gas stick is below hover point |
| else if(StickGas < (StickGasHover - HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MIN)) |
| { |
| if(HeightTrimmingFlag & HC_TRIM_UP) |
| { |
| 1656,9 → 1656,9 |
|---|
| SetPointHeight = ReadingHeight; // update setpoint to current heigth |
| } |
| HeightTrimmingFlag |= HC_TRIM_DOWN; |
| HeightTrimming -= abs(StickGas - (StickGasHoover - HC_STICKTHRESHOLD)); |
| HeightTrimming -= abs(StickGas - (StickGasHover - HC_STICKTHRESHOLD)); |
| } |
| else // gas stick in hoover range |
| else // gas stick in hover range |
| { |
| if(HeightTrimmingFlag & (HC_TRIM_UP | HC_TRIM_DOWN)) |
| { |
| 1674,12 → 1674,12 |
|---|
| SetPointHeight += (HeightTrimming * ParamSet.Height_Gain)/((5 * 512) / 2); // move setpoint |
| HeightTrimming = 0; |
| if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 75; |
| //update hoover gas stick value when setpoint is shifted |
| //update hover gas stick value when setpoint is shifted |
| if(!ParamSet.Height_StickNeutralPoint) |
| { |
| StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
| StickGasHoover = (StickGasHoover * UBat) / LowVoltageWarning; |
| LIMIT_MIN_MAX(StickGasHoover, 70, 150); // reserve some range for trim up and down |
| StickGasHover = HoverGas/STICK_GAIN; // rescale back to stick value |
| StickGasHover = (StickGasHover * UBat) / LowVoltageWarning; |
| LIMIT_MIN_MAX(StickGasHover, 70, 150); // reserve some range for trim up and down |
| } |
| } // EOF trimming height set point |
| if(BaroExpandActive) SetPointHeight = ReadingHeight; // update setpoint to current altitude if expanding is active |
| 1687,11 → 1687,11 |
|---|
| else // not flying but height control is already active |
| { |
| SetPointHeight = ReadingHeight - 400; // setpoint should be 4 meters below actual height to avoid a take off |
| if(ParamSet.Height_StickNeutralPoint) StickGasHoover = ParamSet.Height_StickNeutralPoint; |
| else StickGasHoover = RC_GAS_OFFSET; |
| if(ParamSet.Height_StickNeutralPoint) StickGasHover = ParamSet.Height_StickNeutralPoint; |
| else StickGasHover = RC_GAS_OFFSET; |
| } |
| |
| HCGas = HooverGas; // take hoover gas (neutral point for PD controller) |
| HCGas = HoverGas; // take hover gas (neutral point for PD controller) |
| |
| } //EOF alternative height control |
| |
| 1700,7 → 1700,7 |
|---|
| // from this point the Heigth Control Algorithm is identical for both versions |
| if(BaroExpandActive) // baro range expanding active |
| { |
| HCGas = HooverGas; // hoover while expanding baro adc range |
| HCGas = HoverGas; // hooer while expanding baro adc range |
| } // EOF // baro range expanding active |
| else // no baro range expanding |
| { |
| 1721,10 → 1721,10 |
|---|
| LIMIT_MIN_MAX(tmp_int1, -127, 255); |
| HCGas -= tmp_int1; |
| |
| // limit deviation from hoover point within the target region |
| if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
| // limit deviation from hover point within the target region |
| if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HoverGas > 0)) // height setpoint is not changed and hover gas not zero |
| { |
| LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
| LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hover point |
| } |
| } // EOF no baro range expanding |
| |
| 1754,26 → 1754,26 |
|---|
| }// EOF height control active |
| else // HC not active |
| { |
| // update hoover gas stick value when HC is not active |
| // update hover gas stick value when HC is not active |
| if(ParamSet.Height_StickNeutralPoint) |
| { |
| StickGasHoover = ParamSet.Height_StickNeutralPoint; |
| StickGasHover = ParamSet.Height_StickNeutralPoint; |
| } |
| else // take real hoover stick position |
| else // take real hover stick position |
| { |
| StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
| StickGasHoover = (StickGasHoover * UBat) / LowVoltageWarning; |
| StickGasHover = HoverGas/STICK_GAIN; // rescale back to stick value |
| StickGasHover = (StickGasHover * UBat) / LowVoltageWarning; |
| } |
| LIMIT_MIN_MAX(StickGasHoover, 70, 150); // reserve some range for trim up and down |
| LIMIT_MIN_MAX(StickGasHover, 70, 150); // reserve some range for trim up and down |
| FilterHCGas = GasMixFraction; // init filter for HCGas witch current gas mix fraction |
| } // EOF HC not active |
| |
| // ----------------- Hoover Gas Estimation -------------------------------- |
| // Hoover gas estimation by averaging gas control output on small z-velocities |
| // ----------------- Hover Gas Estimation -------------------------------- |
| // Hover 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)) |
| { |
| if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (uint32_t)(GasMixFraction); // init estimation |
| if(HoverGasFilter == 0) HoverGasFilter = HOVER_GAS_AVERAGE * (uint32_t)(GasMixFraction); // init estimation |
| if(abs(ReadingVario) < 100) // only on small vertical speed |
| { |
| tmp_long2 = (int32_t)GasMixFraction; // take current thrust |
| 1782,39 → 1782,39 |
|---|
| // average vertical projected thrust |
| if(ModelIsFlying < 2000) // the first 4 seconds |
| { // reduce the time constant of averaging by factor of 8 to get much faster a stable value |
| HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/8L); |
| HooverGasFilter += 8L * tmp_long2; |
| HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/8L); |
| HoverGasFilter += 8L * tmp_long2; |
| } |
| else if(ModelIsFlying < 4000) // the first 8 seconds |
| { // reduce the time constant of averaging by factor of 4 to get much faster a stable value |
| HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/4L); |
| HooverGasFilter += 4L * tmp_long2; |
| HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L); |
| HoverGasFilter += 4L * tmp_long2; |
| } |
| else if(ModelIsFlying < 8000) // the first 16 seconds |
| { // reduce the time constant of averaging by factor of 2 to get much faster a stable value |
| HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/2L); |
| HooverGasFilter += 2L * tmp_long2; |
| HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/2L); |
| HoverGasFilter += 2L * tmp_long2; |
| } |
| else //later |
| { |
| HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE; |
| HooverGasFilter += tmp_long2; |
| HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE; |
| HoverGasFilter += tmp_long2; |
| } |
| HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE); |
| if(ParamSet.Height_HooverBand) |
| HoverGas = (int16_t)(HoverGasFilter/HOVER_GAS_AVERAGE); |
| if(ParamSet.Height_HoverBand) |
| { |
| int16_t band; |
| band = HooverGas / ParamSet.Height_HooverBand; // the higher the parameter the smaller the range |
| HooverGasMin = HooverGas - band; |
| HooverGasMax = HooverGas + band; |
| band = HoverGas / ParamSet.Height_HoverBand; // the higher the parameter the smaller the range |
| HoverGasMin = HoverGas - band; |
| HoverGasMax = HoverGas + band; |
| } |
| else |
| { // no limit |
| HooverGasMin = 0; |
| HooverGasMax = 1023; |
| HoverGasMin = 0; |
| HoverGasMax = 1023; |
| } |
| } //EOF only on small vertical speed |
| }// EOF ----------------- Hoover Gas Estimation -------------------------------- |
| }// EOF ----------------- Hover Gas Estimation -------------------------------- |
| |
| }// EOF ParamSet.Config0 & CFG0_AIRPRESS_SENSOR |
| |