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