| 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,33 → 1412,40 |
|---|
| 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)) |
| { |
| // ------------------------- P-Part ---------------------------- |
| tmp_long = (HoehenWert - SollHoehe); // positive when too high |
| LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t |
| HeightDeviation = (int)(tmp_long); // positive when too high |
| 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; |
| LIMIT_MIN_MAX(tmp_int, -180, 180); // avoid overflow when squared |
| 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; |
| 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; |
| // 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 |
| HeightDeviation = (int)(tmp_long); // positive when too high |
| 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; |
| 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) / 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; |