1513,7 → 1513,7 |
if(ParamSet.GlobalConfig & CFG_AIRPRESS_SENSOR) |
{ |
#define HOOVER_GAS_AVERAGE 4192 // 4192 * 2ms = 8.2s averaging |
#define HEIGHT_CONTROL_GAS_AVERAGE 16 // 16 * 2ms= 32 msa averaging |
#define HEIGHT_CONTROL_GAS_AVERAGE 8 // 8 * 2ms= 16 ms averaging |
|
int16_t CosNick, CosRoll; |
int16_t HeightControlGas; |
1583,14 → 1583,45 |
{ |
int16_t HooverGas; |
static int16_t HeightTrimming = 0; |
static uint8_t HeightTrimmingActive = 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 25 * STICK_GAIN |
if( (MKFlags & MKFLAG_FLY) && ( (GasMixFraction > (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD) ) || (GasMixFraction < (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD) ) ) ) |
{ |
HeightTrimmingActive = 1; |
HeightTrimming += (GasMixFraction - HooverGas)/STICK_GAIN; |
|
if(MKFlags & MKFLAG_FLY) // trim setpoint only when flying |
{ // gas stick is above hoover point |
if(GasMixFraction > (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD) ) |
{ |
if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN) |
{ |
HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN; |
SetPointHeight = ReadingHeight; // update setpoint to current heigth |
} |
HeightTrimmingFlag |= HEIGHT_TRIM_UP; |
HeightTrimming += abs(GasMixFraction - (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD))/STICK_GAIN; |
} // gas stick is below hoover point |
else if(GasMixFraction < (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD) ) |
{ |
if(HeightTrimmingFlag & HEIGHT_TRIM_UP) |
{ |
HeightTrimmingFlag &= ~HEIGHT_TRIM_UP; |
SetPointHeight = ReadingHeight; // update setpoint to current heigth |
} |
HeightTrimmingFlag |= HEIGHT_TRIM_DOWN; |
HeightTrimming -= abs(GasMixFraction - (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD))/STICK_GAIN; |
} |
else |
{ |
if(HeightTrimmingFlag & (HEIGHT_TRIM_UP|HEIGHT_TRIM_DOWN)) |
{ |
HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP|HEIGHT_TRIM_DOWN); |
HeightTrimming = 0; |
SetPointHeight = ReadingHeight; // update setpointto current heigth |
} |
} |
// Trim height set point |
if(abs(HeightTrimming) > 512) |
{ |
SetPointHeight += (HeightTrimming * ParamSet.Height_Gain)/512; // move setpoint |
1597,16 → 1628,7 |
HeightTrimming = 0; |
BeepTime = 50; |
} |
} |
else |
{ |
if(HeightTrimmingActive) |
{ |
HeightTrimming = 0; |
SetPointHeight = ReadingHeight; // update setpointto current heigth |
HeightTrimmingActive = 0; |
} |
} |
} //if MKFlags & MKFLAG_FLY |
|
if(HooverGas == 0) HeightControlGas = GasMixFraction; // take stick gas |
else HeightControlGas = HooverGas; // take hoover gas |