| 651,7 → 651,6 |
|---|
| static int32_t CorrectionNick, CorrectionRoll; |
| static uint16_t RcLostTimer; |
| static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0; |
| static uint8_t HeightControlActive = 0; |
| static int8_t TimerDebugOut = 0; |
| static uint16_t UpdateCompassCourse = 0; |
| // high resolution motor values for smoothing of PID motor outputs |
| 1506,21 → 1505,33 |
|---|
| // Height Control |
| // The height control algorithm reduces the gas but does not increase the gas. |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| GasMixFraction = StickGas; // take the direct stick command |
| // at full LiPo the voltage is higher that gives more trust at the same BL-Control settpoint |
| // therefore attenuate the gas proportional to the lipo voltage reserve over the low bat warning level |
| // this yields to a nearly constant effective thrust over lipo discharging at the same stick position |
| if(UBat > LowVoltageWarning) |
| { |
| GasMixFraction = (GasMixFraction * LowVoltageWarning) / UBat; |
| } |
| GasMixFraction *= STICK_GAIN; // scale GasMixFraction to enlarge resolution in the motor mixer |
| |
| GasMixFraction = StickGas * STICK_GAIN; // derive GasMixFractin direct from the stick |
| |
| // if height control is activated |
| if(ParamSet.GlobalConfig & CFG_AIRPRESS_SENSOR) |
| { |
| #define HOOVER_GAS_AVERAGE 4192 // 4192 * 2ms = 8.2s averaging |
| #define HEIGHT_CONTROL_GAS_AVERAGE 8 // 8 * 2ms= 16 ms averaging |
| #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging |
| |
| int16_t CosNick, CosRoll; |
| int16_t HeightControlGas; |
| static int16_t FilterHeightControlGas = 0; |
| static uint32_t HooverGasEstimation = 0; |
| int16_t HCGas,HooverGas, HeightDeviation; |
| static uint8_t HCActive = 0; |
| static int16_t FilterHCGas = 0; |
| static uint32_t HooverGasFilter = 0; |
| static uint8_t delay = 100; |
| |
| |
| // get the current hooverpoint |
| HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE); |
| DebugOut.Analog[25] = HooverGas; |
| DebugOut.Analog[18] = ReadingVario; |
| |
| // if height control is activated by an rc channel |
| 1549,7 → 1560,7 |
|---|
| else |
| { |
| SetPointHeight = ReadingHeight; // update SetPoint with current reading |
| HeightControlActive = 0; // disable height control |
| HCActive = 0; // disable height control |
| delay = 1; |
| } |
| } |
| 1556,7 → 1567,7 |
|---|
| } |
| else |
| { //height control is activated |
| HeightControlActive = 1; // enable height control |
| HCActive = 1; // enable height control |
| delay = 200; |
| } |
| } |
| 1563,7 → 1574,7 |
|---|
| else // no switchable height control |
| { |
| SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain; |
| HeightControlActive = 1; |
| HCActive = 1; |
| } |
| |
| // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
| 1579,16 → 1590,13 |
|---|
| // PD-Control with respect to hoover point |
| // the thrust loss out of horizontal attitude is compensated |
| // the setpoint will be fine adjusted with the gas stick position |
| if(HeightControlActive && !(MKFlags & MKFLAG_EMERGENCY_LANDING)) |
| if(HCActive && !(MKFlags & MKFLAG_EMERGENCY_LANDING)) |
| { |
| int16_t HooverGas; |
| static int16_t HeightTrimming = 0; |
| #define HEIGHT_TRIM_UP 0x01 |
| #define HEIGHT_TRIM_DOWN 0x02 |
| static uint8_t HeightTrimmingFlag = 0x00; |
| |
| HooverGas = (int16_t)(HooverGasEstimation/HOOVER_GAS_AVERAGE); |
| |
| #define HEIGHT_CONTROL_STICKTHRESHOLD 15 * STICK_GAIN |
| |
| if(MKFlags & MKFLAG_FLY) // trim setpoint only when flying |
| 1631,45 → 1639,60 |
|---|
| } |
| } //if MKFlags & MKFLAG_FLY |
| |
| if(HooverGas == 0) HeightControlGas = GasMixFraction; // take stick gas |
| else HeightControlGas = HooverGas; // take hoover gas |
| if(HooverGas == 0) HCGas = GasMixFraction; // take stick gas |
| else HCGas = HooverGas; // take hoover gas |
| |
| #else // Holger original version |
| |
| // start of height control algorithm |
| // the height control is only an attenuation of the actual gas stick. |
| // I.e. it wikll work only if tha gas stiock is heiger than the hover gas |
| // I.e. it will work only if the gas stick is higher than the hover gas |
| // and the hover height will be allways larger than height setpoint. |
| if((ReadingHeight > SetPointHeight) && HeightControlActive && !(MKFlags & MKFLAG_EMERGENCY_LANDING)) |
| if((ReadingHeight > SetPointHeight) && HCActive && !(MKFlags & MKFLAG_EMERGENCY_LANDING)) |
| { |
| HeightControlGas = GasMixFraction; // take stick gas, |
| #endif // from this point the Heigth Control is identical |
| HeightControlGas -= ((int16_t)(ReadingHeight - SetPointHeight) * (int16_t)FCParam.HeightP) / 16; // p-part |
| // quadratic scaling to ReadingVario in the D-Part |
| int16_t vario; |
| vario = ReadingVario / 8; |
| if(vario > 8) vario = 8; // limit quadratic part on upward movement to avoid to much gas reduction |
| if(vario > 0) vario = ReadingVario + (vario * vario) / 4; |
| else vario = ReadingVario - (vario * vario) / 4; |
| HeightControlGas -= (FCParam.HeightD * (int32_t)(vario)) / 128L; // d-part |
| // acceleration sensor effect |
| HCGas = GasMixFraction; // take current stick gas as neutral point for the height control |
| |
| #endif // from this point the Heigth Control Algorithm is identical for both versions |
| |
| |
| // ------------------------- P-Part ---------------------------- |
| HeightDeviation = (int16_t)(ReadingHeight - SetPointHeight); // positive when to high |
| tmp_int = (HeightDeviation * (int16_t)FCParam.HeightP) / 16; // p-part |
| HCGas -= tmp_int; |
| // ------------------------- D-Part ---------------------------- |
| tmp_int = ReadingVario / 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 = ReadingVario + (tmp_int * tmp_int) / 4; |
| else tmp_int = ReadingVario - (tmp_int * tmp_int) / 4; |
| tmp_int = (FCParam.HeightD * (int32_t)(tmp_int)) / 128L; // scale to d-gain parameter |
| LIMIT_MIN_MAX(tmp_int, -63, 63); |
| HCGas -= tmp_int; |
| // ------------------------ ACC-Part ---------------------------- |
| tmp_int = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN); |
| LIMIT_MIN_MAX(tmp_int, -(70 * STICK_GAIN), 70 * STICK_GAIN); // limit acceleration sensor effect |
| HeightControlGas -= tmp_int; // acc-part |
| LIMIT_MIN_MAX(tmp_int, -63, 127); |
| HCGas -= tmp_int; |
| |
| // limit deviation from hoover point within the target region |
| if(abs(HeightDeviation) < 200 && (HooverGas > 0)) // deviation is less than 2 meter |
| { |
| |
| // strech by actual attitude projection |
| tmp_long3 = (int32_t)HeightControlGas; |
| LIMIT_MIN_MAX(HCGas, HooverGas - HooverGas / 6, HooverGas + HooverGas / 6); // +/- 16% around the hoover point |
| } |
| |
| // strech control output by inverse attitude projection |
| tmp_long3 = (int32_t)HCGas; |
| tmp_long3 *= 8192; |
| tmp_long3 /= CosNick; |
| tmp_long3 *= 8192; |
| tmp_long3 /= CosRoll; |
| HeightControlGas = (int16_t)tmp_long3; |
| HCGas = (int16_t)tmp_long3; |
| |
| // update height control gas averaging |
| FilterHeightControlGas = (FilterHeightControlGas * (HEIGHT_CONTROL_GAS_AVERAGE - 1) + HeightControlGas) / HEIGHT_CONTROL_GAS_AVERAGE; |
| // limit gas |
| LIMIT_MIN_MAX(FilterHeightControlGas, ParamSet.HeightMinGas * STICK_GAIN, (ParamSet.GasMax - 20) * STICK_GAIN); |
| 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); |
| // set GasMixFraction to HeightControlGasFilter |
| GasMixFraction = FilterHeightControlGas; |
| GasMixFraction = FilterHCGas; |
| }// EOF height control active |
| |
| // Hoover gas estimation by averaging gas control output on small z-velocities |
| 1676,8 → 1699,8 |
|---|
| // 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(HooverGasEstimation == 0) HooverGasEstimation = HOOVER_GAS_AVERAGE * (uint32_t)GasMixFraction; // init estimation |
| if(abs(ReadingVario) < 150) |
| if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (uint32_t)GasMixFraction; // init estimation |
| if(abs(ReadingVario) < 150) // only on small vertical speed |
| { |
| tmp_long3 = (int32_t)GasMixFraction; // take current thrust |
| tmp_long3 *= CosNick; // apply nick projection |
| 1685,11 → 1708,10 |
|---|
| tmp_long3 *= CosRoll; // apply roll projection |
| tmp_long3 /= 8129; |
| // average vertical projected thrust |
| HooverGasEstimation -= HooverGasEstimation/HOOVER_GAS_AVERAGE; |
| HooverGasEstimation += tmp_long3; |
| HooverGasFilter -= (int32_t)HooverGas; |
| HooverGasFilter += tmp_long3; |
| } |
| } |
| DebugOut.Analog[25] = (int16_t)(HooverGasEstimation/HOOVER_GAS_AVERAGE); |
| }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
| |
| // limit gas to parameter setting |