| 1490,7 → 1490,7 |
|---|
| #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging |
| |
| int16_t CosAttitude; // for projection of hoover gas |
| int16_t HCGas, HeightDeviation; |
| int16_t HCGas, HeightDeviation = 0; |
| static int16_t FilterHCGas = 0; |
| static int16_t HeightTrimming = 0; // rate for change of height setpoint |
| static uint8_t HCActive = 0; |
| 1650,6 → 1650,7 |
|---|
| { |
| SetPointHeight += (HeightTrimming * ParamSet.Height_Gain)/((5 * 512) / 2); // move setpoint |
| HeightTrimming = 0; |
| LIMIT_MIN_MAX(SetPointHeight, (ReadingHeight - 1024), (ReadingHeight + 1024)); // max. 10m deviation |
| if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 75; |
| //update hover gas stick value when setpoint is shifted |
| if(!ParamSet.Height_StickNeutralPoint) |
| 1683,35 → 1684,38 |
|---|
| { |
| // ------------------------- P-Part ---------------------------- |
| tmp_long1 = ReadingHeight - SetPointHeight; // positive when too high |
| LIMIT_MIN_MAX(tmp_long1, -32000, 32000); // avoid overflov when casting to int16_t |
| HeightDeviation = (int16_t)(tmp_long1); // positive when too high |
| tmp_int1 = (HeightDeviation * (int16_t)FCParam.HeightP) / 16; // p-part |
| HCGas -= tmp_int1; |
| LIMIT_MIN_MAX(tmp_long1, -32767L, 32767L); // avoid overflow when casting to int16_t |
| HeightDeviation = (int16_t)(tmp_long1); // positive when too high |
| tmp_long1 = (tmp_long1 * (int32_t)FCParam.HeightP) / 16L; // p-part |
| LIMIT_MIN_MAX(tmp_long1, -255 * STICK_GAIN, 255 * STICK_GAIN); // more than 2 times the full range makes no sense |
| HCGas -= tmp_long1; |
| // ------------------------- D-Part 1: Vario Meter ---------------------------- |
| tmp_int1 = ReadingVario / 8; |
| if(tmp_int1 > 8) tmp_int1 = 8; // limit quadratic part on upward movement to avoid to much gas reduction |
| if(tmp_int1 > 0) tmp_int1 = ReadingVario + (tmp_int1 * tmp_int1) / 4; |
| else tmp_int1 = ReadingVario - (tmp_int1 * tmp_int1) / 4; |
| tmp_int1 = (FCParam.HeightD * (int32_t)(tmp_int1)) / 128L; // scale to d-gain parameter |
| LIMIT_MIN_MAX(tmp_int1, -127, 255); |
| LIMIT_MIN_MAX(tmp_int1, -180, 180); // avoid overflow when squared |
| tmp_int2 = tmp_int1; |
| LIMIT_MAX(tmp_int2,8); // limit quadratic part on upward movement to avoid to much gas reduction |
| if(tmp_int1 > 0) tmp_int1 = tmp_int1 + (tmp_int2 * tmp_int2) / 4L; |
| else tmp_int1 = tmp_int1 - (tmp_int2 * tmp_int2) / 4L; |
| tmp_int1 = (tmp_int1 * (int32_t)FCParam.HeightD ) / 128L; // scale to d-gain parameter |
| LIMIT_MIN_MAX(tmp_int1,-32 * STICK_GAIN, 64 * STICK_GAIN); |
| HCGas -= tmp_int1; |
| // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
| tmp_int1 = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN); |
| LIMIT_MIN_MAX(tmp_int1, -127, 255); |
| HCGas -= tmp_int1; |
| |
| // 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, HoverGasMin, HoverGasMax); // limit gas around the hover point |
| } |
| tmp_long1 = ((ReadingIntegralTop / 128L) * (int32_t) FCParam.Height_ACC_Effect) / (128L / STICK_GAIN); |
| LIMIT_MIN_MAX(tmp_long1, -32 * STICK_GAIN, 64 * STICK_GAIN); |
| HCGas -= tmp_long1; |
| } // EOF no baro range expanding |
| |
| // ------------------------ D-Part 3: GpsZ ---------------------------------- |
| tmp_int1 = (ParamSet.Height_GPS_Z * (int32_t)NCGpsZ)/128L; |
| LIMIT_MIN_MAX(tmp_int1, -127, 255); |
| tmp_int1 = ((int16_t)ParamSet.Height_GPS_Z * (int16_t)NCGpsZ)/128L; |
| LIMIT_MIN_MAX(tmp_int1, -32 * STICK_GAIN, 64 * STICK_GAIN); |
| HCGas -= tmp_int1; |
| |
| // 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, HoverGasMin, HoverGasMax); // limit gas around the hover point |
| } |
| |
| // strech control output by inverse attitude projection 1/cos |
| tmp_long2 = (int32_t)HCGas; |
| tmp_long2 *= 8192L; |
| 1722,7 → 1726,7 |
|---|
| FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
| // limit height control gas pd-control output |
| LIMIT_MIN_MAX(FilterHCGas, ParamSet.HeightMinGas * STICK_GAIN, (ParamSet.GasMax - 20) * STICK_GAIN); |
| // limit gas to stick position for limiting height version |
| // limit gas to stick position when limiting height control version active |
| if(ParamSet.Config2 & CFG2_HEIGHT_LIMIT) |
| { |
| LIMIT_MAX(FilterHCGas, GasMixFraction); |
| 1790,7 → 1794,7 |
|---|
| else |
| { // no limit |
| HoverGasMin = 0; |
| HoverGasMax = 1023; |
| HoverGasMax = 255 * STICK_GAIN; |
| } |
| } //EOF only on small vertical speed |
| }// EOF ----------------- Hover Gas Estimation -------------------------------- |