1240,7 → 1240,7 |
// if height control is activated |
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
{ |
#define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
#define HOVER_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 = 0; |
1412,10 → 1412,18 |
if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint; |
else StickGasHover = 120; |
} |
HCGas = HoverGas; // take hoover gas (neutral point) |
HCGas = HoverGas; // take hover gas (neutral point) |
} |
if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)) |
{ |
// from this point the Heigth Control Algorithm is identical for both versions |
if(BaroExpandActive) // baro range expanding active |
{ |
HCGas = HoverGas; // hover while expanding baro adc range |
HeightDeviation = 0; |
} // EOF // baro range expanding active |
else // valid data from air pressure sensor |
{ |
// ------------------------- P-Part ---------------------------- |
tmp_long = (HoehenWert - SollHoehe); // positive when too high |
LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t |
1425,20 → 1433,19 |
HCGas -= tmp_long; |
// ------------------------- D-Part 1: Vario Meter ---------------------------- |
tmp_int = VarioMeter / 8; |
LIMIT_MIN_MAX(tmp_int, -180, 180); // avoid overflow when squared |
LIMIT_MIN_MAX(tmp_int, -181, 181); // avoid overflow when squared (181^2 = 32761) |
tmp_int2 = tmp_int; |
LIMIT_MAX(tmp_int2,8); // limit quadratic part on upward movement to avoid to much gas reduction |
if(tmp_int2 > 0) tmp_int = tmp_int + (tmp_int2 * tmp_int2) / 4L; |
else tmp_int = tmp_int - (tmp_int2 * tmp_int2) / 4L; |
if(tmp_int2 > 0) tmp_int = tmp_int + (tmp_int2 * tmp_int2) / 4; |
else tmp_int = tmp_int - (tmp_int2 * tmp_int2) / 4; |
tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 128L; // scale to d-gain parameter |
LIMIT_MIN_MAX(tmp_int,-32 * STICK_GAIN, 64 * STICK_GAIN); |
HCGas -= tmp_int; |
} // EOF no baro range expanding |
// ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
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 * (int)FromNaviCtrl_Value.GpsZ)/128L; |
LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1485,7 → 1492,7 |
// 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(HoverGasFilter == 0) HoverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
if(HoverGasFilter == 0) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
if(abs(VarioMeter) < 100) // only on small vertical speed |
{ |
tmp_long2 = (int32_t)GasMischanteil; // take current thrust |
1495,25 → 1502,25 |
// 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 |
HoverGasFilter -= HoverGasFilter/(HOOVER_GAS_AVERAGE/8L); |
HoverGasFilter -= HoverGasFilter/(HOVER_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 |
HoverGasFilter -= HoverGasFilter/(HOOVER_GAS_AVERAGE/4L); |
HoverGasFilter -= HoverGasFilter/(HOVER_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 |
HoverGasFilter -= HoverGasFilter/(HOOVER_GAS_AVERAGE/2L); |
HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/2L); |
HoverGasFilter += 2L * tmp_long2; |
} |
else //later |
{ |
HoverGasFilter -= HoverGasFilter/HOOVER_GAS_AVERAGE; |
HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE; |
HoverGasFilter += tmp_long2; |
} |
HoverGas = (int16_t)(HoverGasFilter/HOOVER_GAS_AVERAGE); |
HoverGas = (int16_t)(HoverGasFilter/HOVER_GAS_AVERAGE); |
if(EE_Parameter.Hoehe_HoverBand) |
{ |
int16_t band; |