1243,16 → 1243,16 |
#define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
#define HC_GAS_AVERAGE 4 // 4 * 2ms= 8ms averaging |
#define OPA_OFFSET_STEP 10 |
int HCGas, HeightDeviation; |
int HCGas, HeightDeviation = 0; |
static int HeightTrimming = 0; // rate for change of height setpoint |
static int FilterHCGas = 0; |
static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
static unsigned long HooverGasFilter = 0; |
static int StickGasHover = 120, HoverGas = 0, HoverGasMin = 0, HoverGasMax = 1023; |
static unsigned long HoverGasFilter = 0; |
static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
int CosAttitude; // for projection of hoover gas |
|
// get the current hooverpoint |
DebugOut.Analog[21] = HooverGas; |
DebugOut.Analog[21] = HoverGas; |
DebugOut.Analog[18] = VarioMeter; |
|
// Expand the measurement |
1358,7 → 1358,7 |
// the setpoint will be fine adjusted with the gas stick position |
if(FCFlags & FCFLAG_FLY) // trim setpoint only when flying |
{ // gas stick is above hoover point |
if(StickGas > (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
{ |
if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN) |
{ |
1366,9 → 1366,9 |
SollHoehe = HoehenWert; // update setpoint to current heigth |
} |
HeightTrimmingFlag |= HEIGHT_TRIM_UP; |
HeightTrimming += abs(StickGas - (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
HeightTrimming += abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
} // gas stick is below hoover point |
else if(StickGas < (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) |
else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) |
{ |
if(HeightTrimmingFlag & HEIGHT_TRIM_UP) |
{ |
1376,9 → 1376,9 |
SollHoehe = HoehenWert; // update setpoint to current heigth |
} |
HeightTrimmingFlag |= HEIGHT_TRIM_DOWN; |
HeightTrimming -= abs(StickGas - (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
} |
else // Gas Stick in Hoover Range |
else // Gas Stick in Hover Range |
{ |
if(HeightTrimmingFlag & (HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN)) |
{ |
1398,10 → 1398,10 |
//update hoover gas stick value when setpoint is shifted |
if(!EE_Parameter.Hoehe_StickNeutralPoint) |
{ |
StickGasHoover = HooverGas/STICK_GAIN; //rescale back to stick value |
StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
if(StickGasHoover < 70) StickGasHoover = 70; |
else if(StickGasHoover > 150) StickGasHoover = 150; |
StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value |
StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning; |
if(StickGasHover < 70) StickGasHover = 70; |
else if(StickGasHover > 150) StickGasHover = 150; |
} |
} |
if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active |
1409,42 → 1409,45 |
else |
{ |
SollHoehe = HoehenWert - 400; |
if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
else StickGasHoover = 120; |
if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint; |
else StickGasHover = 120; |
} |
HCGas = HooverGas; // take hoover gas (neutral point) |
HCGas = HoverGas; // take hoover gas (neutral point) |
} |
if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)) |
{ |
// ------------------------- P-Part ---------------------------- |
tmp_long = (HoehenWert - SollHoehe); // positive when too high |
LIMIT_MIN_MAX(tmp_long, -2000, 2000); // avoid overflov when casting to int |
LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t |
HeightDeviation = (int)(tmp_long); // positive when too high |
tmp_int = (HeightDeviation * (long)Parameter_Hoehe_P) / 16; // p-part |
HCGas -= tmp_int; |
tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 16L; // p-part |
LIMIT_MIN_MAX(tmp_long, -255 * STICK_GAIN, 255 * STICK_GAIN); // more than 2 times the full range makes no sense |
HCGas -= tmp_long; |
// ------------------------- D-Part 1: Vario Meter ---------------------------- |
tmp_int = VarioMeter / 8; |
if(tmp_int > 8) tmp_int = 8; // limit quadratic part on upward movement to avoid to much gas reduction |
if(tmp_int > 0) tmp_int = VarioMeter + (tmp_int * tmp_int) / 4; |
else tmp_int = VarioMeter - (tmp_int * tmp_int) / 4; |
tmp_int = (Parameter_Luftdruck_D * (long)(tmp_int)) / 128L; // scale to d-gain parameter |
LIMIT_MIN_MAX(tmp_int, -127, 255); |
HCGas -= tmp_int; |
tmp_long = VarioMeter / 8; |
if(tmp_long > 8) tmp_long = 8; // limit quadratic part on upward movement to avoid to much gas reduction |
if(tmp_long > 0) tmp_long = VarioMeter + (tmp_long * tmp_long) / 4L; |
else tmp_long = VarioMeter - (tmp_long * tmp_long) / 4L; |
tmp_long = (tmp_long * (long)Parameter_Luftdruck_D) / 128L; // scale to d-gain parameter |
LIMIT_MIN_MAX(tmp_long,-32 * STICK_GAIN, 64 * STICK_GAIN); |
HCGas -= tmp_long; |
// ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
LIMIT_MIN_MAX(tmp_int, -127, 255); |
tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN); |
LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); |
HCGas -= tmp_long; |
|
if(BaroExpandActive) HCGas = HoverGas; |
// ------------------------ D-Part 3: GpsZ ---------------------------------- |
tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L; |
LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); |
HCGas -= tmp_int; |
|
// 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 |
if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HoverGas > 0)) // height setpoint is not changed and hoover 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 hoover point |
} |
|
if(BaroExpandActive) HCGas = HooverGas; |
// ------------------------ D-Part 3: GpsZ ---------------------------------- |
tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L; |
LIMIT_MIN_MAX(tmp_int, -127, 255); |
HCGas -= tmp_int; |
// strech control output by inverse attitude projection 1/cos |
// + 1/cos(angle) ++++++++++++++++++++++++++ |
tmp_long2 = (int32_t)HCGas; |
1458,7 → 1461,7 |
// set GasMischanteil to HeightControlGasFilter |
if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) |
{ // old version |
if(FilterHCGas > GasMischanteil) FilterHCGas = GasMischanteil; // nicht mehr als Gas |
LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
} |
GasMischanteil = FilterHCGas; |
} |
1468,20 → 1471,19 |
//update hoover gas stick value when HC is not active |
if(!EE_Parameter.Hoehe_StickNeutralPoint) |
{ |
StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
StickGasHover = HoverGas/STICK_GAIN; // rescale back to stick value |
StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning; |
} |
else StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
if(StickGasHoover < 70) StickGasHoover = 70; |
else if(StickGasHoover > 150) StickGasHoover = 150; |
else StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint; |
LIMIT_MIN_MAX(StickGasHover, 70, 150); // reserve some range for trim up and down |
FilterHCGas = GasMischanteil; |
} |
|
// Hoover gas estimation by averaging gas control output on small z-velocities |
// 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((FCFlags & FCFLAG_FLY) && !(FCFlags & FCFLAG_NOTLANDUNG)) |
{ |
if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
if(HoverGasFilter == 0) HoverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
if(abs(VarioMeter) < 100) // only on small vertical speed |
{ |
tmp_long2 = (int32_t)GasMischanteil; // take current thrust |
1491,36 → 1493,36 |
// average vertical projected thrust |
if(modell_fliegt < 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/(HOOVER_GAS_AVERAGE/8L); |
HoverGasFilter += 8L * tmp_long2; |
} |
else if(modell_fliegt < 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/(HOOVER_GAS_AVERAGE/4L); |
HoverGasFilter += 4L * tmp_long2; |
} |
else if(modell_fliegt < 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/(HOOVER_GAS_AVERAGE/2L); |
HoverGasFilter += 2L * tmp_long2; |
} |
else //later |
{ |
HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE; |
HooverGasFilter += tmp_long2; |
HoverGasFilter -= HoverGasFilter/HOOVER_GAS_AVERAGE; |
HoverGasFilter += tmp_long2; |
} |
HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE); |
HoverGas = (int16_t)(HoverGasFilter/HOOVER_GAS_AVERAGE); |
if(EE_Parameter.Hoehe_HoverBand) |
{ |
int16_t band; |
band = HooverGas / EE_Parameter.Hoehe_HoverBand; // the higher the parameter the smaller the range |
HooverGasMin = HooverGas - band; |
HooverGasMax = HooverGas + band; |
band = HoverGas / EE_Parameter.Hoehe_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; |
} |
} |
} |